diff --git a/.travis.rosinstall b/.travis.rosinstall index a2f81f4..b10f9d3 100644 --- a/.travis.rosinstall +++ b/.travis.rosinstall @@ -1,7 +1,7 @@ - git: uri: 'https://github.com/iirob/iirob_filters.git' local-name: iirob_filters - version: kinetic-devel + version: raw_with_fts - git: uri: 'https://github.com/ipa320/cob_driver.git' local-name: cob_driver diff --git a/CMakeLists.txt b/CMakeLists.txt index 4a83c9e..f643664 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -21,6 +21,8 @@ find_package(catkin REQUIRED COMPONENTS tf2_ros trajectory_msgs visualization_msgs + realtime_tools + filters ) find_package(Eigen3) @@ -66,7 +68,7 @@ generate_messages( ################################### catkin_package( INCLUDE_DIRS common/include ros/include - CATKIN_DEPENDS cob_generic_can geometry_msgs iirob_filters message_runtime roscpp std_msgs std_srvs rospy tf2 tf2_ros tf2_geometry_msgs trajectory_msgs + CATKIN_DEPENDS cob_generic_can geometry_msgs iirob_filters message_runtime roscpp std_msgs std_srvs rospy tf2 tf2_ros tf2_geometry_msgs trajectory_msgs realtime_tools filters DEPENDS Eigen LIBRARIES ${PROJECT_NAME} ) diff --git a/cfg/PublishConfiguration.params b/cfg/PublishConfiguration.params index c204c50..0c85f7f 100755 --- a/cfg/PublishConfiguration.params +++ b/cfg/PublishConfiguration.params @@ -8,5 +8,6 @@ gen.add("moving_mean", paramtype="bool", description="publish moving mean filter gen.add("transformed_data", paramtype="bool", description="publish transformed data", default=True, configurable=True) gen.add("gravity_compensated", paramtype="bool", description="publish gravity compensated data", default=True, configurable=True) gen.add("threshold_filtered", paramtype="bool", description="publish threshold filtered data", default=True, configurable=True) +gen.add("iirob_led", paramtype="bool", description="publish data for iirob_led", default=True, configurable=True) exit(gen.generate("ati_force_torque", "ForceTorqueSensor", "PublishConfiguration")) diff --git a/config/sensor_configuration.yaml b/config/sensor_configuration.yaml index a793884..4e8e49f 100644 --- a/config/sensor_configuration.yaml +++ b/config/sensor_configuration.yaml @@ -10,59 +10,27 @@ Calibration: n_measurements: 500 T_between_meas: 0.01 isStatic: false # true= use static offset Calibration, false=use calibrate Sensor to 0 in current State - + ThresholdFilter: - linear_threshold: 2.5 - angular_threshold: 0.3 + name: ThresholdFilter + type: iirob_filters/ThresholdFilterWrench + params: {linear_threshold: 2.5, angular_threshold: 0.3} LowPassFilter: - Force_x: - SamplingFrequency: 200.0 - DampingFrequency: 15.0 - DampingIntensity: -6.0 - Force_y: - SamplingFrequency: 200.0 - DampingFrequency: 15.0 - DampingIntensity: -6.0 - Force_z: - SamplingFrequency: 200.0 - DampingFrequency: 15.0 - DampingIntensity: -6.0 - Torque_x: - SamplingFrequency: 200.0 - DampingFrequency: 15.0 - DampingIntensity: -6.0 - Torque_y: - SamplingFrequency: 200.0 - DampingFrequency: 15.0 - DampingIntensity: -6.0 - Torque_z: - SamplingFrequency: 200.0 - DampingFrequency: 15.0 - DampingIntensity: -6.0 - + name: LowPassFilter + type: iirob_filters/LowPassFilterWrench + params: {SamplingFrequency: 200.0, DampingFrequency: 15.0, DampingIntensity: -6.0} + MovingMeanFilter: - Force_x: - divider: 4 - Force_y: - divider: 4 - Force_z: - divider: 4 - Torque_x: - divider: 4 - Torque_y: - divider: 4 - Torque_z: - divider: 4 + name: MovingMeanFilter + type: iirob_filters/MovingMeanFilterWrench + params: {divider: 4} GravityCompensation: - #CoG: - #x: 0 - #y: 0 - #z: 0.027346102 # m - #force: 7.9459955 # f_G - world_frame: "world" - + name: GravityCompensation + type: iirob_filters/GravityCompensatorWrench + params: {world_frame: "base_link"} + Publish: sensor_data: true low_pass: true @@ -70,3 +38,4 @@ Publish: transformed_data: true gravity_compensated: true threshold_filtered: true + diff --git a/config/sensor_configuration_robot.yaml b/config/sensor_configuration_robot.yaml deleted file mode 100644 index 9840c5e..0000000 --- a/config/sensor_configuration_robot.yaml +++ /dev/null @@ -1,73 +0,0 @@ -Node: - sim: false - ft_pub_freq: 200 - ft_pull_freq: 800 - sensor_frame: "fts_reference_link" - transform_frame: "fts_transform_frame" - -Calibration: - Offset: - n_measurements: 500 - T_between_meas: 0.01 - isStatic: false # true= use static offset Calibration, false=use calibrate Sensor to 0 in current State - -ThresholdFilter: - linear_threshold: 2.5 - angular_threshold: 0.3 - -LowPassFilter: - Force_x: - SamplingFrequency: 200.0 - DampingFrequency: 15.0 - DampingIntensity: -6.0 - Force_y: - SamplingFrequency: 200.0 - DampingFrequency: 15.0 - DampingIntensity: -6.0 - Force_z: - SamplingFrequency: 200.0 - DampingFrequency: 15.0 - DampingIntensity: -6.0 - Torque_x: - SamplingFrequency: 200.0 - DampingFrequency: 15.0 - DampingIntensity: -6.0 - Torque_y: - SamplingFrequency: 200.0 - DampingFrequency: 15.0 - DampingIntensity: -6.0 - Torque_z: - SamplingFrequency: 200.0 - DampingFrequency: 15.0 - DampingIntensity: -6.0 - -MovingMeanFilter: - Force_x: - divider: 4 - Force_y: - divider: 4 - Force_z: - divider: 4 - Torque_x: - divider: 4 - Torque_y: - divider: 4 - Torque_z: - divider: 4 - -GravityCompensation: - #CoG: - #x: 0 - #y: 0 - #z: 0.027346102 # m - #force: 7.9459955 # f_G - sensor_frame: "fts_reference_link" - world_frame: "fts_transform_frame" - -Publish: - sensor_data: true - low_pass: true - moving_mean: true - transformed_data: true - gravity_compensated: true - threshold_filtered: true diff --git a/description/urdf/macros/mini58_sim.urdf.xacro b/description/urdf/macros/mini58_sim.urdf.xacro index f1ac709..a601e66 100644 --- a/description/urdf/macros/mini58_sim.urdf.xacro +++ b/description/urdf/macros/mini58_sim.urdf.xacro @@ -1,5 +1,8 @@ + + + diff --git a/description/urdf/sensor_mini58.urdf.xacro b/description/urdf/sensor_mini58.urdf.xacro index 14ec4b6..b95bc13 100644 --- a/description/urdf/sensor_mini58.urdf.xacro +++ b/description/urdf/sensor_mini58.urdf.xacro @@ -1,14 +1,29 @@ - + - + + + + + + + + + + + + + + + + + - + diff --git a/package.xml b/package.xml index 2ae7e97..e61bbec 100644 --- a/package.xml +++ b/package.xml @@ -37,6 +37,8 @@ tf2_geometry_msgs trajectory_msgs visualization_msgs + realtime_tools + filters message_runtime robot_state_publisher diff --git a/ros/include/ati_force_torque/force_torque_sensor.h b/ros/include/ati_force_torque/force_torque_sensor.h index 88552bb..06cade5 100644 --- a/ros/include/ati_force_torque/force_torque_sensor.h +++ b/ros/include/ati_force_torque/force_torque_sensor.h @@ -76,8 +76,10 @@ typedef unsigned char uint8_t; #include #include #include -#include #include +#include +#include +#include #include #include @@ -87,11 +89,15 @@ typedef unsigned char uint8_t; #include #include #include -#include #include #include #include +#include +#include +#include +#include + #define PI 3.14159265 class ForceTorqueSensor @@ -145,7 +151,8 @@ class ForceTorqueSensor geometry_msgs::Wrench offset_; geometry_msgs::TransformStamped transform_ee_base_stamped; tf2_ros::Buffer *p_tfBuffer; - ros::Publisher gravity_compensated_pub_, threshold_filtered_pub_, transformed_data_pub_, sensor_data_pub_, low_pass_pub_, moving_mean_pub_; + realtime_tools::RealtimePublisher *gravity_compensated_pub_, *threshold_filtered_pub_, *transformed_data_pub_, *sensor_data_pub_, *low_pass_pub_, *moving_mean_pub_; + bool is_pub_gravity_compensated_ = false; bool is_pub_threshold_filtered_ = false; bool is_pub_transformed_data_ = false; @@ -160,7 +167,7 @@ class ForceTorqueSensor std::string canPath; int canBaudrate; int ftsBaseID; - double nodePubFreq; + double nodePubFreq, nodePullFreq; uint calibrationNMeasurements; double calibrationTBetween; int coordinateSystemNMeasurements; @@ -188,42 +195,21 @@ class ForceTorqueSensor bool m_staticCalibration; geometry_msgs::Wrench m_calibOffset; - ThresholdFilter threshold_filter_; - - LowPassFilter lp_filter_force_x_; - LowPassFilter lp_filter_force_y_; - LowPassFilter lp_filter_force_z_; - LowPassFilter lp_filter_torque_x_; - LowPassFilter lp_filter_torque_y_; - LowPassFilter lp_filter_torque_z_; - MovingMeanFilter moving_mean_filter_force_x_; - MovingMeanFilter moving_mean_filter_force_y_; - MovingMeanFilter moving_mean_filter_force_z_; - MovingMeanFilter moving_mean_filter_torque_x_; - MovingMeanFilter moving_mean_filter_torque_y_; - MovingMeanFilter moving_mean_filter_torque_z_; - - GravityCompensator gravity_compensator_; - - bool useLowPassFilterForceX=false; - bool useLowPassFilterForceY=false; - bool useLowPassFilterForceZ=false; - bool useLowPassFilterTorqueX=false; - bool useLowPassFilterTorqueY=false; - bool useLowPassFilterTorqueZ=false; - bool useMovinvingMeanForceX= false; - bool useMovinvingMeanForceY= false; - bool useMovinvingMeanForceZ= false; - bool useMovinvingMeanTorqueX= false; - bool useMovinvingMeanTorqueY= false; - bool useMovinvingMeanTorqueZ= false; + + iirob_filters::IIrobFilterChain> low_pass_filter_; + iirob_filters::IIrobFilterChain> moving_mean_filter_; + iirob_filters::IIrobFilterChain> threshold_filter_; + iirob_filters::IIrobFilterChain> gravity_compensator_; + bool useGravityCompensator=false; bool useThresholdFilter=false; + + bool useMovingMean = false; + bool useLowPassFilter = false; + - dynamic_reconfigure::Server reconfigCalibrationSrv_; // Dynamic reconfiguration service - dynamic_reconfigure::Server reconfigPublishSrv_; // Dynamic reconfiguration service + dynamic_reconfigure::Server reconfigCalibrationSrv_; // Dynamic reconfiguration service - void reconfigureCalibrationRequest(ati_force_torque::CalibrationConfig& config, uint32_t level); - void reconfigurePublishRequest(ati_force_torque::PublishConfigurationConfig& config, uint32_t level); + void reconfigureCalibrationRequest(ati_force_torque::CalibrationConfig& config, uint32_t level); }; diff --git a/ros/launch/driver.launch b/ros/launch/driver.launch index 8e45994..b669902 100644 --- a/ros/launch/driver.launch +++ b/ros/launch/driver.launch @@ -4,7 +4,8 @@ - + + diff --git a/ros/src/force_torque_sensor.cpp b/ros/src/force_torque_sensor.cpp index 33feab5..8472578 100644 --- a/ros/src/force_torque_sensor.cpp +++ b/ros/src/force_torque_sensor.cpp @@ -54,9 +54,8 @@ ****************************************************************/ #include -ForceTorqueSensor::ForceTorqueSensor(ros::NodeHandle& nh) : nh_(nh), calibration_params_{nh.getNamespace()+"/Calibration/Offset"}, CS_params_{nh.getNamespace()}, can_params_{nh.getNamespace()+"/CAN"}, FTS_params_{nh.getNamespace()+"/FTS"}, pub_params_{nh.getNamespace()+"/Publish"}, node_params_{nh.getNamespace()+"/Node"}, gravity_params_{nh.getNamespace()+"/GravityCompensation"} -{ - calibration_params_.fromParamServer(); +ForceTorqueSensor::ForceTorqueSensor(ros::NodeHandle& nh) : nh_(nh), calibration_params_{nh.getNamespace()+"/Calibration/Offset"}, CS_params_{nh.getNamespace()}, can_params_{nh.getNamespace()+"/CAN"}, FTS_params_{nh.getNamespace()+"/FTS"}, pub_params_{nh.getNamespace()+"/Publish"}, node_params_{nh.getNamespace()+"/Node"}, gravity_params_{nh.getNamespace()+"/GravityCompensation/params"}//,low_pass_filter_("geometry_msgs::WrenchStamped"),threshold_filter_("geometry_msgs::WrenchStamped"),moving_mean_filter_("geometry_msgs::WrenchStamped"),gravity_compensator_("geometry_msgs::WrenchStamped") +{ CS_params_.fromParamServer(); can_params_.fromParamServer(); FTS_params_.fromParamServer(); @@ -64,24 +63,10 @@ ForceTorqueSensor::ForceTorqueSensor(ros::NodeHandle& nh) : nh_(nh), calibration node_params_.fromParamServer(); gravity_params_.fromParamServer(); - int calibNMeas; - calibNMeas=calibration_params_.n_measurements; - - if (calibNMeas <= 0) - { - ROS_WARN("Parameter 'Calibration/n_measurements' is %d (<=0) using default: 20", calibNMeas); - calibrationNMeasurements = 20; - } - else { - calibrationNMeasurements = (uint)calibNMeas; - } - calibrationTBetween=calibration_params_.T_between_meas; - m_staticCalibration=calibration_params_.isStatic; std::map forceVal,torqueVal; - forceVal= calibration_params_.force; - torqueVal= calibration_params_.torque; - + forceVal = calibration_params_.force; + torqueVal = calibration_params_.torque; m_calibOffset.force.x = forceVal["x"]; m_calibOffset.force.y = forceVal["y"]; @@ -91,7 +76,6 @@ ForceTorqueSensor::ForceTorqueSensor(ros::NodeHandle& nh) : nh_(nh), calibration m_calibOffset.torque.x = torqueVal["z"]; bool isAutoInit = false; - double nodePullFreq = 0; m_isInitialized = false; m_isCalibrated = false; srvServer_Init_ = nh_.advertiseService("Init", &ForceTorqueSensor::srvCallback_Init, this); @@ -100,18 +84,33 @@ ForceTorqueSensor::ForceTorqueSensor(ros::NodeHandle& nh) : nh_(nh), calibration srvServer_DetermineCoordianteSystem_ = nh_.advertiseService("DetermineCoordinateSystem", &ForceTorqueSensor::srvCallback_DetermineCoordinateSystem, this); srvServer_Temp_ = nh_.advertiseService("GetTemperature", &ForceTorqueSensor::srvReadDiagnosticVoltages, this); srvServer_ReCalibrate = nh_.advertiseService("Recalibrate", &ForceTorqueSensor::srvCallback_recalibrate, this); - reconfigCalibrationSrv_.setCallback(boost::bind(&ForceTorqueSensor::reconfigureCalibrationRequest, this, _1, _2)); - reconfigPublishSrv_.setCallback(boost::bind(&ForceTorqueSensor::reconfigurePublishRequest, this, _1, _2)); + + reconfigCalibrationSrv_.setCallback(boost::bind(&ForceTorqueSensor::reconfigureCalibrationRequest, this, _1, _2)); + calibration_params_.fromParamServer(); + int calibNMeas; + + calibNMeas=calibration_params_.n_measurements; + calibrationTBetween = calibration_params_.T_between_meas; + m_staticCalibration = calibration_params_.isStatic; + if (calibNMeas <= 0) + { + ROS_WARN("Parameter 'Calibration/n_measurements' is %d (<=0) using default: 20", calibNMeas); + calibrationNMeasurements = 20; + } + else { + calibrationNMeasurements = (uint)calibNMeas; + } + // Read data from parameter server canType = can_params_.type; canPath = can_params_.path; canBaudrate = can_params_.baudrate; - ftsBaseID=FTS_params_.base_identifier; - isAutoInit=FTS_params_.auto_init; - nodePubFreq=node_params_.ft_pub_freq; - nodePullFreq=node_params_.ft_pull_freq; - sensor_frame_=node_params_.sensor_frame; + ftsBaseID = FTS_params_.base_identifier; + isAutoInit = FTS_params_.auto_init; + nodePubFreq = node_params_.ft_pub_freq; + nodePullFreq = node_params_.ft_pull_freq; + sensor_frame_ = node_params_.sensor_frame; coordinateSystemNMeasurements = CS_params_.n_measurements; coordinateSystemTBetween = CS_params_.T_between_meas; @@ -123,93 +122,54 @@ ForceTorqueSensor::ForceTorqueSensor(ros::NodeHandle& nh) : nh_(nh), calibration //Wrench Publisher is_pub_gravity_compensated_ = pub_params_.gravity_compensated; if(is_pub_gravity_compensated_){ - gravity_compensated_pub_ = nh_.advertise("gravity_compensated", 1); + gravity_compensated_pub_ = new realtime_tools::RealtimePublisher(nh_, "gravity_compensated", 1); } is_pub_low_pass_ = pub_params_.low_pass; if(is_pub_low_pass_){ - low_pass_pub_ = nh_.advertise("low_pass", 1); + low_pass_pub_ = new realtime_tools::RealtimePublisher(nh_, "low_pass", 1); } is_pub_moving_mean_=pub_params_.moving_mean; if(is_pub_moving_mean_){ - moving_mean_pub_ = nh_.advertise("moving_mean", 1); + moving_mean_pub_ = new realtime_tools::RealtimePublisher(nh_, "moving_mean", 1); } is_pub_sensor_data_=pub_params_.sensor_data; if(is_pub_sensor_data_){ - sensor_data_pub_ = nh_.advertise("sensor_data", 1); + sensor_data_pub_ = new realtime_tools::RealtimePublisher(nh_, "sensor_data", 1); } is_pub_threshold_filtered_ =pub_params_.threshold_filtered; if(is_pub_threshold_filtered_){ - threshold_filtered_pub_ = nh_.advertise("threshold_filtered", 1); + threshold_filtered_pub_ = new realtime_tools::RealtimePublisher(nh_, "threshold_filtered", 1); } is_pub_transformed_data_ = pub_params_.transformed_data; if(is_pub_transformed_data_){ - transformed_data_pub_ = nh_.advertise("transformed_data", 1); + transformed_data_pub_ = new realtime_tools::RealtimePublisher(nh_, "transformed_data", 1); } ftUpdateTimer_ = nh.createTimer(ros::Rate(nodePubFreq), &ForceTorqueSensor::updateFTData, this, false, false); ftPullTimer_ = nh.createTimer(ros::Rate(nodePullFreq), &ForceTorqueSensor::pullFTData, this, false, false); - - //Lowpass Filter - if(nh_.hasParam("LowPassFilter/Force_x")) { - lp_filter_force_x_.init(ros::NodeHandle(nh_, "LowPassFilter/Force_x")); - useLowPassFilterForceX=true; - } - if(nh_.hasParam("LowPassFilter/Force_y")) { - lp_filter_force_y_.init(ros::NodeHandle(nh_, "LowPassFilter/Force_y")); - useLowPassFilterForceY=true; - } - if(nh_.hasParam("LowPassFilter/Force_z")) { - lp_filter_force_z_.init(ros::NodeHandle(nh_, "LowPassFilter/Force_z")); - useLowPassFilterForceZ=true; - } - if(nh_.hasParam("LowPassFilter/Torque_x")) { - lp_filter_torque_x_.init(ros::NodeHandle(nh_, "LowPassFilter/Torque_x")); - useLowPassFilterTorqueX=true; - } - if(nh_.hasParam("LowPassFilter/Torque_y")) { - lp_filter_torque_y_.init(ros::NodeHandle(nh_, "LowPassFilter/Torque_y")); - useLowPassFilterTorqueY=true; - } - if(nh_.hasParam("LowPassFilter/Torque_z")) { - lp_filter_torque_z_.init(ros::NodeHandle(nh_, "LowPassFilter/Torque_z")); - useLowPassFilterTorqueZ=true; - } - + + moving_mean_filter_.configure("MovingMeanFilter", nh_); + low_pass_filter_.configure("LowPassFilter", nh_); + gravity_compensator_.configure("GravityCompensation", nh_); + threshold_filter_.configure("ThresholdFilter", nh_); + //Median Filter - if(nh_.hasParam("MovingMeanFilter/Force_x")) { - useMovinvingMeanForceX=true; - moving_mean_filter_force_x_.init(ros::NodeHandle(nh_, "MovingMeanFilter/Force_x")); - } - if(nh_.hasParam("MovingMeanFilter/Force_y")) { - moving_mean_filter_force_y_.init(ros::NodeHandle(nh_, "MovingMeanFilter/Force_y")); - useMovinvingMeanForceY=true; - } - if(nh_.hasParam("MovingMeanFilter/Force_z")) { - moving_mean_filter_force_z_.init(ros::NodeHandle(nh_, "MovingMeanFilter/Force_z")); - useMovinvingMeanForceZ=true; + if(nh_.hasParam("MovingMeanFilter")) { + useMovingMean = true; } - if(nh_.hasParam("MovingMeanFilter/Torque_x")) { - moving_mean_filter_torque_x_.init(ros::NodeHandle(nh_, "MovingMeanFilter/Torque_x")); - useMovinvingMeanTorqueX=true; - } - if(nh_.hasParam("MovingMeanFilter/Torque_y")) { - moving_mean_filter_torque_y_.init(ros::NodeHandle(nh_, "MovingMeanFilter/Torque_y")); - useMovinvingMeanTorqueY=true; - } - if(nh_.hasParam("MovingMeanFilter/Torque_z")) { - moving_mean_filter_torque_z_.init(ros::NodeHandle(nh_, "MovingMeanFilter/Torque_z")); - useMovinvingMeanTorqueZ=true; + + //Low Pass Filter + if(nh_.hasParam("LowPassFilter")) { + useLowPassFilter = true; } - + //Gravity Compenstation if(nh_.hasParam("GravityCompensation")) { - gravity_compensator_.init(ros::NodeHandle(nh_, "GravityCompensation")); useGravityCompensator = true; } //Threshold Filter if(nh_.hasParam("ThresholdFilter")) { - threshold_filter_.init(ros::NodeHandle(nh_, "ThresholdFilter")); useThresholdFilter = true; } @@ -231,8 +191,6 @@ ForceTorqueSensor::ForceTorqueSensor(ros::NodeHandle& nh) : nh_(nh), calibration } } - - void ForceTorqueSensor::init_sensor(std::string& msg, bool& success) { if (!m_isInitialized) @@ -283,7 +241,7 @@ void ForceTorqueSensor::init_sensor(std::string& msg, bool& success) success = false; msg = "FTS could not be initilised! :/"; } - ftUpdateTimer_.start(); + ftUpdateTimer_.start(); } } @@ -383,14 +341,14 @@ bool ForceTorqueSensor::srvCallback_recalibrate(std_srvs::Trigger::Request& req, bool ForceTorqueSensor::calibrate(bool apply_after_calculation, geometry_msgs::Wrench *new_offset) { apply_offset = false; - ROS_INFO("Calibrating using %d measurements and %f s pause between measurements.", calibrationNMeasurements, calibrationTBetween); geometry_msgs::Wrench temp_offset = makeAverageMeasurement(calibrationNMeasurements, calibrationTBetween); apply_offset = true; if (apply_after_calculation) { offset_ = temp_offset; - } + } + ROS_INFO("Calibration Data: Fx: %f; Fy: %f; Fz: %f; Mx: %f; My: %f; Mz: %f", temp_offset.force.x, temp_offset.force.y, temp_offset.force.z, temp_offset.torque.x, temp_offset.torque.y, temp_offset.torque.z); m_isCalibrated = true; @@ -403,34 +361,31 @@ geometry_msgs::Wrench ForceTorqueSensor::makeAverageMeasurement(uint number_of_m { geometry_msgs::Wrench measurement; int num_of_errors = 0; - ros::Duration duration(time_between_meas); for (int i = 0; i < number_of_measurements; i++) { geometry_msgs::Wrench output; if (frame_id.compare("") != 0) { - if (not transform_wrench(frame_id, sensor_frame_, moving_mean_filtered_wrench.wrench, &output)) - { + if (not transform_wrench(frame_id, sensor_frame_, moving_mean_filtered_wrench.wrench, &output)) + { num_of_errors++; if (num_of_errors > 200){ return measurement; } i--; continue; - } + } } else { - output = moving_mean_filtered_wrench.wrench; + output = moving_mean_filtered_wrench.wrench; } - measurement.force.x += output.force.x; measurement.force.y += output.force.y; measurement.force.z += output.force.z; measurement.torque.x += output.torque.x; measurement.torque.y += output.torque.y; measurement.torque.z+= output.torque.z; - duration.sleep(); } measurement.force.x /= number_of_measurements; @@ -504,7 +459,6 @@ void ForceTorqueSensor::pullFTData(const ros::TimerEvent &event) { sensor_data.header.stamp = ros::Time::now(); sensor_data.header.frame_id = sensor_frame_; - if (apply_offset) { sensor_data.wrench.force.x -= offset_.force.x; sensor_data.wrench.force.y -= offset_.force.y; @@ -514,68 +468,86 @@ void ForceTorqueSensor::pullFTData(const ros::TimerEvent &event) sensor_data.wrench.torque.z -= offset_.torque.z; } - //lowpass - low_pass_filtered_data.header = sensor_data.header; - if(useLowPassFilterForceX) low_pass_filtered_data.wrench.force.x = lp_filter_force_x_.applyFilter(sensor_data.wrench.force.x); - else low_pass_filtered_data.wrench.force.x = sensor_data.wrench.force.x; - if(useLowPassFilterForceY) low_pass_filtered_data.wrench.force.y = lp_filter_force_y_.applyFilter(sensor_data.wrench.force.y); - else low_pass_filtered_data.wrench.force.y = sensor_data.wrench.force.y; - if(useLowPassFilterForceZ) low_pass_filtered_data.wrench.force.z = lp_filter_force_z_.applyFilter(sensor_data.wrench.force.z); - else low_pass_filtered_data.wrench.force.z = sensor_data.wrench.force.z; - if(useLowPassFilterTorqueX) low_pass_filtered_data.wrench.torque.x = lp_filter_torque_x_.applyFilter(sensor_data.wrench.torque.x); - else low_pass_filtered_data.wrench.torque.x = sensor_data.wrench.torque.x; - if(useLowPassFilterTorqueY) low_pass_filtered_data.wrench.torque.y = lp_filter_torque_y_.applyFilter(sensor_data.wrench.torque.y); - else low_pass_filtered_data.wrench.torque.y = sensor_data.wrench.torque.y; - if(useLowPassFilterTorqueZ) low_pass_filtered_data.wrench.torque.z = lp_filter_torque_z_.applyFilter(sensor_data.wrench.torque.z); - else low_pass_filtered_data.wrench.torque.z = sensor_data.wrench.torque.z; - - //moving_mean + //lowpass + low_pass_filtered_data.header = sensor_data.header; + if(useLowPassFilter){ + std::vector in_data= {(double)sensor_data.wrench.force.x, double(sensor_data.wrench.force.y), (double)sensor_data.wrench.force.z,(double)sensor_data.wrench.torque.x,(double)sensor_data.wrench.torque.y,(double)sensor_data.wrench.torque.z}; + std::vector out_data= {(double)low_pass_filtered_data.wrench.force.x, double(low_pass_filtered_data.wrench.force.y), (double)low_pass_filtered_data.wrench.force.z,(double)low_pass_filtered_data.wrench.torque.x,(double)low_pass_filtered_data.wrench.torque.y,(double)low_pass_filtered_data.wrench.torque.z}; + + low_pass_filter_.update(sensor_data,low_pass_filtered_data); + } + else low_pass_filtered_data = sensor_data; + //moving_mean moving_mean_filtered_wrench.header = low_pass_filtered_data.header; - if(useMovinvingMeanForceX) moving_mean_filtered_wrench.wrench.force.x = moving_mean_filter_force_x_.applyFilter(low_pass_filtered_data.wrench.force.x); - else moving_mean_filtered_wrench.wrench.force.x = sensor_data.wrench.force.x; - if(useMovinvingMeanForceY) moving_mean_filtered_wrench.wrench.force.y = moving_mean_filter_force_y_.applyFilter(low_pass_filtered_data.wrench.force.y); - else moving_mean_filtered_wrench.wrench.force.y = sensor_data.wrench.force.y; - if(useMovinvingMeanForceZ) moving_mean_filtered_wrench.wrench.force.z = moving_mean_filter_force_z_.applyFilter(low_pass_filtered_data.wrench.force.z); - else moving_mean_filtered_wrench.wrench.force.z = sensor_data.wrench.force.z; - if(useMovinvingMeanTorqueX) moving_mean_filtered_wrench.wrench.torque.x = moving_mean_filter_torque_x_.applyFilter(low_pass_filtered_data.wrench.torque.x); - else moving_mean_filtered_wrench.wrench.torque.x = sensor_data.wrench.torque.x; - if(useMovinvingMeanTorqueY) moving_mean_filtered_wrench.wrench.torque.y = moving_mean_filter_torque_y_.applyFilter(low_pass_filtered_data.wrench.torque.y); - else moving_mean_filtered_wrench.wrench.torque.y = sensor_data.wrench.torque.y; - if(useMovinvingMeanTorqueZ) moving_mean_filtered_wrench.wrench.torque.z = moving_mean_filter_torque_z_.applyFilter(low_pass_filtered_data.wrench.torque.z); - else moving_mean_filtered_wrench.wrench.torque.z = sensor_data.wrench.torque.z; - + if(useMovingMean){ + std::vector in_data= {(double)low_pass_filtered_data.wrench.force.x, double(low_pass_filtered_data.wrench.force.y), (double)low_pass_filtered_data.wrench.force.z,(double)low_pass_filtered_data.wrench.torque.x,(double)low_pass_filtered_data.wrench.torque.y,(double)low_pass_filtered_data.wrench.torque.z}; + std::vector out_data = {(double)moving_mean_filtered_wrench.wrench.force.x, double(moving_mean_filtered_wrench.wrench.force.y), (double)moving_mean_filtered_wrench.wrench.force.z,(double)moving_mean_filtered_wrench.wrench.torque.x,(double)moving_mean_filtered_wrench.wrench.torque.y,(double)moving_mean_filtered_wrench.wrench.torque.z}; + + moving_mean_filter_.update(low_pass_filtered_data, moving_mean_filtered_wrench); + } + else moving_mean_filtered_wrench = low_pass_filtered_data; + if(is_pub_sensor_data_) - sensor_data_pub_.publish(sensor_data); + if (sensor_data_pub_->trylock()){ + sensor_data_pub_->msg_ = sensor_data; + sensor_data_pub_->unlockAndPublish(); + } + if(is_pub_low_pass_) - low_pass_pub_.publish(low_pass_filtered_data); - if(is_pub_moving_mean_) - moving_mean_pub_.publish(moving_mean_filtered_wrench); + if (low_pass_pub_->trylock()){ + low_pass_pub_->msg_ = low_pass_filtered_data; + low_pass_pub_->unlockAndPublish(); + } + + if(is_pub_moving_mean_) + if (moving_mean_pub_->trylock()){ + moving_mean_pub_->msg_ = moving_mean_filtered_wrench; + moving_mean_pub_->unlockAndPublish(); + } } } void ForceTorqueSensor::filterFTData(){ - transformed_data.header.stamp = moving_mean_filtered_wrench.header.stamp; transformed_data.header.frame_id = transform_frame_; if (transform_wrench(transform_frame_, sensor_frame_, moving_mean_filtered_wrench.wrench, &transformed_data.wrench)) { //gravity compensation - if(useGravityCompensator) gravity_compensated_force = gravity_compensator_.compensate(transformed_data); + if(useGravityCompensator) + { + std::vector in_data= {(double)transformed_data.wrench.force.x, double(transformed_data.wrench.force.y), (double)transformed_data.wrench.force.z,(double)transformed_data.wrench.torque.x,(double)transformed_data.wrench.torque.y,(double)transformed_data.wrench.torque.z}; + std::vector out_data = {(double)gravity_compensated_force.wrench.force.x, double(gravity_compensated_force.wrench.force.y), (double)gravity_compensated_force.wrench.force.z,(double)gravity_compensated_force.wrench.torque.x,(double)gravity_compensated_force.wrench.torque.y,(double)gravity_compensated_force.wrench.torque.z}; + gravity_compensator_.update(transformed_data, gravity_compensated_force); + } else gravity_compensated_force = transformed_data; //treshhold filtering - if(useThresholdFilter) threshold_filtered_force = threshold_filter_.applyFilter(gravity_compensated_force); + if(useThresholdFilter) + { + std::vector in_data= {(double)gravity_compensated_force.wrench.force.x, double(gravity_compensated_force.wrench.force.y), (double)gravity_compensated_force.wrench.force.z,(double)gravity_compensated_force.wrench.torque.x,(double)gravity_compensated_force.wrench.torque.y,(double)gravity_compensated_force.wrench.torque.z}; + std::vector out_data = {(double)threshold_filtered_force.wrench.force.x, double(threshold_filtered_force.wrench.force.y), (double)threshold_filtered_force.wrench.force.z,(double)threshold_filtered_force.wrench.torque.x,(double)threshold_filtered_force.wrench.torque.y,(double)threshold_filtered_force.wrench.torque.z}; + threshold_filter_.update(gravity_compensated_force, threshold_filtered_force); + } else threshold_filtered_force = gravity_compensated_force; if(is_pub_transformed_data_) - transformed_data_pub_.publish(transformed_data); + if (transformed_data_pub_->trylock()){ + transformed_data_pub_->msg_ = transformed_data; + transformed_data_pub_->unlockAndPublish(); + } if(is_pub_gravity_compensated_ && useGravityCompensator) - gravity_compensated_pub_.publish(gravity_compensated_force); + if (gravity_compensated_pub_->trylock()){ + gravity_compensated_pub_->msg_ = gravity_compensated_force; + gravity_compensated_pub_->unlockAndPublish(); + } + if(is_pub_threshold_filtered_ && useThresholdFilter) - threshold_filtered_pub_.publish(threshold_filtered_force); + if (threshold_filtered_pub_->trylock()){ + threshold_filtered_pub_->msg_ = threshold_filtered_force; + threshold_filtered_pub_->unlockAndPublish(); + } } - else threshold_filtered_force = moving_mean_filtered_wrench; } bool ForceTorqueSensor::transform_wrench(std::string goal_frame, std::string source_frame, geometry_msgs::Wrench wrench, geometry_msgs::Wrench *transformed) @@ -607,11 +579,22 @@ bool ForceTorqueSensor::transform_wrench(std::string goal_frame, std::string sou return true; } -void ForceTorqueSensor::reconfigureCalibrationRequest(ati_force_torque::CalibrationConfig& config, uint32_t level){ +void ForceTorqueSensor::reconfigureCalibrationRequest(ati_force_torque::CalibrationConfig& config, uint32_t level){ calibration_params_.fromConfig(config); -} -void ForceTorqueSensor::reconfigurePublishRequest(ati_force_torque::PublishConfigurationConfig& config, uint32_t level){ + calibrationTBetween = calibration_params_.T_between_meas; + m_staticCalibration = calibration_params_.isStatic; - pub_params_.fromConfig(config); + std::map forceVal,torqueVal; + forceVal = calibration_params_.force; + torqueVal = calibration_params_.torque; + + m_calibOffset.force.x = forceVal["x"]; + m_calibOffset.force.y = forceVal["y"]; + m_calibOffset.force.z = forceVal["z"]; + m_calibOffset.torque.x = torqueVal["x"]; + m_calibOffset.torque.x = torqueVal["y"]; + m_calibOffset.torque.x = torqueVal["z"]; + } +