From ad3afdef23de853a8c9762816117338c9950b616 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Sat, 27 May 2017 15:45:15 +0200 Subject: [PATCH 01/53] Removed dependecy on schunk_description (#6) * Removed dependecy on schunk_description * Added xacro dependency --- package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/package.xml b/package.xml index b1155c9..096565f 100644 --- a/package.xml +++ b/package.xml @@ -24,7 +24,6 @@ geometry_msgs hardware_interface iirob_filters - schunk_description std_msgs std_srvs tf2 @@ -35,6 +34,7 @@ message_runtime robot_state_publisher + xacro roslaunch From e95298c9556a5c85dd2e035fbe11b07905e73de0 Mon Sep 17 00:00:00 2001 From: Andreea Tulbure Date: Wed, 31 May 2017 10:51:45 +0200 Subject: [PATCH 02/53] changes for working raw with fts --- config/sensor_configuration.yaml | 8 +++++--- config/sensor_offset.yaml | 4 ++-- 2 files changed, 7 insertions(+), 5 deletions(-) diff --git a/config/sensor_configuration.yaml b/config/sensor_configuration.yaml index 885d34b..e4a63f7 100644 --- a/config/sensor_configuration.yaml +++ b/config/sensor_configuration.yaml @@ -2,12 +2,12 @@ Node: ft_pub_freq: 200 ft_pull_freq: 800 sensor_frame: "fts_reference_link" - transform_frame: "fts_base_link" + transform_frame: "fts_transform_frame" Calibration: n_measurements: 500 T_between_meas: 0.01 - static: true # true= use static offset Calibration, false=use calibrate Sensor to 0 in current State + static: false # true= use static offset Calibration, false=use calibrate Sensor to 0 in current State ThresholdFilter: linear_threshold: 2.5 @@ -59,7 +59,9 @@ GravityCompensation: #y: 0 #z: 0.027346102 # m #force: 7.9459955 # f_G - world_frame: "world" + sensor_frame: "fts_reference_link" + world_frame: "fts_transform_frame" + Publish: sensor_data: true diff --git a/config/sensor_offset.yaml b/config/sensor_offset.yaml index 85325ad..1ace112 100644 --- a/config/sensor_offset.yaml +++ b/config/sensor_offset.yaml @@ -1,2 +1,2 @@ -force: {x: -19.104102666356425, y: -1.5473841984049037, z: 0.9332121238751951} -torque: {x: -0.24642145895240927, y: 0.0373995095506341, z: 0.060428603494093} +force: {x: 29.3789054311, y: -4.02509469552, z: -7.12669364182} +torque: {x: -0.30390995786, y: -0.747661107339, z: 1.11224200823} From 5f95cf6ab4c8e048b443dd8f100ac0457bffdf6c Mon Sep 17 00:00:00 2001 From: Andreea Tulbure Date: Mon, 19 Jun 2017 10:19:00 +0200 Subject: [PATCH 03/53] some changes for namespace read --- CMakeLists.txt | 23 ++++ cfg/sensorConfiguration.params | 77 +++++++++++ package.xml | 2 + .../ati_force_torque/force_torque_sensor.h | 31 ++++- ros/src/force_torque_sensor.cpp | 128 +++++++++++++----- 5 files changed, 226 insertions(+), 35 deletions(-) create mode 100755 cfg/sensorConfiguration.params diff --git a/CMakeLists.txt b/CMakeLists.txt index 0d0f658..fb7517d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -14,6 +14,8 @@ find_package(catkin REQUIRED COMPONENTS cob_generic_can cmake_modules iirob_filters + rosparam_handler + dynamic_reconfigure ) find_package(Eigen3) @@ -26,6 +28,21 @@ else() set(Eigen_LIBRARIES ${EIGEN3_LIBRARIES}) endif() +include(CheckCXXCompilerFlag) +CHECK_CXX_COMPILER_FLAG("-std=c++14" Cpp14CompilerFlag) +if (Cpp14CompilerFlag) + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14") +elseif(CMAKE_CXX_COMPILER_ID STREQUAL "MSVC" AND NOT CMAKE_CXX_COMPILER_VERSION VERSION_LESS "17") + #no additional flag is required +else() + message(FATAL_ERROR "Compiler does not have c++14 support. Use at least g++4.9 or Visual Studio 2013 and newer.") +endif() + +generate_ros_parameter_files( + cfg/sensorConfiguration.params +) + + add_service_files( FILES CalculateAverageMasurement.srv @@ -62,16 +79,22 @@ include_directories( ) add_library(${PROJECT_NAME} common/src/ForceTorqueCtrl.cpp ros/src/force_torque_sensor.cpp ros/src/force_torque_sensor_handle.cpp) +add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_gencfg) # For dynamic_reconfigure add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_generate_messages_cpp) +#add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_genparam) # For rosparam_handler target_link_libraries(${PROJECT_NAME} gravity_compensation low_pass_filter moving_mean_filter threshold_filter) add_executable(${PROJECT_NAME}_node ros/src/force_torque_sensor_node.cpp) add_dependencies(${PROJECT_NAME}_node ${PROJECT_NAME}_generate_messages_cpp) +#add_dependencies(${PROJECT_NAME}_node ${PROJECT_NAME}_gencfg) # For dynamic_reconfigure +#add_dependencies(${PROJECT_NAME}_node ${PROJECT_NAME}_genparam) # For rosparam_handler target_link_libraries(${PROJECT_NAME}_node ${PROJECT_NAME} ${catkin_LIBRARIES} ${Eigen_LIBRARIES} gravity_compensation low_pass_filter moving_mean_filter threshold_filter) add_executable(${PROJECT_NAME}_config ros/src/force_torque_sensor_config.cpp) target_link_libraries(${PROJECT_NAME}_config ${PROJECT_NAME} ${catkin_LIBRARIES} ${Eigen_LIBRARIES}) add_dependencies(${PROJECT_NAME}_config ${PROJECT_NAME}_generate_messages_cpp) +#add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_gencfg) # For dynamic_reconfigure +#add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_genparam) # For rosparam_handler ############# ## Install ## diff --git a/cfg/sensorConfiguration.params b/cfg/sensorConfiguration.params new file mode 100755 index 0000000..cbb1eca --- /dev/null +++ b/cfg/sensorConfiguration.params @@ -0,0 +1,77 @@ +#!/usr/bin/env python +from rosparam_handler.parameter_generator_catkin import * +gen = ParameterGenerator() + +#Add a Node group +my_group = gen.add_group("Node") +my_group.add("ft_pub_freq", paramtype="double", description="publish frequency", default=200, configurable=True) +my_group.add("ft_pull_freq", paramtype="double", description="sensor daa pull frequency", default=800, configurable=True) +my_group.add("transform_frameNode", paramtype="std::string", description="reference coordinate system", default="fts_transform_frame", configurable=True) +my_group.add("sensor_frameNode", paramtype="std::string", description="sensor coordinate system", default="fts_reference_link", configurable=True) +#Add a Calibration group +my_group = gen.add_group("Calibration") +my_group.add("n_measurements", paramtype="double", description="number of necessary measurements for calibration", default=500, configurable=True) +my_group.add("T_between_meas", paramtype="double", description="time between two measurements for calibration", default=0.01, configurable=True) +my_group.add("isStatic", paramtype="bool", description="is static calibration active", default=False, configurable=True) + +#Add a Publish group +my_group = gen.add_group("Publish") +my_group.add("sensor_data", paramtype="bool", description="publsih raw sensor data", default=True, configurable=True) +my_group.add("low_pass", paramtype="bool", description="publish low pass filtered data", default=True, configurable=True) +my_group.add("moving_mean", paramtype="bool", description="publish moving mean filtered data", default=True, configurable=True) +my_group.add("transformed_data", paramtype="bool", description="publish transformed data", default=True, configurable=True) +my_group.add("gravity_compensated", paramtype="bool", description="publish gravity compensated data", default=True, configurable=True) +my_group.add("threshold_filtered", paramtype="bool", description="publish threshold filtered data", default=True, configurable=True) + +#Add a Threshold Filter group +my_group = gen.add_group("ThresholdFilter") +my_group.add("linear_threshold", paramtype="double", description="number of measurements for initial calibration", default=2.5, configurable=True) +my_group.add("angular_threshold", paramtype="double", description="time between two measurements", default=0.3, configurable=True) + +# Add a Lowpass Filter group +my_group = gen.add_group("LowPassFilter") +my_group.add("SamplingFrequency", paramtype="int", description="sampling frequency", default=200, configurable=True) +my_group.add("DampingFrequency", paramtype="int", description="sampling frequency", default=15, configurable=True) +my_group.add("DampingIntensity", paramtype="double", description="sampling frequency", default=-6.0, configurable=True) +my_subgroup = my_group.add_group("Force_x") +my_subgroup.add("SamplingFrequencyForce_x", paramtype="int", description="sampling frequency", default=200, configurable=True) +my_subgroup.add("DampingFrequencyForce_x", paramtype="double", description="damping frequency", default=15.0, configurable=True) +my_subgroup.add("DampingIntensityForce_x", paramtype="double", description="damping intensity", default=-6.0, configurable=True) +my_subgroup = my_group.add_group("Force_y") +my_subgroup.add("SamplingFrequencyForce_y", paramtype="int", description="sampling frequency", default=200, configurable=True) +my_subgroup.add("DampingFrequencyForce_y", paramtype="double", description="damping frequency", default=15.0, configurable=True) +my_subgroup.add("DampingIntensityForce_y", paramtype="double", description="damping intensity", default=-6.0, configurable=True) +my_subgroup = my_group.add_group("Force_z") +my_subgroup.add("SamplingFrequencyForce_z", paramtype="int", description="sampling frequency", default=200, configurable=True) +my_subgroup.add("DampingFrequencyForce_z", paramtype="double", description="damping frequency", default=15.0, configurable=True) +my_subgroup.add("DampingIntensityForce_z", paramtype="double", description="damping intensity", default=-6.0, configurable=True) +my_subgroup = my_group.add_group("Torque_x") +my_subgroup.add("SamplingFrequencyTorque_x", paramtype="int", description="sampling frequency", default=200, configurable=True) +my_subgroup.add("DampingFrequencyTorque_x", paramtype="double", description="damping frequency", default=15.0, configurable=True) +my_subgroup.add("DampingIntensityTorque_x", paramtype="double", description="damping intensity", default=-6.0, configurable=True) +my_subgroup = my_group.add_group("Torque_y") +my_subgroup.add("SamplingFrequencyTorque_y", paramtype="int", description="sampling frequency", default=200, configurable=True) +my_subgroup.add("DampingFrequencyTorque_y", paramtype="double", description="damping frequency", default=15.0, configurable=True) +my_subgroup.add("DampingIntensityTorque_y", paramtype="double", description="damping intensity", default=-6.0, configurable=True) +my_subgroup = my_group.add_group("Torque_z") +my_subgroup.add("SamplingFrequencyTorque_z", paramtype="int", description="sampling frequency", default=200, configurable=True) +my_subgroup.add("DampingFrequencyTorque_z", paramtype="double", description="damping frequency", default=15.0, configurable=True) +my_subgroup.add("DampingIntensityTorque_z", paramtype="double", description="damping intensity", default=-6.0, configurable=True) + +# Add a MovingMean Filter group +my_group = gen.add_group("MovingMeanFilter") +my_group.add("divider", paramtype="int", description="mean divider", default=4, configurable=True) +my_group.add("dividerForce_x", paramtype="int", description="mean divider", default=4, configurable=True) +my_group.add("dividerForce_y", paramtype="int", description="mean divider", default=4, configurable=True) +my_group.add("dividerForce_z", paramtype="int", description="mean divider", default=4, configurable=True) +my_group.add("dividerTorque_x", paramtype="int", description="mean divider", default=4, configurable=True) +my_group.add("dividerTorque_y", paramtype="int", description="mean divider", default=4, configurable=True) +my_group.add("dividerTorque_z", paramtype="int", description="mean divider", default=4, configurable=True) + +# Add a Gravity Compensation group +my_group = gen.add_group("GravityCompensation") +my_group.add("sensor_frameGravityCompensation", paramtype="std::string", description="sensor coordinate system", default="fts_reference_link", configurable=True) +my_group.add("transform_frameGravityCompensation", paramtype="std::string", description="tranformed coordinate system", default="fts_transform_link", configurable=True) + +#Syntax : Package, Node, Config Name(The final name will be MyDummyConfig) +exit(gen.generate("ati_force_torque", "ForceTorqueSensor", "sensorConfiguration")) \ No newline at end of file diff --git a/package.xml b/package.xml index d3e39f3..9ad6aaf 100644 --- a/package.xml +++ b/package.xml @@ -28,6 +28,8 @@ cob_generic_can schunk_description iirob_filters + rosparam_handler + dynamic_reconfigure message_runtime diff --git a/ros/include/ati_force_torque/force_torque_sensor.h b/ros/include/ati_force_torque/force_torque_sensor.h index d0b6512..5c596f7 100644 --- a/ros/include/ati_force_torque/force_torque_sensor.h +++ b/ros/include/ati_force_torque/force_torque_sensor.h @@ -80,6 +80,11 @@ typedef unsigned char uint8_t; #include #include + +#include +#include +#include + #define PI 3.14159265 class ForceTorqueSensor @@ -99,7 +104,8 @@ class ForceTorqueSensor bool srvCallback_recalibrate(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res); protected: - + ati_force_torque::sensorConfigurationParameters params_; +// ati_force_torque::sensorConfigurationConfig config_; std::string transform_frame_; std::string sensor_frame_; @@ -152,6 +158,7 @@ class ForceTorqueSensor ros::ServiceServer srvServer_ReCalibrate; ros::Timer ftUpdateTimer_, ftPullTimer_; +// ros::Timer timer_; tf2_ros::TransformListener *p_tfListener; tf2::Transform transform_ee_base; @@ -179,5 +186,25 @@ class ForceTorqueSensor MovingMeanFilter moving_mean_filter_torque_y_; MovingMeanFilter moving_mean_filter_torque_z_; - GravityCompensator gravity_compensator_; + 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; + bool useGravityCompensator=false; + bool useThresholdFilter=false; +// dynamic_reconfigure::Server reconfigSrv_; // Dynamic reconfiguration service + //boost::scoped_ptr< dynamic_reconfigure::Server > reconfigure_server_; + //void timerCallback(const ros::TimerEvent& event); +// void reconfigureRequest(ati_force_torque::sensorConfigurationConfig& config, uint32_t level); }; + diff --git a/ros/src/force_torque_sensor.cpp b/ros/src/force_torque_sensor.cpp index bd8d789..46d2d9d 100644 --- a/ros/src/force_torque_sensor.cpp +++ b/ros/src/force_torque_sensor.cpp @@ -54,8 +54,10 @@ ****************************************************************/ #include -ForceTorqueSensor::ForceTorqueSensor(ros::NodeHandle& nh) : nh_(nh) +ForceTorqueSensor::ForceTorqueSensor(ros::NodeHandle& nh) : nh_(nh), params_{ros::NodeHandle("~")}//,reconfigSrv_{ros::NodeHandle("~")} { + params_.fromParamServer(); + bool isAutoInit = false; double nodePullFreq = 0; m_isInitialized = false; @@ -67,6 +69,7 @@ ForceTorqueSensor::ForceTorqueSensor(ros::NodeHandle& nh) : nh_(nh) 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); +// reconfigSrv_.setCallback(boost::bind(&ForceTorqueSensor::reconfigureRequest, this, _1, _2)); // Read data from parameter server nh_.param("CAN/type", canType, -1); @@ -136,27 +139,69 @@ ForceTorqueSensor::ForceTorqueSensor(ros::NodeHandle& nh) : nh_(nh) ftPullTimer_ = nh.createTimer(ros::Rate(nodePullFreq), &ForceTorqueSensor::pullFTData, this, false, false); //Lowpass Filter - lp_filter_force_x_.init(ros::NodeHandle(nh_, "LowPassFilter/Force_x")); - lp_filter_force_y_.init(ros::NodeHandle(nh_, "LowPassFilter/Force_y")); - lp_filter_force_z_.init(ros::NodeHandle(nh_, "LowPassFilter/Force_z")); - lp_filter_torque_x_.init(ros::NodeHandle(nh_, "LowPassFilter/Torque_x")); - lp_filter_torque_y_.init(ros::NodeHandle(nh_, "LowPassFilter/Torque_y")); - lp_filter_torque_z_.init(ros::NodeHandle(nh_, "LowPassFilter/Torque_z")); + 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; + } //Median Filter - moving_mean_filter_force_x_.init(ros::NodeHandle(nh_, "MovingMeanFilter/Force_x")); - moving_mean_filter_force_y_.init(ros::NodeHandle(nh_, "MovingMeanFilter/Force_y")); - moving_mean_filter_force_z_.init(ros::NodeHandle(nh_, "MovingMeanFilter/Force_z")); - moving_mean_filter_torque_x_.init(ros::NodeHandle(nh_, "MovingMeanFilter/Torque_x")); - moving_mean_filter_torque_y_.init(ros::NodeHandle(nh_, "MovingMeanFilter/Torque_y")); - moving_mean_filter_torque_z_.init(ros::NodeHandle(nh_, "MovingMeanFilter/Torque_z")); + if(ros::param::has("MovingMeanFilter/Force_x")) { + useMovinvingMeanForceX=true; + moving_mean_filter_force_x_.init(ros::NodeHandle(nh_, "MovingMeanFilter/Force_x")); + } + if(ros::param::has("MovingMeanFilter/Force_y")) { + moving_mean_filter_force_y_.init(ros::NodeHandle(nh_, "MovingMeanFilter/Force_y")); + useMovinvingMeanForceY=true; + } + if(ros::param::has("MovingMeanFilter/Force_z")) { + moving_mean_filter_force_z_.init(ros::NodeHandle(nh_, "MovingMeanFilter/Force_z")); + useMovinvingMeanForceZ=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; + } //Gravity Compenstation - gravity_compensator_.init(ros::NodeHandle(nh_, "GravityCompensation")); - + if(nh_.hasParam("GravityCompensation")) { + gravity_compensator_.init(ros::NodeHandle(nh_, "GravityCompensation")); + useThresholdFilter = true; + } + //Threshold Filter - threshold_filter_.init(ros::NodeHandle(nh_, "ThresholdFilter")); - + if(nh_.hasParam("ThresholdFilter")) { + threshold_filter_.init(ros::NodeHandle(nh_, "ThresholdFilter")); + useThresholdFilter = true; + } + if (canType != -1) { p_Ftc = new ForceTorqueCtrl(canType, canPath, canBaudrate, ftsBaseID); @@ -173,6 +218,8 @@ ForceTorqueSensor::ForceTorqueSensor(ros::NodeHandle& nh) : nh_(nh) init_sensor(msg, success); ROS_INFO("Autoinit: %s", msg.c_str()); } + + std::cout<<"params: "< Date: Wed, 28 Jun 2017 12:50:31 +0200 Subject: [PATCH 04/53] rosparam working --- CMakeLists.txt | 9 +- cfg/Calibration.params | 16 ++ cfg/CanConfiguration.params | 10 ++ cfg/CoordinateSystemCalibration.params | 10 ++ cfg/FTSConfiguration.params | 10 ++ cfg/NodeConfiguration.params | 11 ++ cfg/PublishConfiguration.params | 13 ++ cfg/sensorConfiguration.params | 77 ---------- config/sensor_configuration.yaml | 9 +- config/sensor_offset.yaml | 2 - config/socket_can_ati.yaml | 2 +- .../ati_force_torque/force_torque_sensor.h | 31 +++- ros/launch/sensor_parameters.launch | 3 - ros/src/force_torque_sensor.cpp | 144 +++++++++--------- 14 files changed, 184 insertions(+), 163 deletions(-) create mode 100755 cfg/Calibration.params create mode 100755 cfg/CanConfiguration.params create mode 100755 cfg/CoordinateSystemCalibration.params create mode 100755 cfg/FTSConfiguration.params create mode 100755 cfg/NodeConfiguration.params create mode 100755 cfg/PublishConfiguration.params delete mode 100755 cfg/sensorConfiguration.params delete mode 100644 config/sensor_offset.yaml diff --git a/CMakeLists.txt b/CMakeLists.txt index fb7517d..f8b3aba 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -39,7 +39,12 @@ else() endif() generate_ros_parameter_files( - cfg/sensorConfiguration.params + cfg/CoordinateSystemCalibration.params + cfg/CanConfiguration.params + cfg/FTSConfiguration.params + cfg/NodeConfiguration.params + cfg/PublishConfiguration.params + cfg/Calibration.params ) @@ -81,7 +86,7 @@ include_directories( add_library(${PROJECT_NAME} common/src/ForceTorqueCtrl.cpp ros/src/force_torque_sensor.cpp ros/src/force_torque_sensor_handle.cpp) add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_gencfg) # For dynamic_reconfigure add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_generate_messages_cpp) -#add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_genparam) # For rosparam_handler +add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_genparam) # For rosparam_handler target_link_libraries(${PROJECT_NAME} gravity_compensation low_pass_filter moving_mean_filter threshold_filter) add_executable(${PROJECT_NAME}_node ros/src/force_torque_sensor_node.cpp) diff --git a/cfg/Calibration.params b/cfg/Calibration.params new file mode 100755 index 0000000..6e654a3 --- /dev/null +++ b/cfg/Calibration.params @@ -0,0 +1,16 @@ +#!/usr/bin/env python +from rosparam_handler.parameter_generator_catkin import * +gen = ParameterGenerator() + +gen.add("n_measurements", paramtype="int", description="number of necessary measurements for calibration", default=10, configurable=True) +gen.add("T_between_meas", paramtype="double", description="time between two measurements for calibration", default=0.01, configurable=True) +gen.add("isStatic", paramtype="bool", description="is static calibration active", default=False, configurable=True) +gen.add("Offset_Force_x", paramtype="double", description="calibration_offset_force_x", default=29.3789054311, configurable=True) +gen.add("Offset_Force_y", paramtype="double", description="calibration_offset_force_y", default=-4.02509469552, configurable=True) +gen.add("Offset_Force_z", paramtype="double", description="calibration_offset_force_z", default=-7.12669364182, configurable=True) +gen.add("Offset_Torque_x", paramtype="double", description="calibration_offset_torque_x", default=-0.30390995786, configurable=True) +gen.add("Offset_Torque_y", paramtype="double", description="calibration_offset_torque_y", default=-0.747661107339, configurable=True) +gen.add("Offset_Torque_z", paramtype="double", description="calibration_offset_torque_z", default=1.11224200823, configurable=True) + +#Syntax : Package, Node, Config Name(The final name will be MyDummyConfig) +exit(gen.generate("ati_force_torque", "ForceTorqueSensor", "Calibration")) \ No newline at end of file diff --git a/cfg/CanConfiguration.params b/cfg/CanConfiguration.params new file mode 100755 index 0000000..7854f99 --- /dev/null +++ b/cfg/CanConfiguration.params @@ -0,0 +1,10 @@ +#!/usr/bin/env python +from rosparam_handler.parameter_generator_catkin import * +gen = ParameterGenerator() + +gen.add("type", paramtype="int", description="socket_can", default=-1, configurable=False) +gen.add("path", paramtype="std::string", description="socket_can", default=" ", configurable=False) +gen.add("baudrate", paramtype="int", description="baudrate", default=-1, configurable=False) + +#Syntax : Package, Node, Config Name(The final name will be MyDummyConfig) +exit(gen.generate("ati_force_torque", "ForceTorqueSensor", "CanConfiguration")) \ No newline at end of file diff --git a/cfg/CoordinateSystemCalibration.params b/cfg/CoordinateSystemCalibration.params new file mode 100755 index 0000000..2be6d42 --- /dev/null +++ b/cfg/CoordinateSystemCalibration.params @@ -0,0 +1,10 @@ +#!/usr/bin/env python +from rosparam_handler.parameter_generator_catkin import * +gen = ParameterGenerator() + +gen.add("n_measurements", paramtype="int", description="number of necessary measurements for calibration", default=20, configurable=False) +gen.add("T_between_meas", paramtype="int", description="time between two measurements for calibration", default=10000, configurable=False) +gen.add("push_direction", paramtype="int", description="push direction", default=0, configurable=False) + +#Syntax : Package, Node, Config Name(The final name will be MyDummyConfig) +exit(gen.generate("ati_force_torque", "ForceTorqueSensor", "CoordinateSystemCalibration")) \ No newline at end of file diff --git a/cfg/FTSConfiguration.params b/cfg/FTSConfiguration.params new file mode 100755 index 0000000..b9f915d --- /dev/null +++ b/cfg/FTSConfiguration.params @@ -0,0 +1,10 @@ +#!/usr/bin/env python +from rosparam_handler.parameter_generator_catkin import * +gen = ParameterGenerator() + +gen.add("base_identifier", paramtype="int", description="identifier", default=0x20, configurable=False) +gen.add("auto_init", paramtype="bool", description="auto init", default=True, configurable=False) +gen.add("fts_name", paramtype="std::string", description="name of sensor", default="ATI_45_Mini", configurable=False) + +#Syntax : Package, Node, Config Name(The final name will be MyDummyConfig) +exit(gen.generate("ati_force_torque", "ForceTorqueSensor", "FTSConfiguration")) \ No newline at end of file diff --git a/cfg/NodeConfiguration.params b/cfg/NodeConfiguration.params new file mode 100755 index 0000000..78ade7a --- /dev/null +++ b/cfg/NodeConfiguration.params @@ -0,0 +1,11 @@ +#!/usr/bin/env python +from rosparam_handler.parameter_generator_catkin import * +gen = ParameterGenerator() + +gen.add("ft_pub_freq", paramtype="double", description="publish frequency", default=200, configurable=False) +gen.add("ft_pull_freq", paramtype="double", description="sensor daa pull frequency", default=800, configurable=False) +gen.add("transform_frame", paramtype="std::string", description="reference coordinate system", default="fts_transform_frame", configurable=False) +gen.add("sensor_frame", paramtype="std::string", description="sensor coordinate system", default="fts_reference_link", configurable=False) + +#Syntax : Package, Node, Config Name(The final name will be MyDummyConfig) +exit(gen.generate("ati_force_torque", "ForceTorqueSensor", "NodeConfiguration")) \ No newline at end of file diff --git a/cfg/PublishConfiguration.params b/cfg/PublishConfiguration.params new file mode 100755 index 0000000..0a9a3a6 --- /dev/null +++ b/cfg/PublishConfiguration.params @@ -0,0 +1,13 @@ +#!/usr/bin/env python +from rosparam_handler.parameter_generator_catkin import * +gen = ParameterGenerator() + +gen.add("sensor_data", paramtype="bool", description="publsih raw sensor data", default=True, configurable=True) +gen.add("low_pass", paramtype="bool", description="publish low pass filtered data", default=True, configurable=True) +gen.add("moving_mean", paramtype="bool", description="publish moving mean filtered data", default=True, configurable=True) +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) + +#Syntax : Package, Node, Config Name(The final name will be MyDummyConfig) +exit(gen.generate("ati_force_torque", "ForceTorqueSensor", "PublishConfiguration")) \ No newline at end of file diff --git a/cfg/sensorConfiguration.params b/cfg/sensorConfiguration.params deleted file mode 100755 index cbb1eca..0000000 --- a/cfg/sensorConfiguration.params +++ /dev/null @@ -1,77 +0,0 @@ -#!/usr/bin/env python -from rosparam_handler.parameter_generator_catkin import * -gen = ParameterGenerator() - -#Add a Node group -my_group = gen.add_group("Node") -my_group.add("ft_pub_freq", paramtype="double", description="publish frequency", default=200, configurable=True) -my_group.add("ft_pull_freq", paramtype="double", description="sensor daa pull frequency", default=800, configurable=True) -my_group.add("transform_frameNode", paramtype="std::string", description="reference coordinate system", default="fts_transform_frame", configurable=True) -my_group.add("sensor_frameNode", paramtype="std::string", description="sensor coordinate system", default="fts_reference_link", configurable=True) -#Add a Calibration group -my_group = gen.add_group("Calibration") -my_group.add("n_measurements", paramtype="double", description="number of necessary measurements for calibration", default=500, configurable=True) -my_group.add("T_between_meas", paramtype="double", description="time between two measurements for calibration", default=0.01, configurable=True) -my_group.add("isStatic", paramtype="bool", description="is static calibration active", default=False, configurable=True) - -#Add a Publish group -my_group = gen.add_group("Publish") -my_group.add("sensor_data", paramtype="bool", description="publsih raw sensor data", default=True, configurable=True) -my_group.add("low_pass", paramtype="bool", description="publish low pass filtered data", default=True, configurable=True) -my_group.add("moving_mean", paramtype="bool", description="publish moving mean filtered data", default=True, configurable=True) -my_group.add("transformed_data", paramtype="bool", description="publish transformed data", default=True, configurable=True) -my_group.add("gravity_compensated", paramtype="bool", description="publish gravity compensated data", default=True, configurable=True) -my_group.add("threshold_filtered", paramtype="bool", description="publish threshold filtered data", default=True, configurable=True) - -#Add a Threshold Filter group -my_group = gen.add_group("ThresholdFilter") -my_group.add("linear_threshold", paramtype="double", description="number of measurements for initial calibration", default=2.5, configurable=True) -my_group.add("angular_threshold", paramtype="double", description="time between two measurements", default=0.3, configurable=True) - -# Add a Lowpass Filter group -my_group = gen.add_group("LowPassFilter") -my_group.add("SamplingFrequency", paramtype="int", description="sampling frequency", default=200, configurable=True) -my_group.add("DampingFrequency", paramtype="int", description="sampling frequency", default=15, configurable=True) -my_group.add("DampingIntensity", paramtype="double", description="sampling frequency", default=-6.0, configurable=True) -my_subgroup = my_group.add_group("Force_x") -my_subgroup.add("SamplingFrequencyForce_x", paramtype="int", description="sampling frequency", default=200, configurable=True) -my_subgroup.add("DampingFrequencyForce_x", paramtype="double", description="damping frequency", default=15.0, configurable=True) -my_subgroup.add("DampingIntensityForce_x", paramtype="double", description="damping intensity", default=-6.0, configurable=True) -my_subgroup = my_group.add_group("Force_y") -my_subgroup.add("SamplingFrequencyForce_y", paramtype="int", description="sampling frequency", default=200, configurable=True) -my_subgroup.add("DampingFrequencyForce_y", paramtype="double", description="damping frequency", default=15.0, configurable=True) -my_subgroup.add("DampingIntensityForce_y", paramtype="double", description="damping intensity", default=-6.0, configurable=True) -my_subgroup = my_group.add_group("Force_z") -my_subgroup.add("SamplingFrequencyForce_z", paramtype="int", description="sampling frequency", default=200, configurable=True) -my_subgroup.add("DampingFrequencyForce_z", paramtype="double", description="damping frequency", default=15.0, configurable=True) -my_subgroup.add("DampingIntensityForce_z", paramtype="double", description="damping intensity", default=-6.0, configurable=True) -my_subgroup = my_group.add_group("Torque_x") -my_subgroup.add("SamplingFrequencyTorque_x", paramtype="int", description="sampling frequency", default=200, configurable=True) -my_subgroup.add("DampingFrequencyTorque_x", paramtype="double", description="damping frequency", default=15.0, configurable=True) -my_subgroup.add("DampingIntensityTorque_x", paramtype="double", description="damping intensity", default=-6.0, configurable=True) -my_subgroup = my_group.add_group("Torque_y") -my_subgroup.add("SamplingFrequencyTorque_y", paramtype="int", description="sampling frequency", default=200, configurable=True) -my_subgroup.add("DampingFrequencyTorque_y", paramtype="double", description="damping frequency", default=15.0, configurable=True) -my_subgroup.add("DampingIntensityTorque_y", paramtype="double", description="damping intensity", default=-6.0, configurable=True) -my_subgroup = my_group.add_group("Torque_z") -my_subgroup.add("SamplingFrequencyTorque_z", paramtype="int", description="sampling frequency", default=200, configurable=True) -my_subgroup.add("DampingFrequencyTorque_z", paramtype="double", description="damping frequency", default=15.0, configurable=True) -my_subgroup.add("DampingIntensityTorque_z", paramtype="double", description="damping intensity", default=-6.0, configurable=True) - -# Add a MovingMean Filter group -my_group = gen.add_group("MovingMeanFilter") -my_group.add("divider", paramtype="int", description="mean divider", default=4, configurable=True) -my_group.add("dividerForce_x", paramtype="int", description="mean divider", default=4, configurable=True) -my_group.add("dividerForce_y", paramtype="int", description="mean divider", default=4, configurable=True) -my_group.add("dividerForce_z", paramtype="int", description="mean divider", default=4, configurable=True) -my_group.add("dividerTorque_x", paramtype="int", description="mean divider", default=4, configurable=True) -my_group.add("dividerTorque_y", paramtype="int", description="mean divider", default=4, configurable=True) -my_group.add("dividerTorque_z", paramtype="int", description="mean divider", default=4, configurable=True) - -# Add a Gravity Compensation group -my_group = gen.add_group("GravityCompensation") -my_group.add("sensor_frameGravityCompensation", paramtype="std::string", description="sensor coordinate system", default="fts_reference_link", configurable=True) -my_group.add("transform_frameGravityCompensation", paramtype="std::string", description="tranformed coordinate system", default="fts_transform_link", configurable=True) - -#Syntax : Package, Node, Config Name(The final name will be MyDummyConfig) -exit(gen.generate("ati_force_torque", "ForceTorqueSensor", "sensorConfiguration")) \ No newline at end of file diff --git a/config/sensor_configuration.yaml b/config/sensor_configuration.yaml index e4a63f7..1d90af3 100644 --- a/config/sensor_configuration.yaml +++ b/config/sensor_configuration.yaml @@ -7,7 +7,13 @@ Node: Calibration: n_measurements: 500 T_between_meas: 0.01 - static: false # true= use static offset Calibration, false=use calibrate Sensor to 0 in current State + isStatic: false # true= use static offset Calibration, false=use calibrate Sensor to 0 in current State + Offset_Force_x: 29.3789054311 + Offset_Force_y: -4.02509469552 + Offset_Force_z: -7.12669364182 + Offset_Torque_x: -0.30390995786 + Offset_Torque_y: -0.747661107339 + Offset_Torque_z: 1.11224200823 ThresholdFilter: linear_threshold: 2.5 @@ -62,7 +68,6 @@ GravityCompensation: sensor_frame: "fts_reference_link" world_frame: "fts_transform_frame" - Publish: sensor_data: true low_pass: true diff --git a/config/sensor_offset.yaml b/config/sensor_offset.yaml deleted file mode 100644 index 1ace112..0000000 --- a/config/sensor_offset.yaml +++ /dev/null @@ -1,2 +0,0 @@ -force: {x: 29.3789054311, y: -4.02509469552, z: -7.12669364182} -torque: {x: -0.30390995786, y: -0.747661107339, z: 1.11224200823} diff --git a/config/socket_can_ati.yaml b/config/socket_can_ati.yaml index 3e78629..d1e59b3 100644 --- a/config/socket_can_ati.yaml +++ b/config/socket_can_ati.yaml @@ -5,4 +5,4 @@ CAN: FTS: base_identifier: 0x20 auto_init: true - name: "ATI_45_Mini" + fts_name: "ATI_45_Mini" diff --git a/ros/include/ati_force_torque/force_torque_sensor.h b/ros/include/ati_force_torque/force_torque_sensor.h index 5c596f7..3cc0ec2 100644 --- a/ros/include/ati_force_torque/force_torque_sensor.h +++ b/ros/include/ati_force_torque/force_torque_sensor.h @@ -74,6 +74,7 @@ typedef unsigned char uint8_t; #include +#include #include #include #include @@ -82,8 +83,12 @@ typedef unsigned char uint8_t; #include #include -#include -#include +#include +#include +#include +#include +#include +#include #define PI 3.14159265 @@ -104,8 +109,14 @@ class ForceTorqueSensor bool srvCallback_recalibrate(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res); protected: - ati_force_torque::sensorConfigurationParameters params_; -// ati_force_torque::sensorConfigurationConfig config_; + ati_force_torque::CoordinateSystemCalibrationParameters CS_params_; + ati_force_torque::CanConfigurationParameters can_params_; + ati_force_torque::FTSConfigurationParameters FTS_params_; + ati_force_torque::PublishConfigurationParameters pub_params_; + ati_force_torque::NodeConfigurationParameters node_params_; + ati_force_torque::CalibrationParameters calibration_params_; + iirob_filters::GravityCompensationParameters gravity_params_; + std::string transform_frame_; std::string sensor_frame_; @@ -133,7 +144,12 @@ class ForceTorqueSensor 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_; - bool is_pub_gravity_compensated_, is_pub_threshold_filtered_, is_pub_transformed_data_, is_pub_sensor_data_, is_pub_low_pass_, is_pub_moving_mean_; + bool is_pub_gravity_compensated_=false; + bool is_pub_threshold_filtered_=false; + bool is_pub_transformed_data_ =false; + bool is_pub_sensor_data_=false; + bool is_pub_low_pass_=false; + bool is_pub_moving_mean_=false; uint _num_transform_errors; @@ -202,9 +218,8 @@ class ForceTorqueSensor bool useMovinvingMeanTorqueZ= false; bool useGravityCompensator=false; bool useThresholdFilter=false; -// dynamic_reconfigure::Server reconfigSrv_; // Dynamic reconfiguration service - //boost::scoped_ptr< dynamic_reconfigure::Server > reconfigure_server_; + //dynamic_reconfigure::Server reconfigSrv_; // Dynamic reconfiguration service //void timerCallback(const ros::TimerEvent& event); -// void reconfigureRequest(ati_force_torque::sensorConfigurationConfig& config, uint32_t level); + //void reconfigureRequest(ati_force_torque::sensorConfigurationConfig& config, uint32_t level); }; diff --git a/ros/launch/sensor_parameters.launch b/ros/launch/sensor_parameters.launch index 384c373..2fc1b21 100644 --- a/ros/launch/sensor_parameters.launch +++ b/ros/launch/sensor_parameters.launch @@ -5,8 +5,5 @@ - - - diff --git a/ros/src/force_torque_sensor.cpp b/ros/src/force_torque_sensor.cpp index 46d2d9d..7e463ba 100644 --- a/ros/src/force_torque_sensor.cpp +++ b/ros/src/force_torque_sensor.cpp @@ -54,36 +54,50 @@ ****************************************************************/ #include -ForceTorqueSensor::ForceTorqueSensor(ros::NodeHandle& nh) : nh_(nh), params_{ros::NodeHandle("~")}//,reconfigSrv_{ros::NodeHandle("~")} +ForceTorqueSensor::ForceTorqueSensor(ros::NodeHandle& nh) : nh_(nh), CS_params_{nh}, can_params_{nh}, FTS_params_{nh} , pub_params_{nh} , node_params_{nh} , calibration_params_{nh} , gravity_params_{nh} { - params_.fromParamServer(); + can_params_.setNamespace(nh_.getNamespace()+"/CAN"); + CS_params_.setNamespace(nh_.getNamespace()); + FTS_params_.setNamespace(nh_.getNamespace()+"/FTS"); + pub_params_.setNamespace(nh_.getNamespace()+"/Publish"); + node_params_.setNamespace(nh_.getNamespace()+"/Node"); + calibration_params_.setNamespace(nh_.getNamespace()+"/Calibration"); + gravity_params_.setNamespace(nh_.getNamespace()+"/GravityCompensation"); + + CS_params_.fromParamServer(); + can_params_.fromParamServer(); + FTS_params_.fromParamServer(); + pub_params_.fromParamServer(); + node_params_.fromParamServer(); + calibration_params_.fromParamServer(); + gravity_params_.fromParamServer(); + bool isAutoInit = false; double nodePullFreq = 0; m_isInitialized = false; m_isCalibrated = false; - srvServer_Init_ = nh_.advertiseService("Init", &ForceTorqueSensor::srvCallback_Init, this); srvServer_CalculateAverageMasurement_ = nh_.advertiseService("CalculateAverageMasurement", &ForceTorqueSensor::srvCallback_CalculateAverageMasurement, this); srvServer_CalculateOffset_ = nh_.advertiseService("CalculateOffsets", &ForceTorqueSensor::srvCallback_CalculateOffset, this); 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); -// reconfigSrv_.setCallback(boost::bind(&ForceTorqueSensor::reconfigureRequest, this, _1, _2)); +// reconfigSrv_.setCallback(boost::bind(&ForceTorqueSensor::reconfigureRequest, this, _1, _2)); // Read data from parameter server - nh_.param("CAN/type", canType, -1); - - nh_.param("CAN/path", canPath, ""); - nh_.param("CAN/baudrate", canBaudrate, -1); - nh_.param("FTS/base_identifier", ftsBaseID, -1); - nh_.param("FTS/auto_init", isAutoInit, false); - nh_.param("Node/ft_pub_freq", nodePubFreq, 100); - nh_.param("Node/ft_pull_freq", nodePullFreq, 100); - nh_.param("Node/sensor_frame", sensor_frame_, "fts_reference_link"); + 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; int calibNMeas; - nh_.param("Calibration/n_measurements", calibNMeas, 20); + calibNMeas=calibration_params_.n_measurements; + if (calibNMeas <= 0) { ROS_WARN("Parameter 'Calibration/n_measurements' is %d (<=0) using default: 20", calibNMeas); @@ -92,49 +106,48 @@ ForceTorqueSensor::ForceTorqueSensor(ros::NodeHandle& nh) : nh_(nh), params_{ro else { calibrationNMeasurements = (uint)calibNMeas; } - nh_.param("Calibration/T_between_meas", calibrationTBetween, 0.01); - nh_.param("Calibration/static", m_staticCalibration, 0); - nh_.param("Calibration/Offset/force/x", m_calibOffset.force.x, 0); - nh_.param("Calibration/Offset/force/y", m_calibOffset.force.y, 0); - nh_.param("Calibration/Offset/force/z", m_calibOffset.force.z, 0); - nh_.param("Calibration/Offset/torque/x", m_calibOffset.torque.x, 0); - nh_.param("Calibration/Offset/torque/y", m_calibOffset.torque.y, 0); - nh_.param("Calibration/Offset/torque/z", m_calibOffset.torque.z, 0); - nh_.param("CoordinateSystemCal/n_measurements", coordinateSystemNMeasurements, 20); - nh_.param("CoordinateSystemCal/T_between_meas", coordinateSystemTBetween, 10000); - nh_.param("CoordinateSystemCal/push_direction", coordinateSystemPushDirection, 0); + calibrationTBetween=calibration_params_.T_between_meas; + m_staticCalibration=calibration_params_.isStatic; + m_calibOffset.force.x = calibration_params_.Offset_Force_x; + m_calibOffset.force.y = calibration_params_.Offset_Force_y; + m_calibOffset.force.z = calibration_params_.Offset_Force_z; + m_calibOffset.torque.x = calibration_params_.Offset_Torque_x; + m_calibOffset.torque.y = calibration_params_.Offset_Torque_y; + m_calibOffset.torque.z = calibration_params_.Offset_Torque_z; + + coordinateSystemNMeasurements = CS_params_.n_measurements; + coordinateSystemTBetween = CS_params_.T_between_meas; + coordinateSystemPushDirection = CS_params_.push_direction; p_tfBuffer = new tf2_ros::Buffer(); p_tfListener = new tf2_ros::TransformListener(*p_tfBuffer, true); - //Wrench Publisher - nh_.param("Publish/gravity_compensated", is_pub_gravity_compensated_, false); + //Wrench Publisher + is_pub_gravity_compensated_ = pub_params_.gravity_compensated; if(is_pub_gravity_compensated_){ gravity_compensated_pub_ = nh_.advertise("gravity_compensated", 1); } - nh_.param("Publish/low_pass", is_pub_low_pass_, false); + is_pub_low_pass_ = pub_params_.low_pass; if(is_pub_low_pass_){ - low_pass_pub_ = nh_.advertise("low_pass", 1); + low_pass_pub_ = nh_.advertise("low_pass", 1); } - nh_.param("Publish/moving_mean", is_pub_moving_mean_, false); + is_pub_moving_mean_=pub_params_.moving_mean; if(is_pub_moving_mean_){ moving_mean_pub_ = nh_.advertise("moving_mean", 1); } - nh_.param("Publish/sensor_data", is_pub_sensor_data_, false); + is_pub_sensor_data_=pub_params_.sensor_data; if(is_pub_sensor_data_){ sensor_data_pub_ = nh_.advertise("sensor_data", 1); } - nh_.param("Publish/threshold_filtered", is_pub_threshold_filtered_, false); + is_pub_threshold_filtered_ =pub_params_.threshold_filtered; if(is_pub_threshold_filtered_){ threshold_filtered_pub_ = nh_.advertise("threshold_filtered", 1); } - nh_.param("Publish/transformed_data", is_pub_transformed_data_, false); + is_pub_transformed_data_ = pub_params_.transformed_data; if(is_pub_transformed_data_){ transformed_data_pub_ = nh_.advertise("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); @@ -165,15 +178,15 @@ ForceTorqueSensor::ForceTorqueSensor(ros::NodeHandle& nh) : nh_(nh), params_{ro } //Median Filter - if(ros::param::has("MovingMeanFilter/Force_x")) { + if(nh_.hasParam("MovingMeanFilter/Force_x")) { useMovinvingMeanForceX=true; moving_mean_filter_force_x_.init(ros::NodeHandle(nh_, "MovingMeanFilter/Force_x")); } - if(ros::param::has("MovingMeanFilter/Force_y")) { + if(nh_.hasParam("MovingMeanFilter/Force_y")) { moving_mean_filter_force_y_.init(ros::NodeHandle(nh_, "MovingMeanFilter/Force_y")); useMovinvingMeanForceY=true; } - if(ros::param::has("MovingMeanFilter/Force_z")) { + if(nh_.hasParam("MovingMeanFilter/Force_z")) { moving_mean_filter_force_z_.init(ros::NodeHandle(nh_, "MovingMeanFilter/Force_z")); useMovinvingMeanForceZ=true; } @@ -193,7 +206,7 @@ ForceTorqueSensor::ForceTorqueSensor(ros::NodeHandle& nh) : nh_(nh), params_{ro //Gravity Compenstation if(nh_.hasParam("GravityCompensation")) { gravity_compensator_.init(ros::NodeHandle(nh_, "GravityCompensation")); - useThresholdFilter = true; + useGravityCompensator = true; } //Threshold Filter @@ -201,7 +214,7 @@ ForceTorqueSensor::ForceTorqueSensor(ros::NodeHandle& nh) : nh_(nh), params_{ro threshold_filter_.init(ros::NodeHandle(nh_, "ThresholdFilter")); useThresholdFilter = true; } - + if (canType != -1) { p_Ftc = new ForceTorqueCtrl(canType, canPath, canBaudrate, ftsBaseID); @@ -218,8 +231,6 @@ ForceTorqueSensor::ForceTorqueSensor(ros::NodeHandle& nh) : nh_(nh), params_{ro init_sensor(msg, success); ROS_INFO("Autoinit: %s", msg.c_str()); } - - std::cout<<"params: "<lookupTransform(sensor_frame_, transform_frame_, ros::Time(0))); @@ -379,7 +390,6 @@ bool ForceTorqueSensor::calibrate(bool apply_after_calculation, geometry_msgs::W geometry_msgs::Wrench temp_offset = makeAverageMeasurement(calibrationNMeasurements, calibrationTBetween); apply_offset = true; - if (apply_after_calculation) { offset_ = temp_offset; } @@ -431,7 +441,6 @@ geometry_msgs::Wrench ForceTorqueSensor::makeAverageMeasurement(uint number_of_m measurement.torque.x /= number_of_measurements; measurement.torque.y /= number_of_measurements; measurement.torque.z /= number_of_measurements; - return measurement; } @@ -510,20 +519,32 @@ void ForceTorqueSensor::pullFTData(const ros::TimerEvent &event) //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 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(is_pub_sensor_data_) sensor_data_pub_.publish(sensor_data); @@ -543,17 +564,20 @@ void ForceTorqueSensor::filterFTData(){ { //gravity compensation if(useGravityCompensator) gravity_compensated_force = gravity_compensator_.compensate(transformed_data); + else gravity_compensated_force = transformed_data; //treshhold filtering if(useThresholdFilter) threshold_filtered_force = threshold_filter_.applyFilter(gravity_compensated_force); + else threshold_filtered_force = gravity_compensated_force; if(is_pub_transformed_data_) transformed_data_pub_.publish(transformed_data); - if(is_pub_gravity_compensated_) + if(is_pub_gravity_compensated_ && useGravityCompensator) gravity_compensated_pub_.publish(gravity_compensated_force); - if(is_pub_threshold_filtered_) + if(is_pub_threshold_filtered_ && useThresholdFilter) threshold_filtered_pub_.publish(threshold_filtered_force); } + 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) @@ -585,19 +609,3 @@ bool ForceTorqueSensor::transform_wrench(std::string goal_frame, std::string sou return true; } -/* - * Use const ConstPtr for your callbacks. - * The 'const' assures that you can not edit incoming messages. - * The Ptr type guarantees zero copy transportation within nodelets. - */ -/*void ForceTorqueSensor::timerCallback(const ros::TimerEvent& event) { - ROS_INFO("timer"); -}*/ - -/** - * This callback is called whenever a change was made in the dynamic_reconfigure window -*/ -/*void ForceTorqueSensor::reconfigureRequest(ati_force_torque::sensorConfigurationConfig& config, uint32_t level) { - params_.fromConfig(config); - ROS_WARN_STREAM("Parameter update:\n" << params_); -}*/ From fca758b35d5f5bea2ffc07f4e10d22d51ac98687 Mon Sep 17 00:00:00 2001 From: Andreea Tulbure Date: Wed, 12 Jul 2017 11:20:27 +0200 Subject: [PATCH 05/53] added cs calibration params --- cfg/CoordinateSystemCalibration.params | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/cfg/CoordinateSystemCalibration.params b/cfg/CoordinateSystemCalibration.params index 2be6d42..8950b75 100755 --- a/cfg/CoordinateSystemCalibration.params +++ b/cfg/CoordinateSystemCalibration.params @@ -2,8 +2,8 @@ from rosparam_handler.parameter_generator_catkin import * gen = ParameterGenerator() -gen.add("n_measurements", paramtype="int", description="number of necessary measurements for calibration", default=20, configurable=False) -gen.add("T_between_meas", paramtype="int", description="time between two measurements for calibration", default=10000, configurable=False) +gen.add("n_measurements", paramtype="int", description="number of necessary measurements for calibration", default=20, configurable=True) +gen.add("T_between_meas", paramtype="int", description="time between two measurements for calibration", default=10000, configurable=True) gen.add("push_direction", paramtype="int", description="push direction", default=0, configurable=False) #Syntax : Package, Node, Config Name(The final name will be MyDummyConfig) From b3a71d452113b0fa4ec0b7ea83aeba5118f28446 Mon Sep 17 00:00:00 2001 From: Andreea Tulbure Date: Fri, 21 Jul 2017 18:02:38 +0200 Subject: [PATCH 06/53] added necessary code for simulation --- CMakeLists.txt | 2 +- cfg/CoordinateSystemCalibration.params | 2 +- .../ati_force_torque/force_torque_sensor.h | 11 +- .../force_torque_sensor_sim.h | 101 +++++++++++++++++ ros/src/force_torque_sensor.cpp | 11 +- ros/src/force_torque_sensor_sim.cpp | 107 ++++++++++++++++++ 6 files changed, 228 insertions(+), 6 deletions(-) create mode 100644 ros/include/ati_force_torque/force_torque_sensor_sim.h create mode 100644 ros/src/force_torque_sensor_sim.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index f8b3aba..8f18671 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -83,7 +83,7 @@ include_directories( ${Eigen_INCLUDE_DIRS} ) -add_library(${PROJECT_NAME} common/src/ForceTorqueCtrl.cpp ros/src/force_torque_sensor.cpp ros/src/force_torque_sensor_handle.cpp) +add_library(${PROJECT_NAME} common/src/ForceTorqueCtrl.cpp ros/src/force_torque_sensor.cpp ros/src/force_torque_sensor_sim.cpp ros/src/force_torque_sensor_handle.cpp) add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_gencfg) # For dynamic_reconfigure add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_generate_messages_cpp) add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_genparam) # For rosparam_handler diff --git a/cfg/CoordinateSystemCalibration.params b/cfg/CoordinateSystemCalibration.params index 2be6d42..4859c70 100755 --- a/cfg/CoordinateSystemCalibration.params +++ b/cfg/CoordinateSystemCalibration.params @@ -7,4 +7,4 @@ gen.add("T_between_meas", paramtype="int", description="time between two measure gen.add("push_direction", paramtype="int", description="push direction", default=0, configurable=False) #Syntax : Package, Node, Config Name(The final name will be MyDummyConfig) -exit(gen.generate("ati_force_torque", "ForceTorqueSensor", "CoordinateSystemCalibration")) \ No newline at end of file +exit(gen.generate("ati_force_torque", "ForceTorqueSensor", "CoordinateSystemCalibration")) diff --git a/ros/include/ati_force_torque/force_torque_sensor.h b/ros/include/ati_force_torque/force_torque_sensor.h index 3cc0ec2..a9aa4bf 100644 --- a/ros/include/ati_force_torque/force_torque_sensor.h +++ b/ros/include/ati_force_torque/force_torque_sensor.h @@ -87,8 +87,10 @@ typedef unsigned char uint8_t; #include #include #include +#include #include #include +#include #define PI 3.14159265 @@ -218,8 +220,11 @@ class ForceTorqueSensor bool useMovinvingMeanTorqueZ= false; bool useGravityCompensator=false; bool useThresholdFilter=false; - //dynamic_reconfigure::Server reconfigSrv_; // Dynamic reconfiguration service - //void timerCallback(const ros::TimerEvent& event); - //void reconfigureRequest(ati_force_torque::sensorConfigurationConfig& config, uint32_t level); + + dynamic_reconfigure::Server reconfigCalibrationSrv_; // Dynamic reconfiguration service + dynamic_reconfigure::Server reconfigPublishSrv_; // Dynamic reconfiguration service + + void reconfigureCalibrationRequest(ati_force_torque::CalibrationConfig& config, uint32_t level); + void reconfigurePublishRequest(ati_force_torque::PublishConfigurationConfig& config, uint32_t level); }; diff --git a/ros/include/ati_force_torque/force_torque_sensor_sim.h b/ros/include/ati_force_torque/force_torque_sensor_sim.h new file mode 100644 index 0000000..5dc9f2e --- /dev/null +++ b/ros/include/ati_force_torque/force_torque_sensor_sim.h @@ -0,0 +1,101 @@ +/**************************************************************** + * + * Copyright 2016 Intelligent Industrial Robotics (IIROB) Group, + * Institute for Anthropomatics and Robotics (IAR) - + * Intelligent Process Control and Robotics (IPR), + * Karlsruhe Institute of Technology + * + * Maintainers: Denis Štogl, email: denis.stogl@kit.edu + * Florian Heller + * Vanessa Streuer + * + * Date of update: 2014-2016 + * + * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ + * + * Copyright (c) 2010 + * + * Fraunhofer Institute for Manufacturing Engineering + * and Automation (IPA) + * + * Author: Alexander Bubeck, email:alexander.bubeck@ipa.fhg.de + * Supervised by: Alexander Bubeck, email:alexander.bubeck@ipa.fhg.de + * + * Date of creation: June 2010 + * + * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License LGPL as + * published by the Free Software Foundation, either version 3 of the + * License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License LGPL for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License LGPL along with this program. + * If not, see . + * + ****************************************************************/ +#include +typedef unsigned char uint8_t; +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include + + +#define PI 3.14159265 + +class ForceTorqueSensorSim +{ +public: + ForceTorqueSensorSim(ros::NodeHandle &nh); + void init_sensor(); + +protected: + + std::string transform_frame_; + std::string sensor_frame_; + + void pullFTData(const geometry_msgs::WrenchStamped::ConstPtr& joystick_input); + + // Arrays for dumping FT-Data + geometry_msgs::WrenchStamped threshold_filtered_force, transformed_data, joystick_data; + + +private: + bool transform_wrench(std::string goal_frame, std::string source_frame, geometry_msgs::Wrench wrench, geometry_msgs::Wrench *transformed); + + ros::Subscriber force_input_subscriber; + uint _num_transform_errors; + tf2_ros::Buffer *p_tfBuffer; + tf2_ros::TransformListener *p_tfListener; + ros::NodeHandle nh_; +}; + diff --git a/ros/src/force_torque_sensor.cpp b/ros/src/force_torque_sensor.cpp index 7e463ba..5ccfafb 100644 --- a/ros/src/force_torque_sensor.cpp +++ b/ros/src/force_torque_sensor.cpp @@ -83,7 +83,8 @@ ForceTorqueSensor::ForceTorqueSensor(ros::NodeHandle& nh) : nh_(nh), CS_params_ 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); -// reconfigSrv_.setCallback(boost::bind(&ForceTorqueSensor::reconfigureRequest, this, _1, _2)); + reconfigCalibrationSrv_.setCallback(boost::bind(&ForceTorqueSensor::reconfigureCalibrationRequest, this, _1, _2)); + reconfigPublishSrv_.setCallback(boost::bind(&ForceTorqueSensor::reconfigurePublishRequest, this, _1, _2)); // Read data from parameter server canType = can_params_.type; @@ -609,3 +610,11 @@ bool ForceTorqueSensor::transform_wrench(std::string goal_frame, std::string sou return true; } +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){ + + pub_params_.fromConfig(config); +} \ No newline at end of file diff --git a/ros/src/force_torque_sensor_sim.cpp b/ros/src/force_torque_sensor_sim.cpp new file mode 100644 index 0000000..dcc5edf --- /dev/null +++ b/ros/src/force_torque_sensor_sim.cpp @@ -0,0 +1,107 @@ +/**************************************************************** + * + * Copyright 2016 Intelligent Industrial Robotics (IIROB) Group, + * Institute for Anthropomatics and Robotics (IAR) - + * Intelligent Process Control and Robotics (IPR), + * Karlsruhe Institute of Technology + * + * Maintainers: Denis Štogl, email: denis.stogl@kit.edu + * Andreea Tulbure + * + * Date of update: 2014-2016 + * + * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ + * + * Copyright (c) 2010 + * + * Fraunhofer Institute for Manufacturing Engineering + * and Automation (IPA) + * + * Author: Alexander Bubeck, email:alexander.bubeck@ipa.fhg.de + * Supervised by: Alexander Bubeck, email:alexander.bubeck@ipa.fhg.de + * + * Date of creation: June 2010 + * + * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License LGPL as + * published by the Free Software Foundation, either version 3 of the + * License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License LGPL for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License LGPL along with this program. + * If not, see . + * + ****************************************************************/ +#include +ForceTorqueSensorSim::ForceTorqueSensorSim(ros::NodeHandle& nh) : nh_(nh) +{ + std::cout<<"ForceTorqueSensorSim"<lookupTransform(goal_frame, source_frame, ros::Time(0)); + _num_transform_errors = 0; + } + catch (tf2::TransformException ex) + { + if (_num_transform_errors%100 == 0){ + ROS_ERROR("%s", ex.what()); + } + _num_transform_errors++; + return false; + } + + temp_vector_in.vector = wrench.force; + tf2::doTransform(temp_vector_in, temp_vector_out, transform); + transformed->force = temp_vector_out.vector; + + temp_vector_in.vector = wrench.torque; + tf2::doTransform(temp_vector_in, temp_vector_out, transform); + transformed->torque = temp_vector_out.vector; + + return true; +} From 9a2219ef3126fd077810e86290bc25da1650b666 Mon Sep 17 00:00:00 2001 From: Andreea Tulbure Date: Thu, 3 Aug 2017 11:53:19 +0200 Subject: [PATCH 07/53] changes for simulation.need to be cleaned --- CMakeLists.txt | 5 ++- description/urdf/ftm115.gazebo.xacro | 18 ++++++++ description/urdf/ftm115.transmission.xacro | 18 ++++++++ description/urdf/ftm115.urdf.xacro | 19 ++++++--- .../force_torque_sensor_handle_sim.h | 16 +++++++ .../force_torque_sensor_sim.h | 24 ++++++++--- ros/src/force_torque_sensor_handle_sim.cpp | 20 +++++++++ ros/src/force_torque_sensor_sim.cpp | 42 +++++++++++++++---- 8 files changed, 140 insertions(+), 22 deletions(-) create mode 100644 description/urdf/ftm115.gazebo.xacro create mode 100644 description/urdf/ftm115.transmission.xacro create mode 100644 ros/include/ati_force_torque/force_torque_sensor_handle_sim.h create mode 100644 ros/src/force_torque_sensor_handle_sim.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 8f18671..38f059b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -16,6 +16,7 @@ find_package(catkin REQUIRED COMPONENTS iirob_filters rosparam_handler dynamic_reconfigure + schunk_description ) find_package(Eigen3) @@ -66,7 +67,7 @@ generate_messages( ################################### catkin_package( INCLUDE_DIRS common/include ros/include - CATKIN_DEPENDS roscpp message_runtime std_msgs std_srvs cob_generic_can geometry_msgs tf2 tf2_ros tf2_geometry_msgs iirob_filters + CATKIN_DEPENDS roscpp message_runtime std_msgs std_srvs cob_generic_can geometry_msgs tf2 tf2_ros tf2_geometry_msgs iirob_filters schunk_description DEPENDS Eigen LIBRARIES ${PROJECT_NAME} ) @@ -83,7 +84,7 @@ include_directories( ${Eigen_INCLUDE_DIRS} ) -add_library(${PROJECT_NAME} common/src/ForceTorqueCtrl.cpp ros/src/force_torque_sensor.cpp ros/src/force_torque_sensor_sim.cpp ros/src/force_torque_sensor_handle.cpp) +add_library(${PROJECT_NAME} common/src/ForceTorqueCtrl.cpp ros/src/force_torque_sensor.cpp ros/src/force_torque_sensor_sim.cpp ros/src/force_torque_sensor_handle.cpp ros/src/force_torque_sensor_handle_sim.cpp) add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_gencfg) # For dynamic_reconfigure add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_generate_messages_cpp) add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_genparam) # For rosparam_handler diff --git a/description/urdf/ftm115.gazebo.xacro b/description/urdf/ftm115.gazebo.xacro new file mode 100644 index 0000000..bd9fbb6 --- /dev/null +++ b/description/urdf/ftm115.gazebo.xacro @@ -0,0 +1,18 @@ + + + + + + true + false + + + + + + true + false + + + + diff --git a/description/urdf/ftm115.transmission.xacro b/description/urdf/ftm115.transmission.xacro new file mode 100644 index 0000000..3c778ba --- /dev/null +++ b/description/urdf/ftm115.transmission.xacro @@ -0,0 +1,18 @@ + + + + + + + + transmission_interface/SimpleTransmission + + ForceTorqueSensorInterface + + + ForceTorqueSensorInterface + + + + + diff --git a/description/urdf/ftm115.urdf.xacro b/description/urdf/ftm115.urdf.xacro index d8cd1ad..7550dcd 100644 --- a/description/urdf/ftm115.urdf.xacro +++ b/description/urdf/ftm115.urdf.xacro @@ -1,10 +1,14 @@ + + + - + + @@ -12,7 +16,7 @@ - + @@ -26,8 +30,8 @@ - - + + @@ -36,7 +40,7 @@ - + + + + diff --git a/ros/include/ati_force_torque/force_torque_sensor_handle_sim.h b/ros/include/ati_force_torque/force_torque_sensor_handle_sim.h new file mode 100644 index 0000000..627cb58 --- /dev/null +++ b/ros/include/ati_force_torque/force_torque_sensor_handle_sim.h @@ -0,0 +1,16 @@ +#include +#include + +class ForceTorqueSensorHandleSim : public ForceTorqueSensorSim, public hardware_interface::ForceTorqueSensorHandle +{ +public: + + ForceTorqueSensorHandleSim(ros::NodeHandle &nh, std::string sensor_name, std::string output_frame); + +private: + void updateFTData(const ros::TimerEvent &event); + + // Arrays for hardware_interface + double interface_force_[3]; + double interface_torque_[3]; +}; diff --git a/ros/include/ati_force_torque/force_torque_sensor_sim.h b/ros/include/ati_force_torque/force_torque_sensor_sim.h index 5dc9f2e..b48e4e6 100644 --- a/ros/include/ati_force_torque/force_torque_sensor_sim.h +++ b/ros/include/ati_force_torque/force_torque_sensor_sim.h @@ -58,13 +58,19 @@ typedef unsigned char uint8_t; #include #include #include +#include #include #include #include +#include #include #include #include +#include +#include +#include +#include #include #include @@ -72,7 +78,7 @@ typedef unsigned char uint8_t; #define PI 3.14159265 -class ForceTorqueSensorSim +class ForceTorqueSensorSim { public: ForceTorqueSensorSim(ros::NodeHandle &nh); @@ -83,19 +89,25 @@ class ForceTorqueSensorSim std::string transform_frame_; std::string sensor_frame_; - void pullFTData(const geometry_msgs::WrenchStamped::ConstPtr& joystick_input); - + void pullFTData(const geometry_msgs::Twist::ConstPtr& msg); // Arrays for dumping FT-Data - geometry_msgs::WrenchStamped threshold_filtered_force, transformed_data, joystick_data; - + geometry_msgs::WrenchStamped threshold_filtered_force, transformed_data, joystick_data;; + //geometry_msgs::Twist private: + virtual void updateFTData(const ros::TimerEvent &event) = 0; bool transform_wrench(std::string goal_frame, std::string source_frame, geometry_msgs::Wrench wrench, geometry_msgs::Wrench *transformed); - + ati_force_torque::PublishConfigurationParameters pub_params_; + ati_force_torque::NodeConfigurationParameters node_params_; ros::Subscriber force_input_subscriber; uint _num_transform_errors; tf2_ros::Buffer *p_tfBuffer; tf2_ros::TransformListener *p_tfListener; ros::NodeHandle nh_; + ros::Publisher transformed_data_pub_,sensor_data_pub_; + ros::Timer ftUpdateTimer_; + bool is_pub_transformed_data_ =false; + bool is_pub_sensor_data_=false; + double nodeUpdateFreq; }; diff --git a/ros/src/force_torque_sensor_handle_sim.cpp b/ros/src/force_torque_sensor_handle_sim.cpp new file mode 100644 index 0000000..021c355 --- /dev/null +++ b/ros/src/force_torque_sensor_handle_sim.cpp @@ -0,0 +1,20 @@ +#include + +ForceTorqueSensorHandleSim::ForceTorqueSensorHandleSim(ros::NodeHandle& nh, std::string sensor_name, std::string output_frame) : + ForceTorqueSensorSim(nh), hardware_interface::ForceTorqueSensorHandle(sensor_name, output_frame, interface_force_, interface_torque_) +{ + std::cout<<"FTSH Sim init"< -ForceTorqueSensorSim::ForceTorqueSensorSim(ros::NodeHandle& nh) : nh_(nh) +ForceTorqueSensorSim::ForceTorqueSensorSim(ros::NodeHandle& nh) : nh_(nh), pub_params_{nh}, node_params_{nh} { std::cout<<"ForceTorqueSensorSim"<("sensor_data", 1); + } + is_pub_transformed_data_ = pub_params_.transformed_data; + if(is_pub_transformed_data_){ + transformed_data_pub_ = nh_.advertise("transformed_data", 1); + } + ftUpdateTimer_ = nh.createTimer(ros::Rate(nodeUpdateFreq), &ForceTorqueSensorSim::updateFTData, this, false, false); + p_tfBuffer = new tf2_ros::Buffer(); p_tfListener = new tf2_ros::TransformListener(*p_tfBuffer, true); - - + init_sensor(); + ftUpdateTimer_.start(); } void ForceTorqueSensorSim::init_sensor() { - force_input_subscriber = nh_.subscribe("command", 1, &ForceTorqueSensorSim::pullFTData, this); + force_input_subscriber = nh_.subscribe("twist_controller/command", 1, &ForceTorqueSensorSim::pullFTData, this); + std::cout<<"namespace"<linear.x; + joystick_data.wrench.force.y = msg->linear.y; + joystick_data.wrench.force.z = 0; + joystick_data.wrench.torque.z = msg->angular.z; + joystick_data.wrench.torque.x = 0; + joystick_data.wrench.torque.y = 0; + transformed_data.header.stamp = ros::Time::now(); transformed_data.header.frame_id = transform_frame_; transform_wrench(transform_frame_, sensor_frame_, joystick_data.wrench, &transformed_data.wrench); threshold_filtered_force = transformed_data; + if(is_pub_sensor_data_) + sensor_data_pub_.publish(joystick_data); + if(is_pub_transformed_data_) + transformed_data_pub_.publish(threshold_filtered_force); } - bool ForceTorqueSensorSim::transform_wrench(std::string goal_frame, std::string source_frame, geometry_msgs::Wrench wrench, geometry_msgs::Wrench *transformed) { geometry_msgs::TransformStamped transform; From 90c60222519e28449b523596bfd0eb15359bd2b9 Mon Sep 17 00:00:00 2001 From: Andreea Tulbure Date: Thu, 3 Aug 2017 11:55:52 +0200 Subject: [PATCH 08/53] changes in fthandle_sim --- ros/src/force_torque_sensor_handle_sim.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/ros/src/force_torque_sensor_handle_sim.cpp b/ros/src/force_torque_sensor_handle_sim.cpp index 021c355..c790af5 100644 --- a/ros/src/force_torque_sensor_handle_sim.cpp +++ b/ros/src/force_torque_sensor_handle_sim.cpp @@ -9,9 +9,9 @@ ForceTorqueSensorHandleSim::ForceTorqueSensorHandleSim(ros::NodeHandle& nh, std: void ForceTorqueSensorHandleSim::updateFTData(const ros::TimerEvent& event) { - interface_force_[0] = threshold_filtered_force.wrench.force.x; - interface_force_[1] = threshold_filtered_force.wrench.force.y; - interface_force_[2] = threshold_filtered_force.wrench.force.z; + interface_force_[0] = threshold_filtered_force.wrench.force.x*40; + interface_force_[1] = threshold_filtered_force.wrench.force.y*55; + interface_force_[2] = threshold_filtered_force.wrench.force.z*10; interface_torque_[0] = threshold_filtered_force.wrench.torque.x; interface_torque_[1] = threshold_filtered_force.wrench.torque.y; From c2f67de3cdfa3fcd426066a501872a52f846884d Mon Sep 17 00:00:00 2001 From: Andreea Tulbure Date: Thu, 3 Aug 2017 12:06:31 +0200 Subject: [PATCH 09/53] deleted prints --- ros/src/force_torque_sensor_handle_sim.cpp | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/ros/src/force_torque_sensor_handle_sim.cpp b/ros/src/force_torque_sensor_handle_sim.cpp index c790af5..08c6ade 100644 --- a/ros/src/force_torque_sensor_handle_sim.cpp +++ b/ros/src/force_torque_sensor_handle_sim.cpp @@ -3,7 +3,6 @@ ForceTorqueSensorHandleSim::ForceTorqueSensorHandleSim(ros::NodeHandle& nh, std::string sensor_name, std::string output_frame) : ForceTorqueSensorSim(nh), hardware_interface::ForceTorqueSensorHandle(sensor_name, output_frame, interface_force_, interface_torque_) { - std::cout<<"FTSH Sim init"< Date: Thu, 3 Aug 2017 12:55:16 +0200 Subject: [PATCH 10/53] merged master branch with raw_with_fts --- description/urdf/ftm115.urdf.xacro | 31 +++++++++++------------------- 1 file changed, 11 insertions(+), 20 deletions(-) diff --git a/description/urdf/ftm115.urdf.xacro b/description/urdf/ftm115.urdf.xacro index 9bd1041..6228c6a 100644 --- a/description/urdf/ftm115.urdf.xacro +++ b/description/urdf/ftm115.urdf.xacro @@ -14,22 +14,17 @@ - - - - - + + +======= +--> - - - + @@ -42,7 +37,8 @@ +======= +--> @@ -50,25 +46,20 @@ + + +--> From f44d9cb90bc0133bb46a35e411955a7103c8e8d1 Mon Sep 17 00:00:00 2001 From: Andreea Tulbure Date: Mon, 7 Aug 2017 11:42:41 +0200 Subject: [PATCH 11/53] small changes in urdf --- description/urdf/ftm115.urdf.xacro | 16 +++------------- 1 file changed, 3 insertions(+), 13 deletions(-) diff --git a/description/urdf/ftm115.urdf.xacro b/description/urdf/ftm115.urdf.xacro index 7550dcd..4a8e495 100644 --- a/description/urdf/ftm115.urdf.xacro +++ b/description/urdf/ftm115.urdf.xacro @@ -6,9 +6,8 @@ - + - @@ -16,7 +15,7 @@ - + @@ -31,7 +30,7 @@ - + @@ -40,15 +39,6 @@ - From bad5adcbe1371d94a7f9df4e715fbb2f9287c784 Mon Sep 17 00:00:00 2001 From: Andreea Tulbure Date: Tue, 8 Aug 2017 10:21:47 +0200 Subject: [PATCH 12/53] compatibility changes for iirob_fitlers --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 3858bc1..71ff0d5 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -91,7 +91,7 @@ add_library(${PROJECT_NAME} common/src/ForceTorqueCtrl.cpp ros/src/force_torque_ add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_gencfg) # For dynamic_reconfigure add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_generate_messages_cpp) add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_genparam) # For rosparam_handler -target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${Eigen_LIBRARIES} gravity_compensation low_pass_filter moving_mean_filter threshold_filter) +target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${Eigen_LIBRARIES} iirob_filters_gravity_compensation iirob_filters_low_pass_filter iirob_filters_moving_mean_filter iirob_filters_threshold_filter) add_executable(${PROJECT_NAME}_node ros/src/force_torque_sensor_node.cpp) add_dependencies(${PROJECT_NAME}_node ${PROJECT_NAME}_generate_messages_cpp) From fe397dde4b1a54a8e56106ecd121dda4f8549ffb Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Tue, 15 Aug 2017 19:33:39 +0200 Subject: [PATCH 13/53] Use c++11 and update .travis --- .travis.rosinstall | 6 +++++- .travis.yml | 5 ++--- CMakeLists.txt | 26 ++++++++++++-------------- 3 files changed, 19 insertions(+), 18 deletions(-) diff --git a/.travis.rosinstall b/.travis.rosinstall index 4635ec8..1f4079d 100644 --- a/.travis.rosinstall +++ b/.travis.rosinstall @@ -1,4 +1,8 @@ - git: uri: 'https://github.com/iirob/iirob_filters.git' local-name: iirob_filters - version: indigo-devel + version: kinetic-devel +- git: + uri: 'https://github.com/iirob/rosparam_handler.git' + local-name: rosparam_handler + version: ns_get_set diff --git a/.travis.yml b/.travis.yml index b6b4226..3008e2d 100644 --- a/.travis.yml +++ b/.travis.yml @@ -12,10 +12,9 @@ env: - UPSTREAM_WORKSPACE=file - VERBOSE_OUTPUT=true matrix: - - ROS_DISTRO="indigo" ROS_REPOSITORY_PATH=http://packages.ros.org/ros/ubuntu -# - ROS_DISTRO="kinetic" ROS_REPOSITORY_PATH=http://packages.ros.org/ros/ubuntu +# - ROS_DISTRO="indigo" ROS_REPOSITORY_PATH=http://packages.ros.org/ros/ubuntu + - ROS_DISTRO="kinetic" ROS_REPOSITORY_PATH=http://packages.ros.org/ros/ubuntu install: - git clone https://github.com/ros-industrial/industrial_ci.git .ci_config script: - source .ci_config/travis.sh -# - source ./travis.sh # Enable this when you have a package-local script diff --git a/CMakeLists.txt b/CMakeLists.txt index 71ff0d5..ef50350 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -4,11 +4,13 @@ project(ati_force_torque) find_package(catkin REQUIRED COMPONENTS cmake_modules cob_generic_can + dynamic_reconfigure geometry_msgs iirob_filters message_generation roscpp roslaunch + rosparam_handler rospy std_msgs std_srvs @@ -17,8 +19,7 @@ find_package(catkin REQUIRED COMPONENTS tf2_ros trajectory_msgs visualization_msgs - rosparam_handler - dynamic_reconfigure + schunk_description ) @@ -33,13 +34,9 @@ else() endif() include(CheckCXXCompilerFlag) -CHECK_CXX_COMPILER_FLAG("-std=c++14" Cpp14CompilerFlag) -if (Cpp14CompilerFlag) - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14") -elseif(CMAKE_CXX_COMPILER_ID STREQUAL "MSVC" AND NOT CMAKE_CXX_COMPILER_VERSION VERSION_LESS "17") - #no additional flag is required -else() - message(FATAL_ERROR "Compiler does not have c++14 support. Use at least g++4.9 or Visual Studio 2013 and newer.") +CHECK_CXX_COMPILER_FLAG("-std=c++11" Cpp11CompilerFlag) +if (Cpp11CompilerFlag) + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") endif() generate_ros_parameter_files( @@ -51,7 +48,6 @@ generate_ros_parameter_files( cfg/Calibration.params ) - add_service_files( FILES CalculateAverageMasurement.srv @@ -70,7 +66,7 @@ generate_messages( ################################### catkin_package( INCLUDE_DIRS common/include ros/include - CATKIN_DEPENDS roscpp message_runtime std_msgs std_srvs cob_generic_can geometry_msgs tf2 tf2_ros tf2_geometry_msgs iirob_filters schunk_description trajectory_msgs rospy + CATKIN_DEPENDS roscpp message_runtime std_msgs std_srvs cob_generic_can geometry_msgs iirob_filters rospy tf2 tf2_ros tf2_geometry_msgs trajectory_msgs schunk_description DEPENDS Eigen LIBRARIES ${PROJECT_NAME} ) @@ -88,9 +84,11 @@ include_directories( ) add_library(${PROJECT_NAME} common/src/ForceTorqueCtrl.cpp ros/src/force_torque_sensor.cpp ros/src/force_torque_sensor_sim.cpp ros/src/force_torque_sensor_handle.cpp ros/src/force_torque_sensor_handle_sim.cpp) -add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_gencfg) # For dynamic_reconfigure -add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_generate_messages_cpp) -add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_genparam) # For rosparam_handler +add_dependencies(${PROJECT_NAME} + ${PROJECT_NAME}_gencfg # For dynamic_reconfigure + ${PROJECT_NAME}_generate_messages_cpp + ${PROJECT_NAME}_genparam # For rosparam_handler + ) target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${Eigen_LIBRARIES} iirob_filters_gravity_compensation iirob_filters_low_pass_filter iirob_filters_moving_mean_filter iirob_filters_threshold_filter) add_executable(${PROJECT_NAME}_node ros/src/force_torque_sensor_node.cpp) From 3fd59a85905f35ccd87cc4375701d06d58a6869b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Tue, 15 Aug 2017 19:39:31 +0200 Subject: [PATCH 14/53] Update .travis.rosinstall --- .travis.rosinstall | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.travis.rosinstall b/.travis.rosinstall index 681af93..3758a34 100644 --- a/.travis.rosinstall +++ b/.travis.rosinstall @@ -5,7 +5,7 @@ - git: uri: 'https://github.com/iirob/rosparam_handler.git' local-name: rosparam_handler - version: ns_get_set + version: ns_set_get - git: uri: 'https://github.com/ipa320/cob_driver.git' local-name: cob_driver From 6d780dec6de7f8ff608152ee992a6d9dcc71d230 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Wed, 16 Aug 2017 09:20:24 +0200 Subject: [PATCH 15/53] Updated compile files and some Sim files headers --- CMakeLists.txt | 20 +++++++++---------- .../force_torque_sensor_sim.h | 15 -------------- ros/src/force_torque_sensor_sim.cpp | 16 +++------------ 3 files changed, 13 insertions(+), 38 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index ef50350..043de98 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -66,7 +66,7 @@ generate_messages( ################################### catkin_package( INCLUDE_DIRS common/include ros/include - CATKIN_DEPENDS roscpp message_runtime std_msgs std_srvs cob_generic_can geometry_msgs iirob_filters rospy tf2 tf2_ros tf2_geometry_msgs trajectory_msgs schunk_description + 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 schunk_description DEPENDS Eigen LIBRARIES ${PROJECT_NAME} ) @@ -76,7 +76,6 @@ catkin_package( ########### include_directories( - include common/include ros/include ${catkin_INCLUDE_DIRS} @@ -89,7 +88,7 @@ add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_generate_messages_cpp ${PROJECT_NAME}_genparam # For rosparam_handler ) -target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${Eigen_LIBRARIES} iirob_filters_gravity_compensation iirob_filters_low_pass_filter iirob_filters_moving_mean_filter iirob_filters_threshold_filter) +target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${Eigen_LIBRARIES}) add_executable(${PROJECT_NAME}_node ros/src/force_torque_sensor_node.cpp) add_dependencies(${PROJECT_NAME}_node ${PROJECT_NAME}_generate_messages_cpp) @@ -99,16 +98,9 @@ add_executable(${PROJECT_NAME}_config ros/src/force_torque_sensor_config.cpp) target_link_libraries(${PROJECT_NAME}_config ${PROJECT_NAME} ${catkin_LIBRARIES} ${Eigen_LIBRARIES}) add_dependencies(${PROJECT_NAME}_config ${PROJECT_NAME} ${catkin_LIBRARIES} ${Eigen_LIBRARIES} ${PROJECT_NAME}_generate_messages_cpp) -############# -## Tests ## -############# - -roslaunch_add_file_check(ros/launch) - ############# ## Install ## ############# - install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node ${PROJECT_NAME}_config ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} @@ -117,8 +109,16 @@ install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node ${PROJECT_NAME}_config install(DIRECTORY common/include ros/include DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" + PATTERN ".svn" EXCLUDE ) install(DIRECTORY ros/launch config description DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} ) + +############# +## Tests ## +############# + +roslaunch_add_file_check(ros/launch) diff --git a/ros/include/ati_force_torque/force_torque_sensor_sim.h b/ros/include/ati_force_torque/force_torque_sensor_sim.h index b48e4e6..90a126c 100644 --- a/ros/include/ati_force_torque/force_torque_sensor_sim.h +++ b/ros/include/ati_force_torque/force_torque_sensor_sim.h @@ -13,18 +13,6 @@ * * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ * - * Copyright (c) 2010 - * - * Fraunhofer Institute for Manufacturing Engineering - * and Automation (IPA) - * - * Author: Alexander Bubeck, email:alexander.bubeck@ipa.fhg.de - * Supervised by: Alexander Bubeck, email:alexander.bubeck@ipa.fhg.de - * - * Date of creation: June 2010 - * - * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ - * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * @@ -67,15 +55,12 @@ typedef unsigned char uint8_t; #include #include #include -#include #include #include -#include #include #include - #define PI 3.14159265 class ForceTorqueSensorSim diff --git a/ros/src/force_torque_sensor_sim.cpp b/ros/src/force_torque_sensor_sim.cpp index c353f17..2f462b5 100644 --- a/ros/src/force_torque_sensor_sim.cpp +++ b/ros/src/force_torque_sensor_sim.cpp @@ -8,19 +8,7 @@ * Maintainers: Denis Štogl, email: denis.stogl@kit.edu * Andreea Tulbure * - * Date of update: 2014-2016 - * - * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ - * - * Copyright (c) 2010 - * - * Fraunhofer Institute for Manufacturing Engineering - * and Automation (IPA) - * - * Author: Alexander Bubeck, email:alexander.bubeck@ipa.fhg.de - * Supervised by: Alexander Bubeck, email:alexander.bubeck@ipa.fhg.de - * - * Date of creation: June 2010 + * Date of update: 2014-2017 * * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ * @@ -51,7 +39,9 @@ * If not, see . * ****************************************************************/ + #include + ForceTorqueSensorSim::ForceTorqueSensorSim(ros::NodeHandle& nh) : nh_(nh), pub_params_{nh}, node_params_{nh} { std::cout<<"ForceTorqueSensorSim"< Date: Wed, 16 Aug 2017 14:28:19 +0200 Subject: [PATCH 16/53] made some changes --- CMakeLists.txt | 2 -- cfg/Calibration.params | 15 +++++++-------- cfg/CanConfiguration.params | 3 +-- cfg/CoordinateSystemCalibration.params | 1 - cfg/FTSConfiguration.params | 3 +-- cfg/NodeConfiguration.params | 6 ++++-- cfg/PublishConfiguration.params | 3 +-- config/sensor_configuration.yaml | 12 +++--------- .../ati_force_torque/force_torque_sensor.h | 13 ++++++------- .../ati_force_torque/force_torque_sensor_sim.h | 15 +++++++-------- ros/src/force_torque_sensor_handle_sim.cpp | 6 +++--- ros/src/force_torque_sensor_sim.cpp | 5 +---- 12 files changed, 34 insertions(+), 50 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 043de98..eced380 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -19,8 +19,6 @@ find_package(catkin REQUIRED COMPONENTS tf2_ros trajectory_msgs visualization_msgs - - schunk_description ) find_package(Eigen3) diff --git a/cfg/Calibration.params b/cfg/Calibration.params index 6e654a3..f74b0e9 100755 --- a/cfg/Calibration.params +++ b/cfg/Calibration.params @@ -5,12 +5,11 @@ gen = ParameterGenerator() gen.add("n_measurements", paramtype="int", description="number of necessary measurements for calibration", default=10, configurable=True) gen.add("T_between_meas", paramtype="double", description="time between two measurements for calibration", default=0.01, configurable=True) gen.add("isStatic", paramtype="bool", description="is static calibration active", default=False, configurable=True) -gen.add("Offset_Force_x", paramtype="double", description="calibration_offset_force_x", default=29.3789054311, configurable=True) -gen.add("Offset_Force_y", paramtype="double", description="calibration_offset_force_y", default=-4.02509469552, configurable=True) -gen.add("Offset_Force_z", paramtype="double", description="calibration_offset_force_z", default=-7.12669364182, configurable=True) -gen.add("Offset_Torque_x", paramtype="double", description="calibration_offset_torque_x", default=-0.30390995786, configurable=True) -gen.add("Offset_Torque_y", paramtype="double", description="calibration_offset_torque_y", default=-0.747661107339, configurable=True) -gen.add("Offset_Torque_z", paramtype="double", description="calibration_offset_torque_z", default=1.11224200823, configurable=True) +gen.add("Offset_Force_x", paramtype="double", description="calibration_offset_force_x", default=0.0, configurable=True) +gen.add("Offset_Force_y", paramtype="double", description="calibration_offset_force_y", default=0.0, configurable=True) +gen.add("Offset_Force_z", paramtype="double", description="calibration_offset_force_z", default=0.0, configurable=True) +gen.add("Offset_Torque_x", paramtype="double", description="calibration_offset_torque_x", default=0.0, configurable=True) +gen.add("Offset_Torque_y", paramtype="double", description="calibration_offset_torque_y", default=0.0, configurable=True) +gen.add("Offset_Torque_z", paramtype="double", description="calibration_offset_torque_z", default=0.0, configurable=True) -#Syntax : Package, Node, Config Name(The final name will be MyDummyConfig) -exit(gen.generate("ati_force_torque", "ForceTorqueSensor", "Calibration")) \ No newline at end of file +exit(gen.generate("ati_force_torque", "ForceTorqueSensor", "Calibration")) diff --git a/cfg/CanConfiguration.params b/cfg/CanConfiguration.params index 7854f99..221fb38 100755 --- a/cfg/CanConfiguration.params +++ b/cfg/CanConfiguration.params @@ -6,5 +6,4 @@ gen.add("type", paramtype="int", description="socket_can", default=-1, configura gen.add("path", paramtype="std::string", description="socket_can", default=" ", configurable=False) gen.add("baudrate", paramtype="int", description="baudrate", default=-1, configurable=False) -#Syntax : Package, Node, Config Name(The final name will be MyDummyConfig) -exit(gen.generate("ati_force_torque", "ForceTorqueSensor", "CanConfiguration")) \ No newline at end of file +exit(gen.generate("ati_force_torque", "ForceTorqueSensor", "CanConfiguration")) diff --git a/cfg/CoordinateSystemCalibration.params b/cfg/CoordinateSystemCalibration.params index 37986a0..1f9190d 100755 --- a/cfg/CoordinateSystemCalibration.params +++ b/cfg/CoordinateSystemCalibration.params @@ -6,5 +6,4 @@ gen.add("n_measurements", paramtype="int", description="number of necessary meas gen.add("T_between_meas", paramtype="int", description="time between two measurements for calibration", default=10000, configurable=True) gen.add("push_direction", paramtype="int", description="push direction", default=0, configurable=False) -#Syntax : Package, Node, Config Name(The final name will be MyDummyConfig) exit(gen.generate("ati_force_torque", "ForceTorqueSensor", "CoordinateSystemCalibration")) diff --git a/cfg/FTSConfiguration.params b/cfg/FTSConfiguration.params index b9f915d..4ad64f7 100755 --- a/cfg/FTSConfiguration.params +++ b/cfg/FTSConfiguration.params @@ -6,5 +6,4 @@ gen.add("base_identifier", paramtype="int", description="identifier", default=0x gen.add("auto_init", paramtype="bool", description="auto init", default=True, configurable=False) gen.add("fts_name", paramtype="std::string", description="name of sensor", default="ATI_45_Mini", configurable=False) -#Syntax : Package, Node, Config Name(The final name will be MyDummyConfig) -exit(gen.generate("ati_force_torque", "ForceTorqueSensor", "FTSConfiguration")) \ No newline at end of file +exit(gen.generate("ati_force_torque", "ForceTorqueSensor", "FTSConfiguration")) diff --git a/cfg/NodeConfiguration.params b/cfg/NodeConfiguration.params index 78ade7a..f38c368 100755 --- a/cfg/NodeConfiguration.params +++ b/cfg/NodeConfiguration.params @@ -6,6 +6,8 @@ gen.add("ft_pub_freq", paramtype="double", description="publish frequency", defa gen.add("ft_pull_freq", paramtype="double", description="sensor daa pull frequency", default=800, configurable=False) gen.add("transform_frame", paramtype="std::string", description="reference coordinate system", default="fts_transform_frame", configurable=False) gen.add("sensor_frame", paramtype="std::string", description="sensor coordinate system", default="fts_reference_link", configurable=False) +gen.add("sim_forceX_param", paramtype="int", description="factor for simulation for x axis simulated force", default="0", configurable=True) +gen.add("sim_forceY_param", paramtype="int", description="factor for simulation for y axis simulated force", default="0", configurable=True) +gen.add("sim_torqueZ_param", paramtype="int", description="factor for simulation for z axis simulated torque", default="0", configurable=True) -#Syntax : Package, Node, Config Name(The final name will be MyDummyConfig) -exit(gen.generate("ati_force_torque", "ForceTorqueSensor", "NodeConfiguration")) \ No newline at end of file +exit(gen.generate("ati_force_torque", "ForceTorqueSensor", "NodeConfiguration")) diff --git a/cfg/PublishConfiguration.params b/cfg/PublishConfiguration.params index 0a9a3a6..c204c50 100755 --- a/cfg/PublishConfiguration.params +++ b/cfg/PublishConfiguration.params @@ -9,5 +9,4 @@ gen.add("transformed_data", paramtype="bool", description="publish transformed d 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) -#Syntax : Package, Node, Config Name(The final name will be MyDummyConfig) -exit(gen.generate("ati_force_torque", "ForceTorqueSensor", "PublishConfiguration")) \ No newline at end of file +exit(gen.generate("ati_force_torque", "ForceTorqueSensor", "PublishConfiguration")) diff --git a/config/sensor_configuration.yaml b/config/sensor_configuration.yaml index 1d90af3..9dc3e86 100644 --- a/config/sensor_configuration.yaml +++ b/config/sensor_configuration.yaml @@ -1,19 +1,14 @@ Node: + sim: false ft_pub_freq: 200 ft_pull_freq: 800 sensor_frame: "fts_reference_link" - transform_frame: "fts_transform_frame" + transform_frame: "fts_base_link" 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 - Offset_Force_x: 29.3789054311 - Offset_Force_y: -4.02509469552 - Offset_Force_z: -7.12669364182 - Offset_Torque_x: -0.30390995786 - Offset_Torque_y: -0.747661107339 - Offset_Torque_z: 1.11224200823 ThresholdFilter: linear_threshold: 2.5 @@ -65,8 +60,7 @@ GravityCompensation: #y: 0 #z: 0.027346102 # m #force: 7.9459955 # f_G - sensor_frame: "fts_reference_link" - world_frame: "fts_transform_frame" + world_frame: "world" Publish: sensor_data: true diff --git a/ros/include/ati_force_torque/force_torque_sensor.h b/ros/include/ati_force_torque/force_torque_sensor.h index a9aa4bf..88552bb 100644 --- a/ros/include/ati_force_torque/force_torque_sensor.h +++ b/ros/include/ati_force_torque/force_torque_sensor.h @@ -146,12 +146,12 @@ class ForceTorqueSensor 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_; - bool is_pub_gravity_compensated_=false; - bool is_pub_threshold_filtered_=false; - bool is_pub_transformed_data_ =false; - bool is_pub_sensor_data_=false; - bool is_pub_low_pass_=false; - bool is_pub_moving_mean_=false; + bool is_pub_gravity_compensated_ = false; + bool is_pub_threshold_filtered_ = false; + bool is_pub_transformed_data_ = false; + bool is_pub_sensor_data_ = false; + bool is_pub_low_pass_ = false; + bool is_pub_moving_mean_ = false; uint _num_transform_errors; @@ -176,7 +176,6 @@ class ForceTorqueSensor ros::ServiceServer srvServer_ReCalibrate; ros::Timer ftUpdateTimer_, ftPullTimer_; -// ros::Timer timer_; tf2_ros::TransformListener *p_tfListener; tf2::Transform transform_ee_base; diff --git a/ros/include/ati_force_torque/force_torque_sensor_sim.h b/ros/include/ati_force_torque/force_torque_sensor_sim.h index 90a126c..72e9b53 100644 --- a/ros/include/ati_force_torque/force_torque_sensor_sim.h +++ b/ros/include/ati_force_torque/force_torque_sensor_sim.h @@ -5,11 +5,9 @@ * Intelligent Process Control and Robotics (IPR), * Karlsruhe Institute of Technology * - * Maintainers: Denis Štogl, email: denis.stogl@kit.edu - * Florian Heller - * Vanessa Streuer + * Maintainers: Andreea Tulbure, email: andreea.tulbure@student.kit.edu * - * Date of update: 2014-2016 + * Date of update: 2016-2017 * * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ * @@ -73,17 +71,18 @@ class ForceTorqueSensorSim std::string transform_frame_; std::string sensor_frame_; - + ati_force_torque::NodeConfigurationParameters node_params_; + void pullFTData(const geometry_msgs::Twist::ConstPtr& msg); // Arrays for dumping FT-Data geometry_msgs::WrenchStamped threshold_filtered_force, transformed_data, joystick_data;; - //geometry_msgs::Twist + private: virtual void updateFTData(const ros::TimerEvent &event) = 0; bool transform_wrench(std::string goal_frame, std::string source_frame, geometry_msgs::Wrench wrench, geometry_msgs::Wrench *transformed); ati_force_torque::PublishConfigurationParameters pub_params_; - ati_force_torque::NodeConfigurationParameters node_params_; + ros::Subscriber force_input_subscriber; uint _num_transform_errors; tf2_ros::Buffer *p_tfBuffer; @@ -93,6 +92,6 @@ class ForceTorqueSensorSim ros::Timer ftUpdateTimer_; bool is_pub_transformed_data_ =false; bool is_pub_sensor_data_=false; - double nodeUpdateFreq; + }; diff --git a/ros/src/force_torque_sensor_handle_sim.cpp b/ros/src/force_torque_sensor_handle_sim.cpp index 08c6ade..1a083ba 100644 --- a/ros/src/force_torque_sensor_handle_sim.cpp +++ b/ros/src/force_torque_sensor_handle_sim.cpp @@ -8,11 +8,11 @@ ForceTorqueSensorHandleSim::ForceTorqueSensorHandleSim(ros::NodeHandle& nh, std: void ForceTorqueSensorHandleSim::updateFTData(const ros::TimerEvent& event) { - interface_force_[0] = threshold_filtered_force.wrench.force.x*40; - interface_force_[1] = threshold_filtered_force.wrench.force.y*55; + interface_force_[0] = threshold_filtered_force.wrench.force.x*node_params_.sim_forceX_param; + interface_force_[1] = threshold_filtered_force.wrench.force.y*node_params_.sim_forceY_param;; interface_force_[2] = threshold_filtered_force.wrench.force.z; interface_torque_[0] = threshold_filtered_force.wrench.torque.x; interface_torque_[1] = threshold_filtered_force.wrench.torque.y; - interface_torque_[2] = threshold_filtered_force.wrench.torque.z*10; + interface_torque_[2] = threshold_filtered_force.wrench.torque.z*node_params_.sim_torqueZ_param;; } diff --git a/ros/src/force_torque_sensor_sim.cpp b/ros/src/force_torque_sensor_sim.cpp index 2f462b5..2984526 100644 --- a/ros/src/force_torque_sensor_sim.cpp +++ b/ros/src/force_torque_sensor_sim.cpp @@ -51,7 +51,6 @@ ForceTorqueSensorSim::ForceTorqueSensorSim(ros::NodeHandle& nh) : nh_(nh), pub_p pub_params_.fromParamServer(); node_params_.fromParamServer(); - nodeUpdateFreq=node_params_.ft_pub_freq; //Wrench Publisher is_pub_sensor_data_=pub_params_.sensor_data; if(is_pub_sensor_data_){ @@ -61,7 +60,7 @@ ForceTorqueSensorSim::ForceTorqueSensorSim(ros::NodeHandle& nh) : nh_(nh), pub_p if(is_pub_transformed_data_){ transformed_data_pub_ = nh_.advertise("transformed_data", 1); } - ftUpdateTimer_ = nh.createTimer(ros::Rate(nodeUpdateFreq), &ForceTorqueSensorSim::updateFTData, this, false, false); + ftUpdateTimer_ = nh.createTimer(ros::Rate(node_params_.ft_pull_freq), &ForceTorqueSensorSim::updateFTData, this, false, false); p_tfBuffer = new tf2_ros::Buffer(); p_tfListener = new tf2_ros::TransformListener(*p_tfBuffer, true); @@ -70,8 +69,6 @@ ForceTorqueSensorSim::ForceTorqueSensorSim(ros::NodeHandle& nh) : nh_(nh), pub_p } void ForceTorqueSensorSim::init_sensor() { force_input_subscriber = nh_.subscribe("twist_controller/command", 1, &ForceTorqueSensorSim::pullFTData, this); - std::cout<<"namespace"< Date: Wed, 16 Aug 2017 14:33:28 +0200 Subject: [PATCH 17/53] some more changes for PR --- config/sensor_offset.yaml | 2 ++ description/urdf/ftm115.urdf.xacro | 4 +++- 2 files changed, 5 insertions(+), 1 deletion(-) create mode 100644 config/sensor_offset.yaml diff --git a/config/sensor_offset.yaml b/config/sensor_offset.yaml new file mode 100644 index 0000000..85325ad --- /dev/null +++ b/config/sensor_offset.yaml @@ -0,0 +1,2 @@ +force: {x: -19.104102666356425, y: -1.5473841984049037, z: 0.9332121238751951} +torque: {x: -0.24642145895240927, y: 0.0373995095506341, z: 0.060428603494093} diff --git a/description/urdf/ftm115.urdf.xacro b/description/urdf/ftm115.urdf.xacro index 71d388a..3c12f06 100644 --- a/description/urdf/ftm115.urdf.xacro +++ b/description/urdf/ftm115.urdf.xacro @@ -19,7 +19,9 @@ - + + + From acc18f85f987f3e951dde9158d0f05a686d9cad6 Mon Sep 17 00:00:00 2001 From: Andreea Tulbure Date: Wed, 16 Aug 2017 14:57:43 +0200 Subject: [PATCH 18/53] changes for PR --- description/urdf/ftm115.urdf.xacro | 28 +++++++++++-------- .../{ftm115.gazebo.xacro => fts.gazebo.xacro} | 7 +++++ ...nsmission.xacro => fts.transmission.xacro} | 0 3 files changed, 23 insertions(+), 12 deletions(-) rename description/urdf/{ftm115.gazebo.xacro => fts.gazebo.xacro} (69%) rename description/urdf/{ftm115.transmission.xacro => fts.transmission.xacro} (100%) diff --git a/description/urdf/ftm115.urdf.xacro b/description/urdf/ftm115.urdf.xacro index 3c12f06..70e8f45 100644 --- a/description/urdf/ftm115.urdf.xacro +++ b/description/urdf/ftm115.urdf.xacro @@ -1,9 +1,6 @@ - - - @@ -13,14 +10,18 @@ - + + + + + - + - + @@ -31,19 +32,22 @@ - - + + - - - - + + + + + + + diff --git a/description/urdf/ftm115.gazebo.xacro b/description/urdf/fts.gazebo.xacro similarity index 69% rename from description/urdf/ftm115.gazebo.xacro rename to description/urdf/fts.gazebo.xacro index bd9fbb6..a6608b7 100644 --- a/description/urdf/ftm115.gazebo.xacro +++ b/description/urdf/fts.gazebo.xacro @@ -14,5 +14,12 @@ false + + + + true + false + + diff --git a/description/urdf/ftm115.transmission.xacro b/description/urdf/fts.transmission.xacro similarity index 100% rename from description/urdf/ftm115.transmission.xacro rename to description/urdf/fts.transmission.xacro From 08fa91ec2db74653cd054f06d0a062c349840c5e Mon Sep 17 00:00:00 2001 From: Andreea Tulbure Date: Thu, 17 Aug 2017 10:29:51 +0200 Subject: [PATCH 19/53] changed in fts.gazebo --- description/urdf/fts.gazebo.xacro | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/description/urdf/fts.gazebo.xacro b/description/urdf/fts.gazebo.xacro index a6608b7..2651fcc 100644 --- a/description/urdf/fts.gazebo.xacro +++ b/description/urdf/fts.gazebo.xacro @@ -14,9 +14,9 @@ false - - - + + + true false From 70951817bf30b399f34482752a74017a40928645 Mon Sep 17 00:00:00 2001 From: Andreea Tulbure Date: Fri, 18 Aug 2017 12:51:01 +0200 Subject: [PATCH 20/53] changes for PR --- package.xml | 1 - 1 file changed, 1 deletion(-) diff --git a/package.xml b/package.xml index 1400203..d069432 100644 --- a/package.xml +++ b/package.xml @@ -31,7 +31,6 @@ tf2_geometry_msgs trajectory_msgs visualization_msgs - schunk_description rosparam_handler dynamic_reconfigure From 40c9985a09b56c76fd1060c540bb22b071a2e8b0 Mon Sep 17 00:00:00 2001 From: Andreea Tulbure Date: Mon, 21 Aug 2017 09:03:51 +0200 Subject: [PATCH 21/53] changes for PR and added sim parameter --- CMakeLists.txt | 2 +- cfg/NodeConfiguration.params | 1 + config/sensor_configuration_sim.yaml | 14 +++++ description/urdf/macros/mini58_sim.urdf.xacro | 63 +++++++++++++++++++ ros/launch/driver.launch | 4 +- ros/launch/sensor_parameters.launch | 4 +- 6 files changed, 85 insertions(+), 3 deletions(-) create mode 100644 config/sensor_configuration_sim.yaml create mode 100644 description/urdf/macros/mini58_sim.urdf.xacro diff --git a/CMakeLists.txt b/CMakeLists.txt index eced380..20dd3c2 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -64,7 +64,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 schunk_description + 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 DEPENDS Eigen LIBRARIES ${PROJECT_NAME} ) diff --git a/cfg/NodeConfiguration.params b/cfg/NodeConfiguration.params index f38c368..28cc2ab 100755 --- a/cfg/NodeConfiguration.params +++ b/cfg/NodeConfiguration.params @@ -2,6 +2,7 @@ from rosparam_handler.parameter_generator_catkin import * gen = ParameterGenerator() +gen.add("sim", paramtype="bool", description="if simulation or not", default=False, configurable=False) gen.add("ft_pub_freq", paramtype="double", description="publish frequency", default=200, configurable=False) gen.add("ft_pull_freq", paramtype="double", description="sensor daa pull frequency", default=800, configurable=False) gen.add("transform_frame", paramtype="std::string", description="reference coordinate system", default="fts_transform_frame", configurable=False) diff --git a/config/sensor_configuration_sim.yaml b/config/sensor_configuration_sim.yaml new file mode 100644 index 0000000..92ee9fe --- /dev/null +++ b/config/sensor_configuration_sim.yaml @@ -0,0 +1,14 @@ +Node: + sim: true + ft_pub_freq: 200 + ft_pull_freq: 800 + sensor_frame: "" + 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 new file mode 100644 index 0000000..f1ac709 --- /dev/null +++ b/description/urdf/macros/mini58_sim.urdf.xacro @@ -0,0 +1,63 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/ros/launch/driver.launch b/ros/launch/driver.launch index cc09185..b306fb4 100644 --- a/ros/launch/driver.launch +++ b/ros/launch/driver.launch @@ -1,7 +1,8 @@ - + + @@ -14,6 +15,7 @@ + diff --git a/ros/launch/sensor_parameters.launch b/ros/launch/sensor_parameters.launch index 2fc1b21..d6afc4b 100644 --- a/ros/launch/sensor_parameters.launch +++ b/ros/launch/sensor_parameters.launch @@ -1,9 +1,11 @@ + - + + From bfc3ea9ba2192a71347a5c13c1ddb8b70f3c3b8e Mon Sep 17 00:00:00 2001 From: Andreea Tulbure Date: Tue, 22 Aug 2017 12:24:00 +0200 Subject: [PATCH 22/53] changes for working correctly with rosparam_handler --- cfg/Calibration.params | 8 +-- cfg/CoordinateSystemCalibration.params | 4 +- config/sensor_configuration.yaml | 11 ++-- config/sensor_configuration_robot.yaml | 72 ++++++++++++++++++++++++++ config/sensor_configuration_sim.yaml | 4 ++ config/sensor_offset.yaml | 4 +- ros/launch/driver.launch | 2 +- ros/launch/sensor_parameters.launch | 1 + ros/scripts/calculate_offsets.py | 9 ++-- ros/src/force_torque_sensor.cpp | 18 +++---- ros/src/force_torque_sensor_node.cpp | 37 ++++++------- 11 files changed, 115 insertions(+), 55 deletions(-) create mode 100644 config/sensor_configuration_robot.yaml diff --git a/cfg/Calibration.params b/cfg/Calibration.params index f74b0e9..1eff11f 100755 --- a/cfg/Calibration.params +++ b/cfg/Calibration.params @@ -5,11 +5,7 @@ gen = ParameterGenerator() gen.add("n_measurements", paramtype="int", description="number of necessary measurements for calibration", default=10, configurable=True) gen.add("T_between_meas", paramtype="double", description="time between two measurements for calibration", default=0.01, configurable=True) gen.add("isStatic", paramtype="bool", description="is static calibration active", default=False, configurable=True) -gen.add("Offset_Force_x", paramtype="double", description="calibration_offset_force_x", default=0.0, configurable=True) -gen.add("Offset_Force_y", paramtype="double", description="calibration_offset_force_y", default=0.0, configurable=True) -gen.add("Offset_Force_z", paramtype="double", description="calibration_offset_force_z", default=0.0, configurable=True) -gen.add("Offset_Torque_x", paramtype="double", description="calibration_offset_torque_x", default=0.0, configurable=True) -gen.add("Offset_Torque_y", paramtype="double", description="calibration_offset_torque_y", default=0.0, configurable=True) -gen.add("Offset_Torque_z", paramtype="double", description="calibration_offset_torque_z", default=0.0, configurable=True) +gen.add("force", paramtype="std::vector", description="calibration_offset_vector_force") +gen.add("torque", paramtype="std::vector", description="calibration_offset_vector_torque") exit(gen.generate("ati_force_torque", "ForceTorqueSensor", "Calibration")) diff --git a/cfg/CoordinateSystemCalibration.params b/cfg/CoordinateSystemCalibration.params index 1f9190d..e83de6a 100755 --- a/cfg/CoordinateSystemCalibration.params +++ b/cfg/CoordinateSystemCalibration.params @@ -2,8 +2,8 @@ from rosparam_handler.parameter_generator_catkin import * gen = ParameterGenerator() -gen.add("n_measurements", paramtype="int", description="number of necessary measurements for calibration", default=20, configurable=True) -gen.add("T_between_meas", paramtype="int", description="time between two measurements for calibration", default=10000, configurable=True) +gen.add("n_measurements", paramtype="int", description="number of necessary measurements for calibration", default=0, configurable=True) +gen.add("T_between_meas", paramtype="int", description="time between two measurements for calibration", default=0, configurable=True) gen.add("push_direction", paramtype="int", description="push direction", default=0, configurable=False) exit(gen.generate("ati_force_torque", "ForceTorqueSensor", "CoordinateSystemCalibration")) diff --git a/config/sensor_configuration.yaml b/config/sensor_configuration.yaml index 9dc3e86..a3c89f2 100644 --- a/config/sensor_configuration.yaml +++ b/config/sensor_configuration.yaml @@ -2,13 +2,14 @@ Node: sim: false ft_pub_freq: 200 ft_pull_freq: 800 - sensor_frame: "fts_reference_link" - transform_frame: "fts_base_link" + sensor_frame: "" + transform_frame: "" 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 + 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 diff --git a/config/sensor_configuration_robot.yaml b/config/sensor_configuration_robot.yaml new file mode 100644 index 0000000..77133f2 --- /dev/null +++ b/config/sensor_configuration_robot.yaml @@ -0,0 +1,72 @@ +Node: + sim: false + ft_pub_freq: 200 + ft_pull_freq: 800 + sensor_frame: "fts_reference_link" + transform_frame: "fts_base_link" + +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 + +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/config/sensor_configuration_sim.yaml b/config/sensor_configuration_sim.yaml index 92ee9fe..24510dd 100644 --- a/config/sensor_configuration_sim.yaml +++ b/config/sensor_configuration_sim.yaml @@ -4,6 +4,10 @@ Node: ft_pull_freq: 800 sensor_frame: "" transform_frame: "" + sim_forceX_param: 40 + sim_forceY_param: 55 + sim_torqueZ_param: 10 + sim: true Publish: sensor_data: true diff --git a/config/sensor_offset.yaml b/config/sensor_offset.yaml index 85325ad..0a9c6a8 100644 --- a/config/sensor_offset.yaml +++ b/config/sensor_offset.yaml @@ -1,2 +1,2 @@ -force: {x: -19.104102666356425, y: -1.5473841984049037, z: 0.9332121238751951} -torque: {x: -0.24642145895240927, y: 0.0373995095506341, z: 0.060428603494093} +force: [-19.104102666356425, -1.5473841984049037, 0.9332121238751951] +torque: [-0.24642145895240927, 0.0373995095506341, 0.060428603494093] diff --git a/ros/launch/driver.launch b/ros/launch/driver.launch index b306fb4..8e45994 100644 --- a/ros/launch/driver.launch +++ b/ros/launch/driver.launch @@ -4,7 +4,7 @@ - + diff --git a/ros/launch/sensor_parameters.launch b/ros/launch/sensor_parameters.launch index d6afc4b..694ee07 100644 --- a/ros/launch/sensor_parameters.launch +++ b/ros/launch/sensor_parameters.launch @@ -7,5 +7,6 @@ + diff --git a/ros/scripts/calculate_offsets.py b/ros/scripts/calculate_offsets.py index ee99d0b..6676d66 100755 --- a/ros/scripts/calculate_offsets.py +++ b/ros/scripts/calculate_offsets.py @@ -57,12 +57,9 @@ def calculate_sensor_offsets(): measurement.torque.y /= len(poses) measurement.torque.z /= len(poses) - rospy.set_param('/temp/Offset/force/x', measurement.force.x) - rospy.set_param('/temp/Offset/force/y', measurement.force.y) - rospy.set_param('/temp/Offset/force/z', measurement.force.z) - rospy.set_param('/temp/Offset/torque/x', measurement.torque.x) - rospy.set_param('/temp/Offset/torque/y', measurement.torque.y) - rospy.set_param('/temp/Offset/torque/z', measurement.torque.z) + rospy.set_param('/temp/Offset/force', [measurement.force.x, measurement.force.y, measurement.force.z]) + rospy.set_param('/temp/Offset/torque', [measurement.torque.x, measurement.torque.y, measurement.torque.z]) + if store_to_file: call('rosparam dump -v `rospack find ati_force_torque`/config/sensor_offset.yaml /temp/Offset', shell=True) diff --git a/ros/src/force_torque_sensor.cpp b/ros/src/force_torque_sensor.cpp index 5ccfafb..7f405c3 100644 --- a/ros/src/force_torque_sensor.cpp +++ b/ros/src/force_torque_sensor.cpp @@ -54,16 +54,16 @@ ****************************************************************/ #include -ForceTorqueSensor::ForceTorqueSensor(ros::NodeHandle& nh) : nh_(nh), CS_params_{nh}, can_params_{nh}, FTS_params_{nh} , pub_params_{nh} , node_params_{nh} , calibration_params_{nh} , gravity_params_{nh} +ForceTorqueSensor::ForceTorqueSensor(ros::NodeHandle& nh) : nh_(nh), CS_params_{nh}, can_params_{nh.getNamespace()+"/CAN"}, FTS_params_{nh.getNamespace()+"/FTS"} , pub_params_{nh.getNamespace()+"/Publish"} , node_params_{nh.getNamespace()+"/Node"} , calibration_params_{nh.getNamespace()+"/Calibration"} , gravity_params_{nh.getNamespace()+"/GravityCompensation"} { - can_params_.setNamespace(nh_.getNamespace()+"/CAN"); + /*can_params_.setNamespace(nh_.getNamespace()+"/CAN"); CS_params_.setNamespace(nh_.getNamespace()); FTS_params_.setNamespace(nh_.getNamespace()+"/FTS"); pub_params_.setNamespace(nh_.getNamespace()+"/Publish"); node_params_.setNamespace(nh_.getNamespace()+"/Node"); calibration_params_.setNamespace(nh_.getNamespace()+"/Calibration"); - gravity_params_.setNamespace(nh_.getNamespace()+"/GravityCompensation"); - + gravity_params_.setNamespace(nh_.getNamespace()+"/GravityCompensation");*/ + std::cout<<"ns: "<. * ****************************************************************/ -#include +#include +#include +#include -class ForceTorqueSensorNode : public ForceTorqueSensor +class ForceTorqueSensorNode { public: - ForceTorqueSensorNode(ros::NodeHandle &nh); - - void updateFTData(const ros::TimerEvent &event); - - -private: - ros::Timer ftUpdateTimer_; - - tf2_ros::TransformListener *p_tfListener; - tf2::Transform transform_ee_base; -}; - -ForceTorqueSensorNode::ForceTorqueSensorNode(ros::NodeHandle &nh) : ForceTorqueSensor(nh) -{ - nh_.param("Node/sensor_frame", sensor_frame_, "fts_reference_link"); - nh_.param("Node/transform_frame", transform_frame_, "fts_base_link"); -} - -void ForceTorqueSensorNode::updateFTData(const ros::TimerEvent &event) + ForceTorqueSensorNode(ros::NodeHandle &nh):node_params_{nh} { - filterFTData(); + node_params_.setNamespace(nh.getNamespace()+"/Node"); + node_params_.fromParamServer(); + bool sim = node_params_.sim; + if(sim) new ForceTorqueSensorHandleSim(nh,node_params_.sensor_frame,node_params_.transform_frame); + else{ + new ForceTorqueSensorHandle(nh,node_params_.sensor_frame,node_params_.transform_frame);; + } } +private: + ati_force_torque::NodeConfigurationParameters node_params_; +}; int main(int argc, char **argv) { From cdd4da5c26a9d608677b3f6f9755fbda8e5a0842 Mon Sep 17 00:00:00 2001 From: Andreea Tulbure Date: Tue, 22 Aug 2017 12:45:47 +0200 Subject: [PATCH 23/53] changes to work correctly with rosparam and real raw with fts --- config/sensor_configuration.yaml | 4 ++-- config/sensor_configuration_robot.yaml | 9 +++++---- ros/src/force_torque_sensor.cpp | 11 ++--------- 3 files changed, 9 insertions(+), 15 deletions(-) diff --git a/config/sensor_configuration.yaml b/config/sensor_configuration.yaml index a3c89f2..a793884 100644 --- a/config/sensor_configuration.yaml +++ b/config/sensor_configuration.yaml @@ -2,8 +2,8 @@ Node: sim: false ft_pub_freq: 200 ft_pull_freq: 800 - sensor_frame: "" - transform_frame: "" + sensor_frame: "fts_reference_link" + transform_frame: "fts_base_link" Calibration: Offset: diff --git a/config/sensor_configuration_robot.yaml b/config/sensor_configuration_robot.yaml index 77133f2..9840c5e 100644 --- a/config/sensor_configuration_robot.yaml +++ b/config/sensor_configuration_robot.yaml @@ -3,12 +3,13 @@ Node: ft_pub_freq: 200 ft_pull_freq: 800 sensor_frame: "fts_reference_link" - transform_frame: "fts_base_link" + transform_frame: "fts_transform_frame" 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 + 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 diff --git a/ros/src/force_torque_sensor.cpp b/ros/src/force_torque_sensor.cpp index 7f405c3..c310b6a 100644 --- a/ros/src/force_torque_sensor.cpp +++ b/ros/src/force_torque_sensor.cpp @@ -54,16 +54,9 @@ ****************************************************************/ #include -ForceTorqueSensor::ForceTorqueSensor(ros::NodeHandle& nh) : nh_(nh), CS_params_{nh}, can_params_{nh.getNamespace()+"/CAN"}, FTS_params_{nh.getNamespace()+"/FTS"} , pub_params_{nh.getNamespace()+"/Publish"} , node_params_{nh.getNamespace()+"/Node"} , calibration_params_{nh.getNamespace()+"/Calibration"} , gravity_params_{nh.getNamespace()+"/GravityCompensation"} +ForceTorqueSensor::ForceTorqueSensor(ros::NodeHandle& nh) : nh_(nh), CS_params_{nh}, can_params_{nh.getNamespace()+"/CAN"}, FTS_params_{nh.getNamespace()+"/FTS"} , pub_params_{nh.getNamespace()+"/Publish"} , node_params_{nh.getNamespace()+"/Node"} , calibration_params_{nh.getNamespace()+"/Calibration/Offset"} , gravity_params_{nh.getNamespace()+"/GravityCompensation"} { - /*can_params_.setNamespace(nh_.getNamespace()+"/CAN"); - CS_params_.setNamespace(nh_.getNamespace()); - FTS_params_.setNamespace(nh_.getNamespace()+"/FTS"); - pub_params_.setNamespace(nh_.getNamespace()+"/Publish"); - node_params_.setNamespace(nh_.getNamespace()+"/Node"); - calibration_params_.setNamespace(nh_.getNamespace()+"/Calibration"); - gravity_params_.setNamespace(nh_.getNamespace()+"/GravityCompensation");*/ - std::cout<<"ns: "< Date: Tue, 22 Aug 2017 12:53:52 +0200 Subject: [PATCH 24/53] changes for working with real sensor and simulation --- .../force_torque_sensor_sim.h | 19 ++++---- ros/src/force_torque_sensor.cpp | 6 ++- ros/src/force_torque_sensor_handle_sim.cpp | 1 + ros/src/force_torque_sensor_sim.cpp | 44 ++++++++++--------- 4 files changed, 40 insertions(+), 30 deletions(-) diff --git a/ros/include/ati_force_torque/force_torque_sensor_sim.h b/ros/include/ati_force_torque/force_torque_sensor_sim.h index 72e9b53..76e9341 100644 --- a/ros/include/ati_force_torque/force_torque_sensor_sim.h +++ b/ros/include/ati_force_torque/force_torque_sensor_sim.h @@ -53,6 +53,7 @@ typedef unsigned char uint8_t; #include #include #include + #include #include @@ -61,7 +62,7 @@ typedef unsigned char uint8_t; #define PI 3.14159265 -class ForceTorqueSensorSim +class ForceTorqueSensorSim { public: ForceTorqueSensorSim(ros::NodeHandle &nh); @@ -72,24 +73,24 @@ class ForceTorqueSensorSim std::string transform_frame_; std::string sensor_frame_; ati_force_torque::NodeConfigurationParameters node_params_; - - void pullFTData(const geometry_msgs::Twist::ConstPtr& msg); + ati_force_torque::PublishConfigurationParameters pub_params_; + void pullFTData(const ros::TimerEvent &event); + void filterFTData(); + void subscribeData(const geometry_msgs::Twist::ConstPtr& msg); // Arrays for dumping FT-Data - geometry_msgs::WrenchStamped threshold_filtered_force, transformed_data, joystick_data;; + geometry_msgs::WrenchStamped threshold_filtered_force, transformed_data, joystick_data; + virtual void updateFTData(const ros::TimerEvent &event) = 0; private: - virtual void updateFTData(const ros::TimerEvent &event) = 0; bool transform_wrench(std::string goal_frame, std::string source_frame, geometry_msgs::Wrench wrench, geometry_msgs::Wrench *transformed); - ati_force_torque::PublishConfigurationParameters pub_params_; - - ros::Subscriber force_input_subscriber; + ros::Subscriber force_input_subscriber; uint _num_transform_errors; tf2_ros::Buffer *p_tfBuffer; tf2_ros::TransformListener *p_tfListener; ros::NodeHandle nh_; ros::Publisher transformed_data_pub_,sensor_data_pub_; - ros::Timer ftUpdateTimer_; + ros::Timer ftUpdateTimer_, ftPullTimer_; bool is_pub_transformed_data_ =false; bool is_pub_sensor_data_=false; diff --git a/ros/src/force_torque_sensor.cpp b/ros/src/force_torque_sensor.cpp index c310b6a..8685280 100644 --- a/ros/src/force_torque_sensor.cpp +++ b/ros/src/force_torque_sensor.cpp @@ -103,7 +103,11 @@ ForceTorqueSensor::ForceTorqueSensor(ros::NodeHandle& nh) : nh_(nh), CS_params_ calibrationTBetween=calibration_params_.T_between_meas; m_staticCalibration=calibration_params_.isStatic; m_calibOffset.force.x = calibration_params_.force[0]; - std::cout<<"force x:"<< m_calibOffset.force.x< -ForceTorqueSensorSim::ForceTorqueSensorSim(ros::NodeHandle& nh) : nh_(nh), pub_params_{nh}, node_params_{nh} +ForceTorqueSensorSim::ForceTorqueSensorSim(ros::NodeHandle& nh) : nh_(nh), pub_params_{nh.getNamespace()+"/Publish"}, node_params_{nh.getNamespace()+"/Node"} { std::cout<<"ForceTorqueSensorSim"<("transformed_data", 1); } - ftUpdateTimer_ = nh.createTimer(ros::Rate(node_params_.ft_pull_freq), &ForceTorqueSensorSim::updateFTData, this, false, false); - + ftUpdateTimer_ = nh.createTimer(ros::Rate(node_params_.ft_pub_freq), &ForceTorqueSensorSim::updateFTData, this, false, false); + ftPullTimer_ = nh.createTimer(ros::Rate(node_params_.ft_pull_freq), &ForceTorqueSensorSim::pullFTData, this, false, false); p_tfBuffer = new tf2_ros::Buffer(); p_tfListener = new tf2_ros::TransformListener(*p_tfBuffer, true); init_sensor(); ftUpdateTimer_.start(); } void ForceTorqueSensorSim::init_sensor() { - force_input_subscriber = nh_.subscribe("twist_controller/command", 1, &ForceTorqueSensorSim::pullFTData, this); + force_input_subscriber = nh_.subscribe("twist_mux/command_teleop_joy", 1, &ForceTorqueSensorSim::subscribeData, this); + ftPullTimer_.start(); } -void ForceTorqueSensorSim::pullFTData(const geometry_msgs::Twist::ConstPtr &msg) -{ - int status = 0; - std::string transform_frame_ = ""; + +void ForceTorqueSensorSim::subscribeData(const geometry_msgs::Twist::ConstPtr& msg){ joystick_data.wrench.force.x = msg->linear.x; joystick_data.wrench.force.y = msg->linear.y; joystick_data.wrench.force.z = 0; joystick_data.wrench.torque.z = msg->angular.z; joystick_data.wrench.torque.x = 0; joystick_data.wrench.torque.y = 0; - transformed_data.header.stamp = ros::Time::now(); - transformed_data.header.frame_id = transform_frame_; - transform_wrench(transform_frame_, sensor_frame_, joystick_data.wrench, &transformed_data.wrench); - threshold_filtered_force = transformed_data; - if(is_pub_sensor_data_) - sensor_data_pub_.publish(joystick_data); - if(is_pub_transformed_data_) - transformed_data_pub_.publish(threshold_filtered_force); +} + +void ForceTorqueSensorSim::pullFTData(const ros::TimerEvent &event) +{ + if(is_pub_sensor_data_) + sensor_data_pub_.publish(joystick_data); } bool ForceTorqueSensorSim::transform_wrench(std::string goal_frame, std::string source_frame, geometry_msgs::Wrench wrench, geometry_msgs::Wrench *transformed) { @@ -118,3 +114,11 @@ bool ForceTorqueSensorSim::transform_wrench(std::string goal_frame, std::string return true; } +void ForceTorqueSensorSim::filterFTData(){ + transformed_data.header.stamp = ros::Time::now(); + transformed_data.header.frame_id = node_params_.transform_frame; + transform_wrench(node_params_.transform_frame, sensor_frame_, joystick_data.wrench, &transformed_data.wrench); + threshold_filtered_force = transformed_data; + if(is_pub_transformed_data_) + transformed_data_pub_.publish(threshold_filtered_force); +} From 0708b2e68b518f72cc8ca30b6427f72b1d38e240 Mon Sep 17 00:00:00 2001 From: Andreea Tulbure Date: Wed, 23 Aug 2017 09:57:56 +0200 Subject: [PATCH 25/53] not depending on own rosparam_handler anymore --- .travis.rosinstall | 4 ---- ros/src/force_torque_sensor_node.cpp | 3 +-- 2 files changed, 1 insertion(+), 6 deletions(-) diff --git a/.travis.rosinstall b/.travis.rosinstall index 3758a34..a2f81f4 100644 --- a/.travis.rosinstall +++ b/.travis.rosinstall @@ -2,10 +2,6 @@ uri: 'https://github.com/iirob/iirob_filters.git' local-name: iirob_filters version: kinetic-devel -- git: - uri: 'https://github.com/iirob/rosparam_handler.git' - local-name: rosparam_handler - version: ns_set_get - git: uri: 'https://github.com/ipa320/cob_driver.git' local-name: cob_driver diff --git a/ros/src/force_torque_sensor_node.cpp b/ros/src/force_torque_sensor_node.cpp index 3ab2f9d..51d0cbe 100644 --- a/ros/src/force_torque_sensor_node.cpp +++ b/ros/src/force_torque_sensor_node.cpp @@ -59,9 +59,8 @@ class ForceTorqueSensorNode { public: - ForceTorqueSensorNode(ros::NodeHandle &nh):node_params_{nh} + ForceTorqueSensorNode(ros::NodeHandle &nh):node_params_{nh.getNamespace()+"/Node"} { - node_params_.setNamespace(nh.getNamespace()+"/Node"); node_params_.fromParamServer(); bool sim = node_params_.sim; if(sim) new ForceTorqueSensorHandleSim(nh,node_params_.sensor_frame,node_params_.transform_frame); From 70c998bc5d6052348d32af46c098de845dc4fe4a Mon Sep 17 00:00:00 2001 From: Andreea Tulbure Date: Thu, 24 Aug 2017 10:54:30 +0200 Subject: [PATCH 26/53] chages for simulation of sensor --- CMakeLists.txt | 3 +- config/joy.yaml | 2 ++ config/teleop_sim.yaml | 33 ++++++++++++++++++++ package.xml | 1 + ros/launch/driver_socket_sim.launch | 36 ++++++++++++++++++++++ ros/launch/joy.launch | 9 ++++++ ros/launch/teleop_sim.launch | 13 ++++++++ ros/src/force_torque_sensor_handle_sim.cpp | 6 ++-- ros/src/force_torque_sensor_sim.cpp | 10 +++--- 9 files changed, 103 insertions(+), 10 deletions(-) create mode 100644 config/joy.yaml create mode 100644 config/teleop_sim.yaml create mode 100644 ros/launch/driver_socket_sim.launch create mode 100644 ros/launch/joy.launch create mode 100644 ros/launch/teleop_sim.launch diff --git a/CMakeLists.txt b/CMakeLists.txt index 20dd3c2..e191bff 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -19,6 +19,7 @@ find_package(catkin REQUIRED COMPONENTS tf2_ros trajectory_msgs visualization_msgs + cob_teleop ) find_package(Eigen3) @@ -64,7 +65,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 cob_teleop DEPENDS Eigen LIBRARIES ${PROJECT_NAME} ) diff --git a/config/joy.yaml b/config/joy.yaml new file mode 100644 index 0000000..f1ac160 --- /dev/null +++ b/config/joy.yaml @@ -0,0 +1,2 @@ +dev: /dev/input/js0 +deadzone: 0.12 diff --git a/config/teleop_sim.yaml b/config/teleop_sim.yaml new file mode 100644 index 0000000..0346ab5 --- /dev/null +++ b/config/teleop_sim.yaml @@ -0,0 +1,33 @@ +#raw3-5 +########## +# common params +sim: true +run_factor: 10.0 +apply_ramp: false +joy_num_buttons: 12 +joy_num_axes: 6 +joy_num_modes: 1 + +# axes +axis_vx: 1 +axis_vy: 0 +axis_vz: 5 +axis_roll: 4 +axis_pitch: 3 +axis_yaw: 2 + +# buttons +deadman_button: 5 +safety_button: 4 +init_button: 9 + +#mode1: Base +run_button: 7 + +components: { + base: { + twist_topic_name: '/base/twist_mux/command_teleop_joy', + twist_max_velocity: [1.0, 1.0, 1.0, 1.0,1.0, 1.0], + twist_max_acc: [0.5, 0.5, 0.5, 0.7, 0.7, 0.7] + } +} diff --git a/package.xml b/package.xml index d069432..27a6a1e 100644 --- a/package.xml +++ b/package.xml @@ -33,6 +33,7 @@ visualization_msgs rosparam_handler dynamic_reconfigure + cob_teleop message_runtime robot_state_publisher diff --git a/ros/launch/driver_socket_sim.launch b/ros/launch/driver_socket_sim.launch new file mode 100644 index 0000000..e376c3e --- /dev/null +++ b/ros/launch/driver_socket_sim.launch @@ -0,0 +1,36 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/ros/launch/joy.launch b/ros/launch/joy.launch new file mode 100644 index 0000000..8fbe4f4 --- /dev/null +++ b/ros/launch/joy.launch @@ -0,0 +1,9 @@ + + + + + + + + + diff --git a/ros/launch/teleop_sim.launch b/ros/launch/teleop_sim.launch new file mode 100644 index 0000000..f4597b6 --- /dev/null +++ b/ros/launch/teleop_sim.launch @@ -0,0 +1,13 @@ + + + + + + + + + + + + + diff --git a/ros/src/force_torque_sensor_handle_sim.cpp b/ros/src/force_torque_sensor_handle_sim.cpp index 04794a8..a44b3cf 100644 --- a/ros/src/force_torque_sensor_handle_sim.cpp +++ b/ros/src/force_torque_sensor_handle_sim.cpp @@ -9,11 +9,11 @@ ForceTorqueSensorHandleSim::ForceTorqueSensorHandleSim(ros::NodeHandle& nh, std: void ForceTorqueSensorHandleSim::updateFTData(const ros::TimerEvent& event) { filterFTData(); - interface_force_[0] = threshold_filtered_force.wrench.force.x*node_params_.sim_forceX_param; - interface_force_[1] = threshold_filtered_force.wrench.force.y*node_params_.sim_forceY_param;; + interface_force_[0] = threshold_filtered_force.wrench.force.x; + interface_force_[1] = threshold_filtered_force.wrench.force.y; interface_force_[2] = threshold_filtered_force.wrench.force.z; interface_torque_[0] = threshold_filtered_force.wrench.torque.x; interface_torque_[1] = threshold_filtered_force.wrench.torque.y; - interface_torque_[2] = threshold_filtered_force.wrench.torque.z*node_params_.sim_torqueZ_param;; + interface_torque_[2] = threshold_filtered_force.wrench.torque.z; } diff --git a/ros/src/force_torque_sensor_sim.cpp b/ros/src/force_torque_sensor_sim.cpp index 53ffa66..d6c1f30 100644 --- a/ros/src/force_torque_sensor_sim.cpp +++ b/ros/src/force_torque_sensor_sim.cpp @@ -45,8 +45,6 @@ ForceTorqueSensorSim::ForceTorqueSensorSim(ros::NodeHandle& nh) : nh_(nh), pub_params_{nh.getNamespace()+"/Publish"}, node_params_{nh.getNamespace()+"/Node"} { std::cout<<"ForceTorqueSensorSim"<linear.x; joystick_data.wrench.force.y = msg->linear.y; - joystick_data.wrench.force.z = 0; + joystick_data.wrench.force.z = msg->linear.z; joystick_data.wrench.torque.z = msg->angular.z; - joystick_data.wrench.torque.x = 0; - joystick_data.wrench.torque.y = 0; + joystick_data.wrench.torque.x = msg->angular.x;; + joystick_data.wrench.torque.y = msg->angular.y; } void ForceTorqueSensorSim::pullFTData(const ros::TimerEvent &event) From bd7c11051bac1a36a95d060208d76cd5684f669d Mon Sep 17 00:00:00 2001 From: Andreea Tulbure Date: Thu, 24 Aug 2017 12:01:07 +0200 Subject: [PATCH 27/53] using teleop not cob_teleop anymore --- CMakeLists.txt | 3 +- config/teleop_sim.yaml | 34 +++---------------- package.xml | 1 - ...er_socket_sim.launch => driver_sim.launch} | 6 ++-- ros/launch/joy.launch | 9 ----- ros/launch/teleop_sim.launch | 13 ------- ros/src/force_torque_sensor_sim.cpp | 2 +- 7 files changed, 11 insertions(+), 57 deletions(-) rename ros/launch/{driver_socket_sim.launch => driver_sim.launch} (88%) delete mode 100644 ros/launch/joy.launch delete mode 100644 ros/launch/teleop_sim.launch diff --git a/CMakeLists.txt b/CMakeLists.txt index e191bff..20dd3c2 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -19,7 +19,6 @@ find_package(catkin REQUIRED COMPONENTS tf2_ros trajectory_msgs visualization_msgs - cob_teleop ) find_package(Eigen3) @@ -65,7 +64,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 cob_teleop + 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 DEPENDS Eigen LIBRARIES ${PROJECT_NAME} ) diff --git a/config/teleop_sim.yaml b/config/teleop_sim.yaml index 0346ab5..b88f2ed 100644 --- a/config/teleop_sim.yaml +++ b/config/teleop_sim.yaml @@ -1,33 +1,9 @@ -#raw3-5 -########## -# common params -sim: true -run_factor: 10.0 -apply_ramp: false -joy_num_buttons: 12 -joy_num_axes: 6 -joy_num_modes: 1 +axis_linear: { x: 1, y: 0, z: 4 } # Left thumb stick +scale_linear: { x: 12.0, y: 12.0, z: 12.0 } -# axes -axis_vx: 1 -axis_vy: 0 -axis_vz: 5 -axis_roll: 4 -axis_pitch: 3 -axis_yaw: 2 +axis_angular: { roll: 4, pitch: 3, yaw: 2 } # Right thumb stick +scale_angular: { roll: 12.0, pitch: 12.0, yaw: 12.0 } -# buttons -deadman_button: 5 -safety_button: 4 -init_button: 9 +enable_button: 5 -#mode1: Base -run_button: 7 -components: { - base: { - twist_topic_name: '/base/twist_mux/command_teleop_joy', - twist_max_velocity: [1.0, 1.0, 1.0, 1.0,1.0, 1.0], - twist_max_acc: [0.5, 0.5, 0.5, 0.7, 0.7, 0.7] - } -} diff --git a/package.xml b/package.xml index 27a6a1e..d069432 100644 --- a/package.xml +++ b/package.xml @@ -33,7 +33,6 @@ visualization_msgs rosparam_handler dynamic_reconfigure - cob_teleop message_runtime robot_state_publisher diff --git a/ros/launch/driver_socket_sim.launch b/ros/launch/driver_sim.launch similarity index 88% rename from ros/launch/driver_socket_sim.launch rename to ros/launch/driver_sim.launch index e376c3e..91048a6 100644 --- a/ros/launch/driver_socket_sim.launch +++ b/ros/launch/driver_sim.launch @@ -27,8 +27,10 @@ - - + + + + diff --git a/ros/launch/joy.launch b/ros/launch/joy.launch deleted file mode 100644 index 8fbe4f4..0000000 --- a/ros/launch/joy.launch +++ /dev/null @@ -1,9 +0,0 @@ - - - - - - - - - diff --git a/ros/launch/teleop_sim.launch b/ros/launch/teleop_sim.launch deleted file mode 100644 index f4597b6..0000000 --- a/ros/launch/teleop_sim.launch +++ /dev/null @@ -1,13 +0,0 @@ - - - - - - - - - - - - - diff --git a/ros/src/force_torque_sensor_sim.cpp b/ros/src/force_torque_sensor_sim.cpp index d6c1f30..59ea107 100644 --- a/ros/src/force_torque_sensor_sim.cpp +++ b/ros/src/force_torque_sensor_sim.cpp @@ -65,7 +65,7 @@ ForceTorqueSensorSim::ForceTorqueSensorSim(ros::NodeHandle& nh) : nh_(nh), pub_p ftUpdateTimer_.start(); } void ForceTorqueSensorSim::init_sensor() { - force_input_subscriber = nh_.subscribe("/base/twist_mux/command_teleop_joy", 1, &ForceTorqueSensorSim::subscribeData, this); + force_input_subscriber = nh_.subscribe("/cmd_vel", 1, &ForceTorqueSensorSim::subscribeData, this); ftPullTimer_.start(); } From 070306023768388ecf497b2de8ef1087defc1b0a Mon Sep 17 00:00:00 2001 From: Andreea Tulbure Date: Fri, 25 Aug 2017 10:28:22 +0200 Subject: [PATCH 28/53] remapped to cmd_force --- config/joy.yaml | 2 -- ros/launch/driver_sim.launch | 2 +- ros/src/force_torque_sensor_sim.cpp | 2 +- 3 files changed, 2 insertions(+), 4 deletions(-) delete mode 100644 config/joy.yaml diff --git a/config/joy.yaml b/config/joy.yaml deleted file mode 100644 index f1ac160..0000000 --- a/config/joy.yaml +++ /dev/null @@ -1,2 +0,0 @@ -dev: /dev/input/js0 -deadzone: 0.12 diff --git a/ros/launch/driver_sim.launch b/ros/launch/driver_sim.launch index 91048a6..8df9a36 100644 --- a/ros/launch/driver_sim.launch +++ b/ros/launch/driver_sim.launch @@ -9,7 +9,7 @@ - + diff --git a/ros/src/force_torque_sensor_sim.cpp b/ros/src/force_torque_sensor_sim.cpp index 59ea107..8011f51 100644 --- a/ros/src/force_torque_sensor_sim.cpp +++ b/ros/src/force_torque_sensor_sim.cpp @@ -65,7 +65,7 @@ ForceTorqueSensorSim::ForceTorqueSensorSim(ros::NodeHandle& nh) : nh_(nh), pub_p ftUpdateTimer_.start(); } void ForceTorqueSensorSim::init_sensor() { - force_input_subscriber = nh_.subscribe("/cmd_vel", 1, &ForceTorqueSensorSim::subscribeData, this); + force_input_subscriber = nh_.subscribe("/cmd_force", 1, &ForceTorqueSensorSim::subscribeData, this); ftPullTimer_.start(); } From 6f40f7a479424dcbddafd6d837ead50c52d1b705 Mon Sep 17 00:00:00 2001 From: Andreea Tulbure Date: Fri, 25 Aug 2017 10:49:57 +0200 Subject: [PATCH 29/53] changed default values for easier debugging --- cfg/Calibration.params | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cfg/Calibration.params b/cfg/Calibration.params index 1eff11f..6d80ac2 100755 --- a/cfg/Calibration.params +++ b/cfg/Calibration.params @@ -2,7 +2,7 @@ from rosparam_handler.parameter_generator_catkin import * gen = ParameterGenerator() -gen.add("n_measurements", paramtype="int", description="number of necessary measurements for calibration", default=10, configurable=True) +gen.add("n_measurements", paramtype="int", description="number of necessary measurements for calibration", default=100, configurable=True) gen.add("T_between_meas", paramtype="double", description="time between two measurements for calibration", default=0.01, configurable=True) gen.add("isStatic", paramtype="bool", description="is static calibration active", default=False, configurable=True) gen.add("force", paramtype="std::vector", description="calibration_offset_vector_force") From d55fb2b50b8431e4577a0c3fd7a6b538c2f6e932 Mon Sep 17 00:00:00 2001 From: Andreea Tulbure Date: Mon, 28 Aug 2017 09:34:28 +0200 Subject: [PATCH 30/53] small changes --- cfg/Calibration.params | 2 +- cfg/FTSConfiguration.params | 4 +-- cfg/NodeConfiguration.params | 3 --- ros/src/force_torque_sensor.cpp | 46 ++++++++++++++++----------------- 4 files changed, 25 insertions(+), 30 deletions(-) diff --git a/cfg/Calibration.params b/cfg/Calibration.params index 6d80ac2..13dc697 100755 --- a/cfg/Calibration.params +++ b/cfg/Calibration.params @@ -3,7 +3,7 @@ from rosparam_handler.parameter_generator_catkin import * gen = ParameterGenerator() gen.add("n_measurements", paramtype="int", description="number of necessary measurements for calibration", default=100, configurable=True) -gen.add("T_between_meas", paramtype="double", description="time between two measurements for calibration", default=0.01, configurable=True) +gen.add("T_between_meas", paramtype="double", description="time between two measurements for calibration", default=0.1, configurable=True) gen.add("isStatic", paramtype="bool", description="is static calibration active", default=False, configurable=True) gen.add("force", paramtype="std::vector", description="calibration_offset_vector_force") gen.add("torque", paramtype="std::vector", description="calibration_offset_vector_torque") diff --git a/cfg/FTSConfiguration.params b/cfg/FTSConfiguration.params index 4ad64f7..0dd17f9 100755 --- a/cfg/FTSConfiguration.params +++ b/cfg/FTSConfiguration.params @@ -2,8 +2,8 @@ from rosparam_handler.parameter_generator_catkin import * gen = ParameterGenerator() -gen.add("base_identifier", paramtype="int", description="identifier", default=0x20, configurable=False) +gen.add("base_identifier", paramtype="int", description="identifier", default=0x00, configurable=False) gen.add("auto_init", paramtype="bool", description="auto init", default=True, configurable=False) -gen.add("fts_name", paramtype="std::string", description="name of sensor", default="ATI_45_Mini", configurable=False) +gen.add("fts_name", paramtype="std::string", description="name of sensor", default="", configurable=False) exit(gen.generate("ati_force_torque", "ForceTorqueSensor", "FTSConfiguration")) diff --git a/cfg/NodeConfiguration.params b/cfg/NodeConfiguration.params index 28cc2ab..565ee84 100755 --- a/cfg/NodeConfiguration.params +++ b/cfg/NodeConfiguration.params @@ -7,8 +7,5 @@ gen.add("ft_pub_freq", paramtype="double", description="publish frequency", defa gen.add("ft_pull_freq", paramtype="double", description="sensor daa pull frequency", default=800, configurable=False) gen.add("transform_frame", paramtype="std::string", description="reference coordinate system", default="fts_transform_frame", configurable=False) gen.add("sensor_frame", paramtype="std::string", description="sensor coordinate system", default="fts_reference_link", configurable=False) -gen.add("sim_forceX_param", paramtype="int", description="factor for simulation for x axis simulated force", default="0", configurable=True) -gen.add("sim_forceY_param", paramtype="int", description="factor for simulation for y axis simulated force", default="0", configurable=True) -gen.add("sim_torqueZ_param", paramtype="int", description="factor for simulation for z axis simulated torque", default="0", configurable=True) exit(gen.generate("ati_force_torque", "ForceTorqueSensor", "NodeConfiguration")) diff --git a/ros/src/force_torque_sensor.cpp b/ros/src/force_torque_sensor.cpp index 8685280..222599c 100644 --- a/ros/src/force_torque_sensor.cpp +++ b/ros/src/force_torque_sensor.cpp @@ -54,18 +54,36 @@ ****************************************************************/ #include -ForceTorqueSensor::ForceTorqueSensor(ros::NodeHandle& nh) : nh_(nh), CS_params_{nh}, can_params_{nh.getNamespace()+"/CAN"}, FTS_params_{nh.getNamespace()+"/FTS"} , pub_params_{nh.getNamespace()+"/Publish"} , node_params_{nh.getNamespace()+"/Node"} , calibration_params_{nh.getNamespace()+"/Calibration/Offset"} , gravity_params_{nh.getNamespace()+"/GravityCompensation"} +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(); CS_params_.fromParamServer(); can_params_.fromParamServer(); FTS_params_.fromParamServer(); pub_params_.fromParamServer(); node_params_.fromParamServer(); - calibration_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; + m_calibOffset.force.x = calibration_params_.force[0]; + m_calibOffset.force.y = calibration_params_.force[1]; + m_calibOffset.force.z = calibration_params_.force[2]; + m_calibOffset.torque.x = calibration_params_.torque[0]; + m_calibOffset.torque.x = calibration_params_.torque[1]; + m_calibOffset.torque.x = calibration_params_.torque[2]; + bool isAutoInit = false; double nodePullFreq = 0; m_isInitialized = false; @@ -89,26 +107,6 @@ ForceTorqueSensor::ForceTorqueSensor(ros::NodeHandle& nh) : nh_(nh), CS_params_ nodePullFreq=node_params_.ft_pull_freq; sensor_frame_=node_params_.sensor_frame; - 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; - m_calibOffset.force.x = calibration_params_.force[0]; - m_calibOffset.force.y = calibration_params_.force[1]; - m_calibOffset.force.z = calibration_params_.force[2]; - m_calibOffset.torque.x = calibration_params_.torque[0]; - m_calibOffset.torque.x = calibration_params_.torque[1]; - m_calibOffset.torque.x = calibration_params_.torque[2]; - coordinateSystemNMeasurements = CS_params_.n_measurements; coordinateSystemTBetween = CS_params_.T_between_meas; coordinateSystemPushDirection = CS_params_.push_direction; From 9bca0473482d154612cc6f269a87213975e9645e Mon Sep 17 00:00:00 2001 From: Andreea Tulbure Date: Mon, 28 Aug 2017 10:46:39 +0200 Subject: [PATCH 31/53] changes to use unchanged sensor_offsets --- cfg/Calibration.params | 4 ++-- config/sensor_offset.yaml | 5 +++-- ros/scripts/calculate_offsets.py | 9 ++++++--- ros/src/force_torque_sensor.cpp | 22 ++++++++++++++-------- 4 files changed, 25 insertions(+), 15 deletions(-) diff --git a/cfg/Calibration.params b/cfg/Calibration.params index 13dc697..a55960a 100755 --- a/cfg/Calibration.params +++ b/cfg/Calibration.params @@ -5,7 +5,7 @@ gen = ParameterGenerator() gen.add("n_measurements", paramtype="int", description="number of necessary measurements for calibration", default=100, configurable=True) gen.add("T_between_meas", paramtype="double", description="time between two measurements for calibration", default=0.1, configurable=True) gen.add("isStatic", paramtype="bool", description="is static calibration active", default=False, configurable=True) -gen.add("force", paramtype="std::vector", description="calibration_offset_vector_force") -gen.add("torque", paramtype="std::vector", description="calibration_offset_vector_torque") +gen.add("force", paramtype="std::map", description="calibration_offset_vector_force", default={"x":0.0, "y":0.0, "z":0.0}) +gen.add("torque", paramtype="std::map", description="calibration_offset_vector_torque",default={"x":0.0, "y":0.0, "z":0.0}) exit(gen.generate("ati_force_torque", "ForceTorqueSensor", "Calibration")) diff --git a/config/sensor_offset.yaml b/config/sensor_offset.yaml index 0a9c6a8..ee0beef 100644 --- a/config/sensor_offset.yaml +++ b/config/sensor_offset.yaml @@ -1,2 +1,3 @@ -force: [-19.104102666356425, -1.5473841984049037, 0.9332121238751951] -torque: [-0.24642145895240927, 0.0373995095506341, 0.060428603494093] +force: {x: -19.104102666356425, y: -1.5473841984049037, z: 0.9332121238751951} +torque: {x: -0.24642145895240927, y: 0.0373995095506341, z: 0.060428603494093} + diff --git a/ros/scripts/calculate_offsets.py b/ros/scripts/calculate_offsets.py index 6676d66..590e16f 100755 --- a/ros/scripts/calculate_offsets.py +++ b/ros/scripts/calculate_offsets.py @@ -57,9 +57,12 @@ def calculate_sensor_offsets(): measurement.torque.y /= len(poses) measurement.torque.z /= len(poses) - rospy.set_param('/temp/Offset/force', [measurement.force.x, measurement.force.y, measurement.force.z]) - rospy.set_param('/temp/Offset/torque', [measurement.torque.x, measurement.torque.y, measurement.torque.z]) - + rospy.set_param('/temp/Offset/force/x', measurement.force.x) + rospy.set_param('/temp/Offset/force/y', measurement.force.y) + rospy.set_param('/temp/Offset/force/z', measurement.force.z) + rospy.set_param('/temp/Offset/torque/x', measurement.torque.x) + rospy.set_param('/temp/Offset/torque/y', measurement.torque.y) + rospy.set_param('/temp/Offset/torque/z', measurement.torque.z) if store_to_file: call('rosparam dump -v `rospack find ati_force_torque`/config/sensor_offset.yaml /temp/Offset', shell=True) diff --git a/ros/src/force_torque_sensor.cpp b/ros/src/force_torque_sensor.cpp index 222599c..33feab5 100644 --- a/ros/src/force_torque_sensor.cpp +++ b/ros/src/force_torque_sensor.cpp @@ -54,7 +54,7 @@ ****************************************************************/ #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"} +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(); CS_params_.fromParamServer(); @@ -77,13 +77,19 @@ ForceTorqueSensor::ForceTorqueSensor(ros::NodeHandle& nh) : nh_(nh), calibratio } calibrationTBetween=calibration_params_.T_between_meas; m_staticCalibration=calibration_params_.isStatic; - m_calibOffset.force.x = calibration_params_.force[0]; - m_calibOffset.force.y = calibration_params_.force[1]; - m_calibOffset.force.z = calibration_params_.force[2]; - m_calibOffset.torque.x = calibration_params_.torque[0]; - m_calibOffset.torque.x = calibration_params_.torque[1]; - m_calibOffset.torque.x = calibration_params_.torque[2]; - + + 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"]; + bool isAutoInit = false; double nodePullFreq = 0; m_isInitialized = false; From 88a5482e68d1b6daa20604b77356c47dd151b12d Mon Sep 17 00:00:00 2001 From: Andreea Tulbure Date: Thu, 28 Sep 2017 09:56:55 +0200 Subject: [PATCH 32/53] added realtime publisher and working with iirob_led --- CMakeLists.txt | 6 +- cfg/Led.params | 11 +++ cfg/PublishConfiguration.params | 1 + config/sensor_configuration_robot.yaml | 8 ++ description/urdf/fts.gazebo.xacro | 25 ----- package.xml | 2 + .../ati_force_torque/force_torque_sensor.h | 15 ++- ros/src/force_torque_sensor.cpp | 94 +++++++++++++++---- 8 files changed, 116 insertions(+), 46 deletions(-) create mode 100755 cfg/Led.params delete mode 100644 description/urdf/fts.gazebo.xacro diff --git a/CMakeLists.txt b/CMakeLists.txt index 20dd3c2..1be48a1 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -19,6 +19,8 @@ find_package(catkin REQUIRED COMPONENTS tf2_ros trajectory_msgs visualization_msgs + realtime_tools + iirob_led ) find_package(Eigen3) @@ -37,6 +39,7 @@ if (Cpp11CompilerFlag) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") endif() + generate_ros_parameter_files( cfg/CoordinateSystemCalibration.params cfg/CanConfiguration.params @@ -44,6 +47,7 @@ generate_ros_parameter_files( cfg/NodeConfiguration.params cfg/PublishConfiguration.params cfg/Calibration.params + cfg/Led.params ) add_service_files( @@ -64,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 iirob_led DEPENDS Eigen LIBRARIES ${PROJECT_NAME} ) diff --git a/cfg/Led.params b/cfg/Led.params new file mode 100755 index 0000000..2d644b7 --- /dev/null +++ b/cfg/Led.params @@ -0,0 +1,11 @@ +#!/usr/bin/env python +from rosparam_handler.parameter_generator_catkin import * +gen = ParameterGenerator() + +gen.add("port", paramtype="std::string", description="port number for leds", default="/dev/ttyUSB0", configurable=True) +gen.add("ledNum", paramtype="int", description="number of leds to be used", default=0, configurable=True) +gen.add("link", paramtype="std::string", description="link for transformation", default="", configurable=True) +gen.add("maxForce", paramtype="double", description="maximum force", default=50.0) + +exit(gen.generate("ati_force_torque", "ForceTorqueSensor", "Led")) + 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_robot.yaml b/config/sensor_configuration_robot.yaml index 9840c5e..207cdf1 100644 --- a/config/sensor_configuration_robot.yaml +++ b/config/sensor_configuration_robot.yaml @@ -71,3 +71,11 @@ Publish: transformed_data: true gravity_compensated: true threshold_filtered: true + iirob_led: true + +Led: + port: "/dev/ttyUSB0" + ledNum: 384 + link: "led_rectangle_link" + maxForce: 50 + diff --git a/description/urdf/fts.gazebo.xacro b/description/urdf/fts.gazebo.xacro deleted file mode 100644 index 2651fcc..0000000 --- a/description/urdf/fts.gazebo.xacro +++ /dev/null @@ -1,25 +0,0 @@ - - - - - - true - false - - - - - - true - false - - - - - - true - false - - - - diff --git a/package.xml b/package.xml index d069432..30d0f4f 100644 --- a/package.xml +++ b/package.xml @@ -33,6 +33,8 @@ visualization_msgs rosparam_handler dynamic_reconfigure + realtime_tools + iirob_led 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..5b328f7 100644 --- a/ros/include/ati_force_torque/force_torque_sensor.h +++ b/ros/include/ati_force_torque/force_torque_sensor.h @@ -91,6 +91,11 @@ typedef unsigned char uint8_t; #include #include #include +#include + +#include +#include +#include #define PI 3.14159265 @@ -117,6 +122,7 @@ class ForceTorqueSensor ati_force_torque::PublishConfigurationParameters pub_params_; ati_force_torque::NodeConfigurationParameters node_params_; ati_force_torque::CalibrationParameters calibration_params_; + ati_force_torque::LedParameters led_params_; iirob_filters::GravityCompensationParameters gravity_params_; std::string transform_frame_; @@ -145,13 +151,19 @@ 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_; + realtime_tools::RealtimePublisher * iirob_led_pub; + ros::Subscriber led_sub_; + + IIROB_LED_Rectangle *rectangleStrip; + bool is_pub_gravity_compensated_ = false; bool is_pub_threshold_filtered_ = false; bool is_pub_transformed_data_ = false; bool is_pub_sensor_data_ = false; bool is_pub_low_pass_ = false; bool is_pub_moving_mean_ = false; + bool is_pub_iirob_led_ = false; uint _num_transform_errors; @@ -219,6 +231,7 @@ class ForceTorqueSensor bool useMovinvingMeanTorqueZ= false; bool useGravityCompensator=false; bool useThresholdFilter=false; + bool useiirobLED=false; dynamic_reconfigure::Server reconfigCalibrationSrv_; // Dynamic reconfiguration service dynamic_reconfigure::Server reconfigPublishSrv_; // Dynamic reconfiguration service diff --git a/ros/src/force_torque_sensor.cpp b/ros/src/force_torque_sensor.cpp index 33feab5..567974a 100644 --- a/ros/src/force_torque_sensor.cpp +++ b/ros/src/force_torque_sensor.cpp @@ -54,7 +54,7 @@ ****************************************************************/ #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"} +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"}, led_params_{nh.getNamespace()+"/Led"} { calibration_params_.fromParamServer(); CS_params_.fromParamServer(); @@ -82,7 +82,6 @@ ForceTorqueSensor::ForceTorqueSensor(ros::NodeHandle& nh) : nh_(nh), calibration 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"]; @@ -102,7 +101,7 @@ ForceTorqueSensor::ForceTorqueSensor(ros::NodeHandle& nh) : nh_(nh), calibration 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)); - + // Read data from parameter server canType = can_params_.type; canPath = can_params_.path; @@ -123,27 +122,31 @@ 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); + low_pass_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); + } + is_pub_iirob_led_ = pub_params_.iirob_led; + if(is_pub_iirob_led_){ + iirob_led_pub = new realtime_tools::RealtimePublisher(nh_, "direction_with_force_led", 1); } ftUpdateTimer_ = nh.createTimer(ros::Rate(nodePubFreq), &ForceTorqueSensor::updateFTData, this, false, false); @@ -212,6 +215,12 @@ ForceTorqueSensor::ForceTorqueSensor(ros::NodeHandle& nh) : nh_(nh), calibration threshold_filter_.init(ros::NodeHandle(nh_, "ThresholdFilter")); useThresholdFilter = true; } + + // topic needed for iirob_led + if(nh_.hasParam("iirob_led")) { + useiirobLED = true; + } + if (canType != -1) { @@ -229,9 +238,15 @@ ForceTorqueSensor::ForceTorqueSensor(ros::NodeHandle& nh) : nh_(nh), calibration init_sensor(msg, success); ROS_INFO("Autoinit: %s", msg.c_str()); } -} - + //LEDs config + std::string port = led_params_.port; + int ledNum = led_params_.ledNum; + double maxForce = led_params_.maxForce; + std::string link = led_params_.link; + rectangleStrip = new IIROB_LED_Rectangle(nh_, port, ledNum, maxForce, link); + led_sub_= nh.subscribe("direction_with_force_led", 1, &IIROB_LED_Rectangle::forceCallback, rectangleStrip); +} void ForceTorqueSensor::init_sensor(std::string& msg, bool& success) { @@ -545,11 +560,23 @@ void ForceTorqueSensor::pullFTData(const ros::TimerEvent &event) else moving_mean_filtered_wrench.wrench.torque.z = sensor_data.wrench.torque.z; 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(); + } + //publish(sensor_data); 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(); + } + //low_pass_pub_.publish(low_pass_filtered_data); + if(is_pub_moving_mean_) + if (moving_mean_pub_->trylock()){ + moving_mean_pub_->msg_ = moving_mean_filtered_wrench; + moving_mean_pub_->unlockAndPublish(); + } + //moving_mean_pub_.publish(moving_mean_filtered_wrench); } } @@ -569,13 +596,42 @@ void ForceTorqueSensor::filterFTData(){ 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(); + } + //transformed_data_pub_.publish(transformed_data); 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(); + } + //gravity_compensated_pub_.publish(gravity_compensated_force); 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(); + } + //threshold_filtered_pub_.publish(threshold_filtered_force); + else threshold_filtered_force = moving_mean_filtered_wrench; + } + if(is_pub_iirob_led_ && useiirobLED) + { + iirob_led::DirectionWithForce LEDForceMsg; + LEDForceMsg.force.wrench.force = threshold_filtered_force.wrench.force; + LEDForceMsg.force.wrench.torque = threshold_filtered_force.wrench.torque; + std::cout<<"force msgs led:" << LEDForceMsg.force.wrench.force<trylock()){ + iirob_led_pub->msg_= LEDForceMsg; + iirob_led_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) From 23e52105f1acaca24219ac2001744c728f06843a Mon Sep 17 00:00:00 2001 From: Andreea Tulbure Date: Thu, 28 Sep 2017 12:43:32 +0200 Subject: [PATCH 33/53] changes to work with iirob led --- CMakeLists.txt | 1 - cfg/Led.params | 11 ---------- config/sensor_configuration_robot.yaml | 6 ----- .../ati_force_torque/force_torque_sensor.h | 5 ----- ros/src/force_torque_sensor.cpp | 22 ++++--------------- 5 files changed, 4 insertions(+), 41 deletions(-) delete mode 100755 cfg/Led.params diff --git a/CMakeLists.txt b/CMakeLists.txt index 1be48a1..90181b5 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -47,7 +47,6 @@ generate_ros_parameter_files( cfg/NodeConfiguration.params cfg/PublishConfiguration.params cfg/Calibration.params - cfg/Led.params ) add_service_files( diff --git a/cfg/Led.params b/cfg/Led.params deleted file mode 100755 index 2d644b7..0000000 --- a/cfg/Led.params +++ /dev/null @@ -1,11 +0,0 @@ -#!/usr/bin/env python -from rosparam_handler.parameter_generator_catkin import * -gen = ParameterGenerator() - -gen.add("port", paramtype="std::string", description="port number for leds", default="/dev/ttyUSB0", configurable=True) -gen.add("ledNum", paramtype="int", description="number of leds to be used", default=0, configurable=True) -gen.add("link", paramtype="std::string", description="link for transformation", default="", configurable=True) -gen.add("maxForce", paramtype="double", description="maximum force", default=50.0) - -exit(gen.generate("ati_force_torque", "ForceTorqueSensor", "Led")) - diff --git a/config/sensor_configuration_robot.yaml b/config/sensor_configuration_robot.yaml index 207cdf1..2123ba3 100644 --- a/config/sensor_configuration_robot.yaml +++ b/config/sensor_configuration_robot.yaml @@ -73,9 +73,3 @@ Publish: threshold_filtered: true iirob_led: true -Led: - port: "/dev/ttyUSB0" - ledNum: 384 - link: "led_rectangle_link" - maxForce: 50 - diff --git a/ros/include/ati_force_torque/force_torque_sensor.h b/ros/include/ati_force_torque/force_torque_sensor.h index 5b328f7..0a8bb9a 100644 --- a/ros/include/ati_force_torque/force_torque_sensor.h +++ b/ros/include/ati_force_torque/force_torque_sensor.h @@ -91,10 +91,8 @@ typedef unsigned char uint8_t; #include #include #include -#include #include -#include #include #define PI 3.14159265 @@ -122,7 +120,6 @@ class ForceTorqueSensor ati_force_torque::PublishConfigurationParameters pub_params_; ati_force_torque::NodeConfigurationParameters node_params_; ati_force_torque::CalibrationParameters calibration_params_; - ati_force_torque::LedParameters led_params_; iirob_filters::GravityCompensationParameters gravity_params_; std::string transform_frame_; @@ -155,8 +152,6 @@ class ForceTorqueSensor realtime_tools::RealtimePublisher * iirob_led_pub; ros::Subscriber led_sub_; - IIROB_LED_Rectangle *rectangleStrip; - bool is_pub_gravity_compensated_ = false; bool is_pub_threshold_filtered_ = false; bool is_pub_transformed_data_ = false; diff --git a/ros/src/force_torque_sensor.cpp b/ros/src/force_torque_sensor.cpp index 567974a..4c36159 100644 --- a/ros/src/force_torque_sensor.cpp +++ b/ros/src/force_torque_sensor.cpp @@ -54,7 +54,7 @@ ****************************************************************/ #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"}, led_params_{nh.getNamespace()+"/Led"} +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(); CS_params_.fromParamServer(); @@ -63,7 +63,7 @@ ForceTorqueSensor::ForceTorqueSensor(ros::NodeHandle& nh) : nh_(nh), calibration pub_params_.fromParamServer(); node_params_.fromParamServer(); gravity_params_.fromParamServer(); - + int calibNMeas; calibNMeas=calibration_params_.n_measurements; @@ -146,7 +146,7 @@ ForceTorqueSensor::ForceTorqueSensor(ros::NodeHandle& nh) : nh_(nh), calibration } is_pub_iirob_led_ = pub_params_.iirob_led; if(is_pub_iirob_led_){ - iirob_led_pub = new realtime_tools::RealtimePublisher(nh_, "direction_with_force_led", 1); + iirob_led_pub = new realtime_tools::RealtimePublisher(nh_, "/fts/force_values_transformed", 1); } ftUpdateTimer_ = nh.createTimer(ros::Rate(nodePubFreq), &ForceTorqueSensor::updateFTData, this, false, false); @@ -215,12 +215,6 @@ ForceTorqueSensor::ForceTorqueSensor(ros::NodeHandle& nh) : nh_(nh), calibration threshold_filter_.init(ros::NodeHandle(nh_, "ThresholdFilter")); useThresholdFilter = true; } - - // topic needed for iirob_led - if(nh_.hasParam("iirob_led")) { - useiirobLED = true; - } - if (canType != -1) { @@ -238,14 +232,6 @@ ForceTorqueSensor::ForceTorqueSensor(ros::NodeHandle& nh) : nh_(nh), calibration init_sensor(msg, success); ROS_INFO("Autoinit: %s", msg.c_str()); } - - //LEDs config - std::string port = led_params_.port; - int ledNum = led_params_.ledNum; - double maxForce = led_params_.maxForce; - std::string link = led_params_.link; - rectangleStrip = new IIROB_LED_Rectangle(nh_, port, ledNum, maxForce, link); - led_sub_= nh.subscribe("direction_with_force_led", 1, &IIROB_LED_Rectangle::forceCallback, rectangleStrip); } void ForceTorqueSensor::init_sensor(std::string& msg, bool& success) @@ -615,7 +601,7 @@ void ForceTorqueSensor::filterFTData(){ //threshold_filtered_pub_.publish(threshold_filtered_force); else threshold_filtered_force = moving_mean_filtered_wrench; } - if(is_pub_iirob_led_ && useiirobLED) + if(is_pub_iirob_led_) { iirob_led::DirectionWithForce LEDForceMsg; LEDForceMsg.force.wrench.force = threshold_filtered_force.wrench.force; From 8381779bd493927e4bad279d1ba1b5ba799973ce Mon Sep 17 00:00:00 2001 From: Andreea Tulbure Date: Fri, 29 Sep 2017 14:11:22 +0200 Subject: [PATCH 34/53] some changes to work with rt publisher and iirob_led after test --- .../ati_force_torque/force_torque_sensor.h | 4 +- ros/src/force_torque_sensor.cpp | 41 ++++++++----------- 2 files changed, 19 insertions(+), 26 deletions(-) diff --git a/ros/include/ati_force_torque/force_torque_sensor.h b/ros/include/ati_force_torque/force_torque_sensor.h index 0a8bb9a..5f81be4 100644 --- a/ros/include/ati_force_torque/force_torque_sensor.h +++ b/ros/include/ati_force_torque/force_torque_sensor.h @@ -149,7 +149,7 @@ class ForceTorqueSensor geometry_msgs::TransformStamped transform_ee_base_stamped; tf2_ros::Buffer *p_tfBuffer; realtime_tools::RealtimePublisher *gravity_compensated_pub_, *threshold_filtered_pub_, *transformed_data_pub_, *sensor_data_pub_, *low_pass_pub_, *moving_mean_pub_; - realtime_tools::RealtimePublisher * iirob_led_pub; + realtime_tools::RealtimePublisher *iirob_led_pub; ros::Subscriber led_sub_; bool is_pub_gravity_compensated_ = false; @@ -226,7 +226,7 @@ class ForceTorqueSensor bool useMovinvingMeanTorqueZ= false; bool useGravityCompensator=false; bool useThresholdFilter=false; - bool useiirobLED=false; + dynamic_reconfigure::Server reconfigCalibrationSrv_; // Dynamic reconfiguration service dynamic_reconfigure::Server reconfigPublishSrv_; // Dynamic reconfiguration service diff --git a/ros/src/force_torque_sensor.cpp b/ros/src/force_torque_sensor.cpp index 4c36159..75e08fe 100644 --- a/ros/src/force_torque_sensor.cpp +++ b/ros/src/force_torque_sensor.cpp @@ -284,7 +284,7 @@ void ForceTorqueSensor::init_sensor(std::string& msg, bool& success) success = false; msg = "FTS could not be initilised! :/"; } - ftUpdateTimer_.start(); + ftUpdateTimer_.start(); } } @@ -384,14 +384,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; @@ -404,7 +404,6 @@ 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++) { @@ -422,16 +421,14 @@ geometry_msgs::Wrench ForceTorqueSensor::makeAverageMeasurement(uint number_of_m } 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; @@ -501,11 +498,11 @@ void ForceTorqueSensor::pullFTData(const ros::TimerEvent &event) bool bRet = p_Ftc->ReadSGData(status, sensor_data.wrench.force.x, sensor_data.wrench.force.y, sensor_data.wrench.force.z, sensor_data.wrench.torque.x, sensor_data.wrench.torque.y, sensor_data.wrench.torque.z); + if (bRet != false) { 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; @@ -544,25 +541,24 @@ void ForceTorqueSensor::pullFTData(const ros::TimerEvent &event) 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(is_pub_sensor_data_) if (sensor_data_pub_->trylock()){ sensor_data_pub_->msg_ = sensor_data; sensor_data_pub_->unlockAndPublish(); - } - //publish(sensor_data); + } + if(is_pub_low_pass_) if (low_pass_pub_->trylock()){ low_pass_pub_->msg_ = low_pass_filtered_data; low_pass_pub_->unlockAndPublish(); } - //low_pass_pub_.publish(low_pass_filtered_data); - if(is_pub_moving_mean_) - if (moving_mean_pub_->trylock()){ +/* if(is_pub_moving_mean_) + if (moving_mean_pub_->trylock()){ + std::cout<<"locked"<msg_ = moving_mean_filtered_wrench; moving_mean_pub_->unlockAndPublish(); - } - //moving_mean_pub_.publish(moving_mean_filtered_wrench); + }*/ } } @@ -585,28 +581,25 @@ void ForceTorqueSensor::filterFTData(){ if (transformed_data_pub_->trylock()){ transformed_data_pub_->msg_ = transformed_data; transformed_data_pub_->unlockAndPublish(); - } - //transformed_data_pub_.publish(transformed_data); + } if(is_pub_gravity_compensated_ && useGravityCompensator) if (gravity_compensated_pub_->trylock()){ gravity_compensated_pub_->msg_ = gravity_compensated_force; gravity_compensated_pub_->unlockAndPublish(); - } - //gravity_compensated_pub_.publish(gravity_compensated_force); + } + if(is_pub_threshold_filtered_ && useThresholdFilter) if (threshold_filtered_pub_->trylock()){ threshold_filtered_pub_->msg_ = threshold_filtered_force; threshold_filtered_pub_->unlockAndPublish(); } - //threshold_filtered_pub_.publish(threshold_filtered_force); else threshold_filtered_force = moving_mean_filtered_wrench; } + if(is_pub_iirob_led_) { iirob_led::DirectionWithForce LEDForceMsg; - LEDForceMsg.force.wrench.force = threshold_filtered_force.wrench.force; - LEDForceMsg.force.wrench.torque = threshold_filtered_force.wrench.torque; - std::cout<<"force msgs led:" << LEDForceMsg.force.wrench.force< Date: Thu, 5 Oct 2017 10:41:45 +0200 Subject: [PATCH 35/53] changes for working with iirob filters after changing to plugin --- CMakeLists.txt | 2 ++ .../ati_force_torque/force_torque_sensor.h | 32 +++++++++---------- 2 files changed, 18 insertions(+), 16 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 90181b5..f849e44 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -118,6 +118,8 @@ install(DIRECTORY ros/launch config description DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} ) + + ############# ## Tests ## ############# diff --git a/ros/include/ati_force_torque/force_torque_sensor.h b/ros/include/ati_force_torque/force_torque_sensor.h index 5f81be4..3c56155 100644 --- a/ros/include/ati_force_torque/force_torque_sensor.h +++ b/ros/include/ati_force_torque/force_torque_sensor.h @@ -195,22 +195,22 @@ 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_; + iirob_filters::ThresholdFilter threshold_filter_; + + iirob_filters::LowPassFilter lp_filter_force_x_; + iirob_filters::LowPassFilter lp_filter_force_y_; + iirob_filters::LowPassFilter lp_filter_force_z_; + iirob_filters::LowPassFilter lp_filter_torque_x_; + iirob_filters::LowPassFilter lp_filter_torque_y_; + iirob_filters::LowPassFilter lp_filter_torque_z_; + iirob_filters::MovingMeanFilter moving_mean_filter_force_x_; + iirob_filters::MovingMeanFilter moving_mean_filter_force_y_; + iirob_filters::MovingMeanFilter moving_mean_filter_force_z_; + iirob_filters::MovingMeanFilter moving_mean_filter_torque_x_; + iirob_filters::MovingMeanFilter moving_mean_filter_torque_y_; + iirob_filters::MovingMeanFilter moving_mean_filter_torque_z_; + + iirob_filters::GravityCompensator gravity_compensator_; bool useLowPassFilterForceX=false; bool useLowPassFilterForceY=false; From f8e82f9f2ac59681a771aedc4ea035411f9047e7 Mon Sep 17 00:00:00 2001 From: Andreea Tulbure Date: Thu, 26 Oct 2017 10:47:05 +0200 Subject: [PATCH 36/53] changes in urdf file for correct description when using sensor with sr2 --- config/sensor_configuration.yaml | 2 +- config/sensor_configuration_robot.yaml | 75 ------------------- description/urdf/macros/mini58.urdf.xacro | 8 +- description/urdf/macros/mini58_sim.urdf.xacro | 6 +- 4 files changed, 9 insertions(+), 82 deletions(-) delete mode 100644 config/sensor_configuration_robot.yaml diff --git a/config/sensor_configuration.yaml b/config/sensor_configuration.yaml index a793884..dc664a5 100644 --- a/config/sensor_configuration.yaml +++ b/config/sensor_configuration.yaml @@ -61,7 +61,7 @@ GravityCompensation: #y: 0 #z: 0.027346102 # m #force: 7.9459955 # f_G - world_frame: "world" + world_frame: "base_link" Publish: sensor_data: true diff --git a/config/sensor_configuration_robot.yaml b/config/sensor_configuration_robot.yaml deleted file mode 100644 index 2123ba3..0000000 --- a/config/sensor_configuration_robot.yaml +++ /dev/null @@ -1,75 +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 - iirob_led: true - diff --git a/description/urdf/macros/mini58.urdf.xacro b/description/urdf/macros/mini58.urdf.xacro index bc0a4df..bef0ab7 100644 --- a/description/urdf/macros/mini58.urdf.xacro +++ b/description/urdf/macros/mini58.urdf.xacro @@ -31,17 +31,17 @@ - + - - + + diff --git a/description/urdf/macros/mini58_sim.urdf.xacro b/description/urdf/macros/mini58_sim.urdf.xacro index f1ac709..4913c72 100644 --- a/description/urdf/macros/mini58_sim.urdf.xacro +++ b/description/urdf/macros/mini58_sim.urdf.xacro @@ -1,5 +1,8 @@ + + + @@ -47,11 +50,10 @@ - + - From eaff13ed7a5a069b3dc5760748f9c2217dad2763 Mon Sep 17 00:00:00 2001 From: Andreea Tulbure Date: Fri, 27 Oct 2017 08:29:27 +0200 Subject: [PATCH 37/53] changes to work with filters pluginlib --- .../ati_force_torque/force_torque_sensor.h | 32 +++++++++---------- 1 file changed, 16 insertions(+), 16 deletions(-) diff --git a/ros/include/ati_force_torque/force_torque_sensor.h b/ros/include/ati_force_torque/force_torque_sensor.h index 5f81be4..3c56155 100644 --- a/ros/include/ati_force_torque/force_torque_sensor.h +++ b/ros/include/ati_force_torque/force_torque_sensor.h @@ -195,22 +195,22 @@ 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_; + iirob_filters::ThresholdFilter threshold_filter_; + + iirob_filters::LowPassFilter lp_filter_force_x_; + iirob_filters::LowPassFilter lp_filter_force_y_; + iirob_filters::LowPassFilter lp_filter_force_z_; + iirob_filters::LowPassFilter lp_filter_torque_x_; + iirob_filters::LowPassFilter lp_filter_torque_y_; + iirob_filters::LowPassFilter lp_filter_torque_z_; + iirob_filters::MovingMeanFilter moving_mean_filter_force_x_; + iirob_filters::MovingMeanFilter moving_mean_filter_force_y_; + iirob_filters::MovingMeanFilter moving_mean_filter_force_z_; + iirob_filters::MovingMeanFilter moving_mean_filter_torque_x_; + iirob_filters::MovingMeanFilter moving_mean_filter_torque_y_; + iirob_filters::MovingMeanFilter moving_mean_filter_torque_z_; + + iirob_filters::GravityCompensator gravity_compensator_; bool useLowPassFilterForceX=false; bool useLowPassFilterForceY=false; From 102a783736559eb01ce81d8a9a66a578748f76e5 Mon Sep 17 00:00:00 2001 From: Andreea Tulbure Date: Fri, 27 Oct 2017 10:00:36 +0200 Subject: [PATCH 38/53] moved ati ft for sr2 to sr2_bringup --- description/urdf/macros/mini58.urdf.xacro | 8 ++++---- description/urdf/macros/mini58_sim.urdf.xacro | 3 ++- description/urdf/sensor_mini58.urdf.xacro | 11 ++++++++--- ros/launch/driver.launch | 3 ++- ros/launch/driver_sim.launch | 4 +--- 5 files changed, 17 insertions(+), 12 deletions(-) diff --git a/description/urdf/macros/mini58.urdf.xacro b/description/urdf/macros/mini58.urdf.xacro index bef0ab7..edeb27f 100644 --- a/description/urdf/macros/mini58.urdf.xacro +++ b/description/urdf/macros/mini58.urdf.xacro @@ -31,18 +31,18 @@ - + - + - + diff --git a/description/urdf/macros/mini58_sim.urdf.xacro b/description/urdf/macros/mini58_sim.urdf.xacro index 4913c72..f6f1b10 100644 --- a/description/urdf/macros/mini58_sim.urdf.xacro +++ b/description/urdf/macros/mini58_sim.urdf.xacro @@ -50,10 +50,11 @@ - + + diff --git a/description/urdf/sensor_mini58.urdf.xacro b/description/urdf/sensor_mini58.urdf.xacro index 14ec4b6..ca60205 100644 --- a/description/urdf/sensor_mini58.urdf.xacro +++ b/description/urdf/sensor_mini58.urdf.xacro @@ -1,9 +1,14 @@ - + - + + + + + + + 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/launch/driver_sim.launch b/ros/launch/driver_sim.launch index 8df9a36..19f1070 100644 --- a/ros/launch/driver_sim.launch +++ b/ros/launch/driver_sim.launch @@ -30,9 +30,7 @@ - - - + From d715e1c439516597b49023807e20db262dc779a2 Mon Sep 17 00:00:00 2001 From: Andreea Tulbure Date: Fri, 27 Oct 2017 10:33:07 +0200 Subject: [PATCH 39/53] undone small change in urdf --- description/urdf/macros/mini58.urdf.xacro | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/description/urdf/macros/mini58.urdf.xacro b/description/urdf/macros/mini58.urdf.xacro index edeb27f..bc0a4df 100644 --- a/description/urdf/macros/mini58.urdf.xacro +++ b/description/urdf/macros/mini58.urdf.xacro @@ -41,8 +41,8 @@ - - + + From 9c05cd049e3843d32ea35df6dc1eba25b0b2d94c Mon Sep 17 00:00:00 2001 From: Andreea Tulbure Date: Fri, 27 Oct 2017 10:46:40 +0200 Subject: [PATCH 40/53] small change for sensor simulation --- description/urdf/sensor_mini58.urdf.xacro | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) diff --git a/description/urdf/sensor_mini58.urdf.xacro b/description/urdf/sensor_mini58.urdf.xacro index ca60205..b95bc13 100644 --- a/description/urdf/sensor_mini58.urdf.xacro +++ b/description/urdf/sensor_mini58.urdf.xacro @@ -5,6 +5,16 @@ + + + + + + + + + + @@ -13,7 +23,7 @@ - + From af782261fe981ee9e6cc0defe1472f6a3a50b302 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Wed, 8 Nov 2017 23:07:10 +0100 Subject: [PATCH 41/53] Update package.xml --- package.xml | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/package.xml b/package.xml index 08db715..4005a84 100644 --- a/package.xml +++ b/package.xml @@ -37,10 +37,11 @@ tf2_geometry_msgs trajectory_msgs visualization_msgs - rosparam_handler - dynamic_reconfigure realtime_tools iirob_led + + rosparam_handler + dynamic_reconfigure message_runtime robot_state_publisher From 24bc24b7a799cc8e97d8786ac22c5e5d6319f403 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Wed, 8 Nov 2017 23:24:34 +0100 Subject: [PATCH 42/53] Update package.xml --- package.xml | 3 --- 1 file changed, 3 deletions(-) diff --git a/package.xml b/package.xml index 4005a84..a6097ff 100644 --- a/package.xml +++ b/package.xml @@ -39,9 +39,6 @@ visualization_msgs realtime_tools iirob_led - - rosparam_handler - dynamic_reconfigure message_runtime robot_state_publisher From 7048059354b84c8aa7ae1d5325b4954c491b64e6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Thu, 16 Nov 2017 15:22:04 +0100 Subject: [PATCH 43/53] Removed iirob_led dependency. --- CMakeLists.txt | 3 +-- package.xml | 1 - .../ati_force_torque/force_torque_sensor.h | 3 --- ros/src/force_torque_sensor.cpp | 27 ++----------------- 4 files changed, 3 insertions(+), 31 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index cf12821..d912f9a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -22,7 +22,6 @@ find_package(catkin REQUIRED COMPONENTS trajectory_msgs visualization_msgs realtime_tools - iirob_led ) find_package(Eigen3) @@ -68,7 +67,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 realtime_tools iirob_led + 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 DEPENDS Eigen LIBRARIES ${PROJECT_NAME} ) diff --git a/package.xml b/package.xml index a6097ff..efed52c 100644 --- a/package.xml +++ b/package.xml @@ -38,7 +38,6 @@ trajectory_msgs visualization_msgs realtime_tools - iirob_led 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 879399c..4ed6ac2 100644 --- a/ros/include/ati_force_torque/force_torque_sensor.h +++ b/ros/include/ati_force_torque/force_torque_sensor.h @@ -93,7 +93,6 @@ typedef unsigned char uint8_t; #include #include -#include #define PI 3.14159265 @@ -149,7 +148,6 @@ class ForceTorqueSensor geometry_msgs::TransformStamped transform_ee_base_stamped; tf2_ros::Buffer *p_tfBuffer; realtime_tools::RealtimePublisher *gravity_compensated_pub_, *threshold_filtered_pub_, *transformed_data_pub_, *sensor_data_pub_, *low_pass_pub_, *moving_mean_pub_; - realtime_tools::RealtimePublisher *iirob_led_pub; ros::Subscriber led_sub_; bool is_pub_gravity_compensated_ = false; @@ -158,7 +156,6 @@ class ForceTorqueSensor bool is_pub_sensor_data_ = false; bool is_pub_low_pass_ = false; bool is_pub_moving_mean_ = false; - bool is_pub_iirob_led_ = false; uint _num_transform_errors; diff --git a/ros/src/force_torque_sensor.cpp b/ros/src/force_torque_sensor.cpp index 0f1a3e9..6717215 100644 --- a/ros/src/force_torque_sensor.cpp +++ b/ros/src/force_torque_sensor.cpp @@ -144,10 +144,6 @@ ForceTorqueSensor::ForceTorqueSensor(ros::NodeHandle& nh) : nh_(nh), calibration if(is_pub_transformed_data_){ transformed_data_pub_ = new realtime_tools::RealtimePublisher(nh_, "transformed_data", 1); } - is_pub_iirob_led_ = pub_params_.iirob_led; - if(is_pub_iirob_led_){ - iirob_led_pub = new realtime_tools::RealtimePublisher(nh_, "/fts/force_values_transformed", 1); - } ftUpdateTimer_ = nh.createTimer(ros::Rate(nodePubFreq), &ForceTorqueSensor::updateFTData, this, false, false); ftPullTimer_ = nh.createTimer(ros::Rate(nodePullFreq), &ForceTorqueSensor::pullFTData, this, false, false); @@ -553,18 +549,17 @@ void ForceTorqueSensor::pullFTData(const ros::TimerEvent &event) low_pass_pub_->msg_ = low_pass_filtered_data; low_pass_pub_->unlockAndPublish(); } -/* if(is_pub_moving_mean_) + if(is_pub_moving_mean_) if (moving_mean_pub_->trylock()){ std::cout<<"locked"<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)) @@ -593,25 +588,7 @@ void ForceTorqueSensor::filterFTData(){ threshold_filtered_pub_->msg_ = threshold_filtered_force; threshold_filtered_pub_->unlockAndPublish(); } - else threshold_filtered_force = moving_mean_filtered_wrench; - } - - if(is_pub_iirob_led_) - { - iirob_led::DirectionWithForce LEDForceMsg; - LEDForceMsg.force.wrench = threshold_filtered_force.wrench; - std_msgs::ColorRGBA LEDColorMsg; - LEDColorMsg.r = 255; - LEDColorMsg.g = 0; - LEDColorMsg.b = 0; - LEDColorMsg.a = 1; - LEDForceMsg.color = LEDColorMsg; - if(iirob_led_pub->trylock()){ - iirob_led_pub->msg_= LEDForceMsg; - iirob_led_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) From ce643a81d5b6f429ffc3a5fbbfa110cf363dd9ab Mon Sep 17 00:00:00 2001 From: Andreea Tulbure Date: Fri, 17 Nov 2017 10:30:11 +0100 Subject: [PATCH 44/53] working branch with iirob_filters --- CMakeLists.txt | 3 +- config/sensor_configuration.yaml | 160 ++++++++++++---- package.xml | 1 + .../ati_force_torque/force_torque_sensor.h | 42 +--- ros/src/force_torque_sensor.cpp | 180 ++++++++---------- 5 files changed, 217 insertions(+), 169 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index f849e44..b90fcc1 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -21,6 +21,7 @@ find_package(catkin REQUIRED COMPONENTS visualization_msgs realtime_tools iirob_led + filters ) find_package(Eigen3) @@ -67,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 realtime_tools iirob_led + 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 iirob_led filters DEPENDS Eigen LIBRARIES ${PROJECT_NAME} ) diff --git a/config/sensor_configuration.yaml b/config/sensor_configuration.yaml index dc664a5..b9fe295 100644 --- a/config/sensor_configuration.yaml +++ b/config/sensor_configuration.yaml @@ -12,56 +12,138 @@ Calibration: 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/MultiChannelThresholdFilter + 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 + filter_chain: + - + name: Force_x + type: iirob_filters/MultiChannelLowPassFilter + params: {SamplingFrequency: 200.0, DampingFrequency: 15.0, DampingIntensity: -6.0} + - + name: Force_y + type: iirob_filters/MultiChannelLowPassFilter + params: {SamplingFrequency: 200.0, DampingFrequency: 15.0, DampingIntensity: -6.0} + - + name: Force_z + type: iirob_filters/MultiChannelLowPassFilter + params: {SamplingFrequency: 200.0, DampingFrequency: 15.0, DampingIntensity: -6.0} + - + name: Torque_x + type: iirob_filters/MultiChannelLowPassFilter + params: {SamplingFrequency: 200.0, DampingFrequency: 15.0, DampingIntensity: -6.0} + - + name: Torque_y + type: iirob_filters/MultiChannelLowPassFilter + params: {SamplingFrequency: 200.0, DampingFrequency: 15.0, DampingIntensity: -6.0} + - + name: Torque_z + type: iirob_filters/MultiChannelLowPassFilter + params: {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 + filter_chain: + - + name: Force_x + type: filters/MultiChannelMeanFilterDouble + params: {number_of_observations: 4} + - + name: Force_y + type: filters/MultiChannelMeanFilterDouble + params: {number_of_observations: 4} + - + name: Force_z + type: filters/MultiChannelMeanFilterDouble + params: {number_of_observations: 4} + - + name: Torque_x + type: filters/MultiChannelMeanFilterDouble + params: {number_of_observations: 4} + - + name: Torque_y + type: filters/MultiChannelMeanFilterDouble + params: {number_of_observations: 4} + - + name: Torque_z + type: filters/MultiChannelMeanFilterDouble + params: {number_of_observations: 4} +# name: Force_x +# type: MultiChannelMeanFilterDouble +# params: +# number_of_observations: 4 +# name: Force_y +# type: MultiChannelMeanFilterDouble +# params: +# number_of_observations: 4 +# name: Force_z +# type: MultiChannelMeanFilterDouble +# params: +# number_of_observations: 4 +# name: Toque_x +# type: MultiChannelMeanFilterDouble +# params: +# number_of_observations: 4 +# name: Toque_y +# type: MultiChannelMeanFilterDouble +# params: +# number_of_observations: 4 +# name: Toque_z +# type: MultiChannelMeanFilterDouble +# params: +# number_of_observations: 4 + + #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: + - + name: GravityCompensation + type: iirob_filters/MultiChannelGravityCompensator + params: {world_frame: "base_link"} + +#GravityCompensation: #CoG: #x: 0 #y: 0 #z: 0.027346102 # m #force: 7.9459955 # f_G - world_frame: "base_link" + # world_frame: "base_link" Publish: sensor_data: true diff --git a/package.xml b/package.xml index 30d0f4f..4acca2f 100644 --- a/package.xml +++ b/package.xml @@ -35,6 +35,7 @@ dynamic_reconfigure realtime_tools iirob_led + 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 3c56155..9828bdf 100644 --- a/ros/include/ati_force_torque/force_torque_sensor.h +++ b/ros/include/ati_force_torque/force_torque_sensor.h @@ -76,7 +76,7 @@ typedef unsigned char uint8_t; #include #include #include -#include +//#include #include #include @@ -92,6 +92,7 @@ typedef unsigned char uint8_t; #include #include +#include #include #include @@ -194,39 +195,16 @@ class ForceTorqueSensor // Variables for Static offset bool m_staticCalibration; geometry_msgs::Wrench m_calibOffset; - - iirob_filters::ThresholdFilter threshold_filter_; - - iirob_filters::LowPassFilter lp_filter_force_x_; - iirob_filters::LowPassFilter lp_filter_force_y_; - iirob_filters::LowPassFilter lp_filter_force_z_; - iirob_filters::LowPassFilter lp_filter_torque_x_; - iirob_filters::LowPassFilter lp_filter_torque_y_; - iirob_filters::LowPassFilter lp_filter_torque_z_; - iirob_filters::MovingMeanFilter moving_mean_filter_force_x_; - iirob_filters::MovingMeanFilter moving_mean_filter_force_y_; - iirob_filters::MovingMeanFilter moving_mean_filter_force_z_; - iirob_filters::MovingMeanFilter moving_mean_filter_torque_x_; - iirob_filters::MovingMeanFilter moving_mean_filter_torque_y_; - iirob_filters::MovingMeanFilter moving_mean_filter_torque_z_; - - iirob_filters::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; + + filters::MultiChannelFilterChain chain_moving_mean_ ; + filters::MultiChannelFilterChain chain_low_pass_; + filters::MultiChannelFilterChain threshold_filters_; + filters::MultiChannelFilterChain 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 diff --git a/ros/src/force_torque_sensor.cpp b/ros/src/force_torque_sensor.cpp index 75e08fe..4469461 100644 --- a/ros/src/force_torque_sensor.cpp +++ b/ros/src/force_torque_sensor.cpp @@ -54,7 +54,7 @@ ****************************************************************/ #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"} +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"}, chain_moving_mean_("double"), chain_low_pass_("double"),threshold_filters_("double"), gravity_compensator_("double") { calibration_params_.fromParamServer(); CS_params_.fromParamServer(); @@ -99,8 +99,8 @@ 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)); + //reconfigPublishSrv_.setCallback(boost::bind(&ForceTorqueSensor::reconfigurePublishRequest, this, _1, _2)); // Read data from parameter server canType = can_params_.type; @@ -130,7 +130,7 @@ ForceTorqueSensor::ForceTorqueSensor(ros::NodeHandle& nh) : nh_(nh), calibration } is_pub_moving_mean_=pub_params_.moving_mean; if(is_pub_moving_mean_){ - low_pass_pub_ = new realtime_tools::RealtimePublisher(nh_, "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_){ @@ -151,68 +151,29 @@ ForceTorqueSensor::ForceTorqueSensor(ros::NodeHandle& nh) : nh_(nh), calibration 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; - } - + + chain_moving_mean_.configure(6,"MovingMeanFilter/filter_chain"); + chain_low_pass_.configure(6,"LowPassFilter/filter_chain"); + gravity_compensator_.configure(6,"GravityCompensation"); + threshold_filters_.configure(6,"ThresholdFilter"); + //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")) { + useMovingMean = true; } - 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/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; } @@ -408,16 +369,17 @@ geometry_msgs::Wrench ForceTorqueSensor::makeAverageMeasurement(uint number_of_m for (int i = 0; i < number_of_measurements; i++) { geometry_msgs::Wrench output; + //std::cout<<"frame id"<< frame_id< 200){ return measurement; } i--; continue; - } + } } else { @@ -498,7 +460,6 @@ void ForceTorqueSensor::pullFTData(const ros::TimerEvent &event) bool bRet = p_Ftc->ReadSGData(status, sensor_data.wrench.force.x, sensor_data.wrench.force.y, sensor_data.wrench.force.z, sensor_data.wrench.torque.x, sensor_data.wrench.torque.y, sensor_data.wrench.torque.z); - if (bRet != false) { sensor_data.header.stamp = ros::Time::now(); @@ -511,37 +472,37 @@ void ForceTorqueSensor::pullFTData(const ros::TimerEvent &event) sensor_data.wrench.torque.y -= offset_.torque.y; 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 - 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; + //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}; + chain_low_pass_.update(in_data,out_data); + low_pass_filtered_data.wrench.force.x = out_data.at(0); + low_pass_filtered_data.wrench.force.y = out_data.at(1); + low_pass_filtered_data.wrench.force.z = out_data.at(2); + low_pass_filtered_data.wrench.torque.x = out_data.at(3); + low_pass_filtered_data.wrench.torque.y = out_data.at(4); + low_pass_filtered_data.wrench.torque.z = out_data.at(5); + } + else low_pass_filtered_data = sensor_data; + //moving_mean + moving_mean_filtered_wrench.header = low_pass_filtered_data.header; + 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}; + + chain_moving_mean_.update(in_data,out_data); + moving_mean_filtered_wrench.wrench.force.x = out_data.at(0); + moving_mean_filtered_wrench.wrench.force.y = out_data.at(1); + moving_mean_filtered_wrench.wrench.force.z = out_data.at(2); + moving_mean_filtered_wrench.wrench.torque.x = out_data.at(3); + moving_mean_filtered_wrench.wrench.torque.y = out_data.at(4); + moving_mean_filtered_wrench.wrench.torque.z = out_data.at(5); + } + else moving_mean_filtered_wrench = low_pass_filtered_data; + if(is_pub_sensor_data_) if (sensor_data_pub_->trylock()){ sensor_data_pub_->msg_ = sensor_data; @@ -553,12 +514,12 @@ void ForceTorqueSensor::pullFTData(const ros::TimerEvent &event) low_pass_pub_->msg_ = low_pass_filtered_data; low_pass_pub_->unlockAndPublish(); } -/* if(is_pub_moving_mean_) + + if(is_pub_moving_mean_) if (moving_mean_pub_->trylock()){ - std::cout<<"locked"<msg_ = moving_mean_filtered_wrench; moving_mean_pub_->unlockAndPublish(); - }*/ + } } } @@ -570,11 +531,36 @@ void ForceTorqueSensor::filterFTData(){ 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_.setFrame(transformed_data.header.frame_id); + gravity_compensator_.update(in_data, out_data); + gravity_compensated_force.wrench.force.x = out_data.at(0); + gravity_compensated_force.wrench.force.y = out_data.at(1); + gravity_compensated_force.wrench.force.z = out_data.at(2); + gravity_compensated_force.wrench.torque.x = out_data.at(3); + gravity_compensated_force.wrench.torque.y = out_data.at(4); + gravity_compensated_force.wrench.torque.z = out_data.at(5); + gravity_compensated_force.header = transformed_data.header; + } 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_filters_.update(in_data, out_data); + threshold_filtered_force.wrench.force.x = out_data.at(0); + threshold_filtered_force.wrench.force.y = out_data.at(1); + threshold_filtered_force.wrench.force.z = out_data.at(2); + threshold_filtered_force.wrench.torque.x = out_data.at(3); + threshold_filtered_force.wrench.torque.y = out_data.at(4); + threshold_filtered_force.wrench.torque.z = out_data.at(5); + threshold_filtered_force.header = gravity_compensated_force.header; + } else threshold_filtered_force = gravity_compensated_force; if(is_pub_transformed_data_) @@ -642,11 +628,11 @@ 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){ pub_params_.fromConfig(config); -} +}*/ From f99553744d148779d8f8ad0d996e1581776119c2 Mon Sep 17 00:00:00 2001 From: Andreea Tulbure Date: Fri, 17 Nov 2017 12:38:58 +0100 Subject: [PATCH 45/53] changes to work with wrench stamped as template --- config/sensor_configuration.yaml | 138 ++---------------- .../ati_force_torque/force_torque_sensor.h | 11 +- ros/src/force_torque_sensor.cpp | 39 ++--- 3 files changed, 27 insertions(+), 161 deletions(-) diff --git a/config/sensor_configuration.yaml b/config/sensor_configuration.yaml index b9fe295..9d4a596 100644 --- a/config/sensor_configuration.yaml +++ b/config/sensor_configuration.yaml @@ -12,138 +12,24 @@ Calibration: isStatic: false # true= use static offset Calibration, false=use calibrate Sensor to 0 in current State ThresholdFilter: - - - name: ThresholdFilter - type: iirob_filters/MultiChannelThresholdFilter - params: {linear_threshold: 2.5, angular_threshold: 0.3} + name: ThresholdFilter + type: iirob_filters/ThresholdFilterWrench + params: {linear_threshold: 2.5, angular_threshold: 0.3} LowPassFilter: - filter_chain: - - - name: Force_x - type: iirob_filters/MultiChannelLowPassFilter - params: {SamplingFrequency: 200.0, DampingFrequency: 15.0, DampingIntensity: -6.0} - - - name: Force_y - type: iirob_filters/MultiChannelLowPassFilter - params: {SamplingFrequency: 200.0, DampingFrequency: 15.0, DampingIntensity: -6.0} - - - name: Force_z - type: iirob_filters/MultiChannelLowPassFilter - params: {SamplingFrequency: 200.0, DampingFrequency: 15.0, DampingIntensity: -6.0} - - - name: Torque_x - type: iirob_filters/MultiChannelLowPassFilter - params: {SamplingFrequency: 200.0, DampingFrequency: 15.0, DampingIntensity: -6.0} - - - name: Torque_y - type: iirob_filters/MultiChannelLowPassFilter - params: {SamplingFrequency: 200.0, DampingFrequency: 15.0, DampingIntensity: -6.0} - - - name: Torque_z - type: iirob_filters/MultiChannelLowPassFilter - params: {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: - filter_chain: - - - name: Force_x - type: filters/MultiChannelMeanFilterDouble - params: {number_of_observations: 4} - - - name: Force_y - type: filters/MultiChannelMeanFilterDouble - params: {number_of_observations: 4} - - - name: Force_z - type: filters/MultiChannelMeanFilterDouble - params: {number_of_observations: 4} - - - name: Torque_x - type: filters/MultiChannelMeanFilterDouble - params: {number_of_observations: 4} - - - name: Torque_y - type: filters/MultiChannelMeanFilterDouble - params: {number_of_observations: 4} - - - name: Torque_z - type: filters/MultiChannelMeanFilterDouble - params: {number_of_observations: 4} + name: MovingMeanFilter + type: filters/MultiChannelMeanFilterDouble + params: {number_of_observations: 4} -# name: Force_x -# type: MultiChannelMeanFilterDouble -# params: -# number_of_observations: 4 -# name: Force_y -# type: MultiChannelMeanFilterDouble -# params: -# number_of_observations: 4 -# name: Force_z -# type: MultiChannelMeanFilterDouble -# params: -# number_of_observations: 4 -# name: Toque_x -# type: MultiChannelMeanFilterDouble -# params: -# number_of_observations: 4 -# name: Toque_y -# type: MultiChannelMeanFilterDouble -# params: -# number_of_observations: 4 -# name: Toque_z -# type: MultiChannelMeanFilterDouble -# params: -# number_of_observations: 4 - - #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: - - - name: GravityCompensation - type: iirob_filters/MultiChannelGravityCompensator - params: {world_frame: "base_link"} - -#GravityCompensation: - #CoG: - #x: 0 - #y: 0 - #z: 0.027346102 # m - #force: 7.9459955 # f_G - # world_frame: "base_link" + name: GravityCompensationWrench + type: iirob_filters/GravityCompensator + params: {world_frame: "base_link"} Publish: sensor_data: true diff --git a/ros/include/ati_force_torque/force_torque_sensor.h b/ros/include/ati_force_torque/force_torque_sensor.h index 67304f8..8fc65af 100644 --- a/ros/include/ati_force_torque/force_torque_sensor.h +++ b/ros/include/ati_force_torque/force_torque_sensor.h @@ -76,7 +76,6 @@ typedef unsigned char uint8_t; #include #include #include -//#include #include #include @@ -93,6 +92,8 @@ typedef unsigned char uint8_t; #include #include +#include +#include #include #define PI 3.14159265 @@ -192,10 +193,10 @@ class ForceTorqueSensor bool m_staticCalibration; geometry_msgs::Wrench m_calibOffset; - filters::MultiChannelFilterChain chain_moving_mean_ ; - filters::MultiChannelFilterChain chain_low_pass_; - filters::MultiChannelFilterChain threshold_filters_; - filters::MultiChannelFilterChain gravity_compensator_; + filters::MultiChannelFilterBase *chain_moving_mean_ = new filters::MultiChannelMeanFilter(); + filters::FilterBase *chain_low_pass_ = new iirob_filters::LowPassFilter(); + filters::FilterBase *threshold_filters_ = new iirob_filters::ThresholdFilter(); + filters::FilterBase *gravity_compensator_ = new iirob_filters::GravityCompensator(); bool useGravityCompensator=false; bool useThresholdFilter=false; diff --git a/ros/src/force_torque_sensor.cpp b/ros/src/force_torque_sensor.cpp index 05566ca..8893005 100644 --- a/ros/src/force_torque_sensor.cpp +++ b/ros/src/force_torque_sensor.cpp @@ -54,7 +54,7 @@ ****************************************************************/ #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"}, chain_moving_mean_("double"), chain_low_pass_("double"),threshold_filters_("double"), gravity_compensator_("double") +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(); CS_params_.fromParamServer(); @@ -149,10 +149,10 @@ ForceTorqueSensor::ForceTorqueSensor(ros::NodeHandle& nh) : nh_(nh), calibration ftUpdateTimer_ = nh.createTimer(ros::Rate(nodePubFreq), &ForceTorqueSensor::updateFTData, this, false, false); ftPullTimer_ = nh.createTimer(ros::Rate(nodePullFreq), &ForceTorqueSensor::pullFTData, this, false, false); - chain_moving_mean_.configure(6,"MovingMeanFilter/filter_chain"); - chain_low_pass_.configure(6,"LowPassFilter/filter_chain"); - gravity_compensator_.configure(6,"GravityCompensation"); - threshold_filters_.configure(6,"ThresholdFilter"); + chain_moving_mean_->configure(6,"MovingMeanFilter"); + chain_low_pass_->configure("LowPassFilter"); + gravity_compensator_->configure("GravityCompensation"); + threshold_filters_->configure("ThresholdFilter"); //Median Filter if(nh_.hasParam("MovingMeanFilter")) { @@ -476,13 +476,7 @@ void ForceTorqueSensor::pullFTData(const ros::TimerEvent &event) 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}; - chain_low_pass_.update(in_data,out_data); - low_pass_filtered_data.wrench.force.x = out_data.at(0); - low_pass_filtered_data.wrench.force.y = out_data.at(1); - low_pass_filtered_data.wrench.force.z = out_data.at(2); - low_pass_filtered_data.wrench.torque.x = out_data.at(3); - low_pass_filtered_data.wrench.torque.y = out_data.at(4); - low_pass_filtered_data.wrench.torque.z = out_data.at(5); + chain_low_pass_->update(sensor_data,low_pass_filtered_data); } else low_pass_filtered_data = sensor_data; //moving_mean @@ -491,7 +485,7 @@ void ForceTorqueSensor::pullFTData(const ros::TimerEvent &event) 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}; - chain_moving_mean_.update(in_data,out_data); + chain_moving_mean_->update(in_data,out_data); moving_mean_filtered_wrench.wrench.force.x = out_data.at(0); moving_mean_filtered_wrench.wrench.force.y = out_data.at(1); moving_mean_filtered_wrench.wrench.force.z = out_data.at(2); @@ -532,15 +526,7 @@ void ForceTorqueSensor::filterFTData(){ { 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_.setFrame(transformed_data.header.frame_id); - gravity_compensator_.update(in_data, out_data); - gravity_compensated_force.wrench.force.x = out_data.at(0); - gravity_compensated_force.wrench.force.y = out_data.at(1); - gravity_compensated_force.wrench.force.z = out_data.at(2); - gravity_compensated_force.wrench.torque.x = out_data.at(3); - gravity_compensated_force.wrench.torque.y = out_data.at(4); - gravity_compensated_force.wrench.torque.z = out_data.at(5); - gravity_compensated_force.header = transformed_data.header; + gravity_compensator_->update(transformed_data, gravity_compensated_force); } else gravity_compensated_force = transformed_data; @@ -549,14 +535,7 @@ void ForceTorqueSensor::filterFTData(){ { 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_filters_.update(in_data, out_data); - threshold_filtered_force.wrench.force.x = out_data.at(0); - threshold_filtered_force.wrench.force.y = out_data.at(1); - threshold_filtered_force.wrench.force.z = out_data.at(2); - threshold_filtered_force.wrench.torque.x = out_data.at(3); - threshold_filtered_force.wrench.torque.y = out_data.at(4); - threshold_filtered_force.wrench.torque.z = out_data.at(5); - threshold_filtered_force.header = gravity_compensated_force.header; + threshold_filters_->update(gravity_compensated_force, threshold_filtered_force); } else threshold_filtered_force = gravity_compensated_force; From 2e5c378a1f48312920040757c3e3090e7077cac8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Fri, 17 Nov 2017 14:32:36 +0100 Subject: [PATCH 46/53] Update .travis.rosinstall --- .travis.rosinstall | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 From 9373669d94492444d99571966e77ddfb8ad22fe1 Mon Sep 17 00:00:00 2001 From: Andreea Tulbure Date: Mon, 20 Nov 2017 10:28:59 +0100 Subject: [PATCH 47/53] changes to work with iirob_filters moving mean filter --- config/sensor_configuration.yaml | 4 ++-- .../ati_force_torque/force_torque_sensor.h | 8 +++++--- ros/src/force_torque_sensor.cpp | 18 ++++++------------ 3 files changed, 13 insertions(+), 17 deletions(-) diff --git a/config/sensor_configuration.yaml b/config/sensor_configuration.yaml index 9d4a596..ef270bb 100644 --- a/config/sensor_configuration.yaml +++ b/config/sensor_configuration.yaml @@ -23,8 +23,8 @@ LowPassFilter: MovingMeanFilter: name: MovingMeanFilter - type: filters/MultiChannelMeanFilterDouble - params: {number_of_observations: 4} + type: filters/MovingMeanFilterWrench + params: {divider: 4} GravityCompensation: name: GravityCompensationWrench diff --git a/ros/include/ati_force_torque/force_torque_sensor.h b/ros/include/ati_force_torque/force_torque_sensor.h index 8fc65af..76c830f 100644 --- a/ros/include/ati_force_torque/force_torque_sensor.h +++ b/ros/include/ati_force_torque/force_torque_sensor.h @@ -77,6 +77,7 @@ typedef unsigned char uint8_t; #include #include #include +#include #include #include @@ -193,9 +194,10 @@ class ForceTorqueSensor bool m_staticCalibration; geometry_msgs::Wrench m_calibOffset; - filters::MultiChannelFilterBase *chain_moving_mean_ = new filters::MultiChannelMeanFilter(); - filters::FilterBase *chain_low_pass_ = new iirob_filters::LowPassFilter(); - filters::FilterBase *threshold_filters_ = new iirob_filters::ThresholdFilter(); + //filters::MultiChannelFilterBase *chain_moving_mean_ = new filters::MultiChannelMeanFilter(); + filters::FilterBase *moving_mean_filter_ = new iirob_filters::MovingMeanFilter(); + filters::FilterBase *low_pass_filter_ = new iirob_filters::LowPassFilter(); + filters::FilterBase *threshold_filter_ = new iirob_filters::ThresholdFilter(); filters::FilterBase *gravity_compensator_ = new iirob_filters::GravityCompensator(); bool useGravityCompensator=false; diff --git a/ros/src/force_torque_sensor.cpp b/ros/src/force_torque_sensor.cpp index 8893005..2247193 100644 --- a/ros/src/force_torque_sensor.cpp +++ b/ros/src/force_torque_sensor.cpp @@ -149,10 +149,10 @@ ForceTorqueSensor::ForceTorqueSensor(ros::NodeHandle& nh) : nh_(nh), calibration ftUpdateTimer_ = nh.createTimer(ros::Rate(nodePubFreq), &ForceTorqueSensor::updateFTData, this, false, false); ftPullTimer_ = nh.createTimer(ros::Rate(nodePullFreq), &ForceTorqueSensor::pullFTData, this, false, false); - chain_moving_mean_->configure(6,"MovingMeanFilter"); - chain_low_pass_->configure("LowPassFilter"); + moving_mean_filter_->configure("MovingMeanFilter"); + low_pass_filter_->configure("LowPassFilter"); gravity_compensator_->configure("GravityCompensation"); - threshold_filters_->configure("ThresholdFilter"); + threshold_filter_->configure("ThresholdFilter"); //Median Filter if(nh_.hasParam("MovingMeanFilter")) { @@ -476,7 +476,7 @@ void ForceTorqueSensor::pullFTData(const ros::TimerEvent &event) 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}; - chain_low_pass_->update(sensor_data,low_pass_filtered_data); + low_pass_filter_->update(sensor_data,low_pass_filtered_data); } else low_pass_filtered_data = sensor_data; //moving_mean @@ -485,13 +485,7 @@ void ForceTorqueSensor::pullFTData(const ros::TimerEvent &event) 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}; - chain_moving_mean_->update(in_data,out_data); - moving_mean_filtered_wrench.wrench.force.x = out_data.at(0); - moving_mean_filtered_wrench.wrench.force.y = out_data.at(1); - moving_mean_filtered_wrench.wrench.force.z = out_data.at(2); - moving_mean_filtered_wrench.wrench.torque.x = out_data.at(3); - moving_mean_filtered_wrench.wrench.torque.y = out_data.at(4); - moving_mean_filtered_wrench.wrench.torque.z = out_data.at(5); + moving_mean_filter_->update(low_pass_filtered_data, moving_mean_filtered_wrench); } else moving_mean_filtered_wrench = low_pass_filtered_data; @@ -535,7 +529,7 @@ void ForceTorqueSensor::filterFTData(){ { 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_filters_->update(gravity_compensated_force, threshold_filtered_force); + threshold_filter_->update(gravity_compensated_force, threshold_filtered_force); } else threshold_filtered_force = gravity_compensated_force; From d6235a6f4ec82ab20cdfa8a8fa211512f007b559 Mon Sep 17 00:00:00 2001 From: Andreea Tulbure Date: Mon, 20 Nov 2017 11:13:22 +0100 Subject: [PATCH 48/53] use rosparam handler for gravity compensator --- ros/src/force_torque_sensor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ros/src/force_torque_sensor.cpp b/ros/src/force_torque_sensor.cpp index 2247193..a871c12 100644 --- a/ros/src/force_torque_sensor.cpp +++ b/ros/src/force_torque_sensor.cpp @@ -54,7 +54,7 @@ ****************************************************************/ #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"} +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"} { calibration_params_.fromParamServer(); CS_params_.fromParamServer(); From bd04566782d48eaf2a80c0cab05031107d8fa6e3 Mon Sep 17 00:00:00 2001 From: Andreea Tulbure Date: Wed, 22 Nov 2017 13:17:34 +0100 Subject: [PATCH 49/53] changes to work with filters --- .../ati_force_torque/force_torque_sensor.h | 13 ++---- ros/src/force_torque_sensor.cpp | 46 +++++++++++-------- 2 files changed, 33 insertions(+), 26 deletions(-) diff --git a/ros/include/ati_force_torque/force_torque_sensor.h b/ros/include/ati_force_torque/force_torque_sensor.h index 76c830f..290d27c 100644 --- a/ros/include/ati_force_torque/force_torque_sensor.h +++ b/ros/include/ati_force_torque/force_torque_sensor.h @@ -87,7 +87,6 @@ typedef unsigned char uint8_t; #include #include #include -#include #include #include #include @@ -166,7 +165,7 @@ class ForceTorqueSensor std::string canPath; int canBaudrate; int ftsBaseID; - double nodePubFreq; + double nodePubFreq, nodePullFreq; uint calibrationNMeasurements; double calibrationTBetween; int coordinateSystemNMeasurements; @@ -193,11 +192,11 @@ class ForceTorqueSensor // Variables for Static offset bool m_staticCalibration; geometry_msgs::Wrench m_calibOffset; + - //filters::MultiChannelFilterBase *chain_moving_mean_ = new filters::MultiChannelMeanFilter(); filters::FilterBase *moving_mean_filter_ = new iirob_filters::MovingMeanFilter(); filters::FilterBase *low_pass_filter_ = new iirob_filters::LowPassFilter(); - filters::FilterBase *threshold_filter_ = new iirob_filters::ThresholdFilter(); + filters::FilterBase *threshold_filter_ = new iirob_filters::ThresholdFilter(); filters::FilterBase *gravity_compensator_ = new iirob_filters::GravityCompensator(); bool useGravityCompensator=false; @@ -207,10 +206,8 @@ class ForceTorqueSensor 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/src/force_torque_sensor.cpp b/ros/src/force_torque_sensor.cpp index a871c12..4ee2e0d 100644 --- a/ros/src/force_torque_sensor.cpp +++ b/ros/src/force_torque_sensor.cpp @@ -75,12 +75,12 @@ ForceTorqueSensor::ForceTorqueSensor(ros::NodeHandle& nh) : nh_(nh), calibration else { calibrationNMeasurements = (uint)calibNMeas; } - calibrationTBetween=calibration_params_.T_between_meas; - m_staticCalibration=calibration_params_.isStatic; + 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"]; @@ -90,7 +90,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 +99,17 @@ ForceTorqueSensor::ForceTorqueSensor(ros::NodeHandle& nh) : nh_(nh), calibration 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)); // 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; @@ -148,9 +146,9 @@ ForceTorqueSensor::ForceTorqueSensor(ros::NodeHandle& nh) : nh_(nh), calibration ftUpdateTimer_ = nh.createTimer(ros::Rate(nodePubFreq), &ForceTorqueSensor::updateFTData, this, false, false); ftPullTimer_ = nh.createTimer(ros::Rate(nodePullFreq), &ForceTorqueSensor::pullFTData, this, false, false); - + moving_mean_filter_->configure("MovingMeanFilter"); - low_pass_filter_->configure("LowPassFilter"); + low_pass_filter_->configure("LowPassFilter"); gravity_compensator_->configure("GravityCompensation"); threshold_filter_->configure("ThresholdFilter"); @@ -476,7 +474,7 @@ void ForceTorqueSensor::pullFTData(const ros::TimerEvent &event) 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); + low_pass_filter_->update(sensor_data,low_pass_filtered_data); } else low_pass_filtered_data = sensor_data; //moving_mean @@ -584,8 +582,20 @@ bool ForceTorqueSensor::transform_wrench(std::string goal_frame, std::string sou void ForceTorqueSensor::reconfigureCalibrationRequest(ati_force_torque::CalibrationConfig& config, uint32_t level){ calibration_params_.fromConfig(config); -} + + calibrationTBetween = calibration_params_.T_between_meas; + m_staticCalibration = calibration_params_.isStatic; -void ForceTorqueSensor::reconfigurePublishRequest(ati_force_torque::PublishConfigurationConfig& config, uint32_t level){ - 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"]; + } + From 1755a8e0f76b2e32cf0f8cbd46c325690d477926 Mon Sep 17 00:00:00 2001 From: Andreea Tulbure Date: Thu, 23 Nov 2017 11:50:43 +0100 Subject: [PATCH 50/53] added iirobFilterBase --- config/sensor_configuration.yaml | 5 +++ .../ati_force_torque/force_torque_sensor.h | 9 ++-- ros/src/force_torque_sensor.cpp | 41 +++++++++---------- 3 files changed, 30 insertions(+), 25 deletions(-) diff --git a/config/sensor_configuration.yaml b/config/sensor_configuration.yaml index ef270bb..8414fce 100644 --- a/config/sensor_configuration.yaml +++ b/config/sensor_configuration.yaml @@ -11,6 +11,11 @@ Calibration: T_between_meas: 0.01 isStatic: false # true= use static offset Calibration, false=use calibrate Sensor to 0 in current State +#Filters: +# name: ThresholdFilter +# type: iirob_filters/ThresholdFilterWrench +# params: {linear_threshold: 2.5, angular_threshold: 0.3} + ThresholdFilter: name: ThresholdFilter type: iirob_filters/ThresholdFilterWrench diff --git a/ros/include/ati_force_torque/force_torque_sensor.h b/ros/include/ati_force_torque/force_torque_sensor.h index 290d27c..0a364ce 100644 --- a/ros/include/ati_force_torque/force_torque_sensor.h +++ b/ros/include/ati_force_torque/force_torque_sensor.h @@ -78,6 +78,7 @@ typedef unsigned char uint8_t; #include #include #include +#include #include #include @@ -194,10 +195,10 @@ class ForceTorqueSensor geometry_msgs::Wrench m_calibOffset; - filters::FilterBase *moving_mean_filter_ = new iirob_filters::MovingMeanFilter(); - filters::FilterBase *low_pass_filter_ = new iirob_filters::LowPassFilter(); - filters::FilterBase *threshold_filter_ = new iirob_filters::ThresholdFilter(); - filters::FilterBase *gravity_compensator_ = new iirob_filters::GravityCompensator(); + iirob_filters::iirobFilterBase *moving_mean_filter_ = new iirob_filters::MovingMeanFilter(); + iirob_filters::iirobFilterBase *low_pass_filter_ = new iirob_filters::LowPassFilter(); + iirob_filters::iirobFilterBase *threshold_filter_ = new iirob_filters::ThresholdFilter(); + iirob_filters::iirobFilterBase *gravity_compensator_ = new iirob_filters::GravityCompensator(); bool useGravityCompensator=false; bool useThresholdFilter=false; diff --git a/ros/src/force_torque_sensor.cpp b/ros/src/force_torque_sensor.cpp index 4ee2e0d..f087b26 100644 --- a/ros/src/force_torque_sensor.cpp +++ b/ros/src/force_torque_sensor.cpp @@ -55,8 +55,7 @@ #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/params"} -{ - calibration_params_.fromParamServer(); +{ CS_params_.fromParamServer(); can_params_.fromParamServer(); FTS_params_.fromParamServer(); @@ -64,19 +63,6 @@ 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; @@ -101,6 +87,21 @@ ForceTorqueSensor::ForceTorqueSensor(ros::NodeHandle& nh) : nh_(nh), calibration 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; @@ -147,10 +148,10 @@ ForceTorqueSensor::ForceTorqueSensor(ros::NodeHandle& nh) : nh_(nh), calibration ftUpdateTimer_ = nh.createTimer(ros::Rate(nodePubFreq), &ForceTorqueSensor::updateFTData, this, false, false); ftPullTimer_ = nh.createTimer(ros::Rate(nodePullFreq), &ForceTorqueSensor::pullFTData, this, false, false); - moving_mean_filter_->configure("MovingMeanFilter"); - low_pass_filter_->configure("LowPassFilter"); - gravity_compensator_->configure("GravityCompensation"); - threshold_filter_->configure("ThresholdFilter"); + 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")) { @@ -364,7 +365,6 @@ geometry_msgs::Wrench ForceTorqueSensor::makeAverageMeasurement(uint number_of_m for (int i = 0; i < number_of_measurements; i++) { geometry_msgs::Wrench output; - //std::cout<<"frame id"<< frame_id< Date: Tue, 28 Nov 2017 11:27:07 +0100 Subject: [PATCH 51/53] changes to work with dynamic loading of filters --- config/sensor_configuration.yaml | 6 +++--- .../ati_force_torque/force_torque_sensor.h | 8 ++++---- ros/src/force_torque_sensor.cpp | 16 ++++++++-------- 3 files changed, 15 insertions(+), 15 deletions(-) diff --git a/config/sensor_configuration.yaml b/config/sensor_configuration.yaml index 8414fce..cff4b0e 100644 --- a/config/sensor_configuration.yaml +++ b/config/sensor_configuration.yaml @@ -28,12 +28,12 @@ LowPassFilter: MovingMeanFilter: name: MovingMeanFilter - type: filters/MovingMeanFilterWrench + type: iirob_filters/MovingMeanFilterWrench params: {divider: 4} GravityCompensation: - name: GravityCompensationWrench - type: iirob_filters/GravityCompensator + name: GravityCompensation + type: iirob_filters/GravityCompensatorWrench params: {world_frame: "base_link"} Publish: diff --git a/ros/include/ati_force_torque/force_torque_sensor.h b/ros/include/ati_force_torque/force_torque_sensor.h index 0a364ce..564b0ba 100644 --- a/ros/include/ati_force_torque/force_torque_sensor.h +++ b/ros/include/ati_force_torque/force_torque_sensor.h @@ -195,10 +195,10 @@ class ForceTorqueSensor geometry_msgs::Wrench m_calibOffset; - iirob_filters::iirobFilterBase *moving_mean_filter_ = new iirob_filters::MovingMeanFilter(); - iirob_filters::iirobFilterBase *low_pass_filter_ = new iirob_filters::LowPassFilter(); - iirob_filters::iirobFilterBase *threshold_filter_ = new iirob_filters::ThresholdFilter(); - iirob_filters::iirobFilterBase *gravity_compensator_ = new iirob_filters::GravityCompensator(); + iirob_filters::IIrobFilterBase low_pass_filter_; + iirob_filters::IIrobFilterBase moving_mean_filter_; + iirob_filters::IIrobFilterBase threshold_filter_; + iirob_filters::IIrobFilterBase gravity_compensator_; bool useGravityCompensator=false; bool useThresholdFilter=false; diff --git a/ros/src/force_torque_sensor.cpp b/ros/src/force_torque_sensor.cpp index f087b26..a3a2eb6 100644 --- a/ros/src/force_torque_sensor.cpp +++ b/ros/src/force_torque_sensor.cpp @@ -148,10 +148,10 @@ ForceTorqueSensor::ForceTorqueSensor(ros::NodeHandle& nh) : nh_(nh), calibration ftUpdateTimer_ = nh.createTimer(ros::Rate(nodePubFreq), &ForceTorqueSensor::updateFTData, this, false, false); ftPullTimer_ = nh.createTimer(ros::Rate(nodePullFreq), &ForceTorqueSensor::pullFTData, this, false, false); - moving_mean_filter_->configure("MovingMeanFilter", nh_); - low_pass_filter_->configure("LowPassFilter", nh_); - gravity_compensator_->configure("GravityCompensation", nh_); - threshold_filter_->configure("ThresholdFilter", nh_); + 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")) { @@ -474,7 +474,7 @@ void ForceTorqueSensor::pullFTData(const ros::TimerEvent &event) 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); + low_pass_filter_.update(sensor_data,low_pass_filtered_data); } else low_pass_filtered_data = sensor_data; //moving_mean @@ -483,7 +483,7 @@ void ForceTorqueSensor::pullFTData(const ros::TimerEvent &event) 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); + moving_mean_filter_.update(low_pass_filtered_data, moving_mean_filtered_wrench); } else moving_mean_filtered_wrench = low_pass_filtered_data; @@ -518,7 +518,7 @@ void ForceTorqueSensor::filterFTData(){ { 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); + gravity_compensator_.update(transformed_data, gravity_compensated_force); } else gravity_compensated_force = transformed_data; @@ -527,7 +527,7 @@ void ForceTorqueSensor::filterFTData(){ { 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); + threshold_filter_.update(gravity_compensated_force, threshold_filtered_force); } else threshold_filtered_force = gravity_compensated_force; From c94c2c2713a5b703ffa3e9d9621eff241981cbab Mon Sep 17 00:00:00 2001 From: Andreea Tulbure Date: Wed, 29 Nov 2017 13:44:32 +0100 Subject: [PATCH 52/53] changes to fully work with dynamic loading of filters --- ros/include/ati_force_torque/force_torque_sensor.h | 9 +++++---- ros/src/force_torque_sensor.cpp | 4 ++-- 2 files changed, 7 insertions(+), 6 deletions(-) diff --git a/ros/include/ati_force_torque/force_torque_sensor.h b/ros/include/ati_force_torque/force_torque_sensor.h index 564b0ba..be7d3aa 100644 --- a/ros/include/ati_force_torque/force_torque_sensor.h +++ b/ros/include/ati_force_torque/force_torque_sensor.h @@ -79,6 +79,7 @@ typedef unsigned char uint8_t; #include #include #include +#include #include #include @@ -195,10 +196,10 @@ class ForceTorqueSensor geometry_msgs::Wrench m_calibOffset; - iirob_filters::IIrobFilterBase low_pass_filter_; - iirob_filters::IIrobFilterBase moving_mean_filter_; - iirob_filters::IIrobFilterBase threshold_filter_; - iirob_filters::IIrobFilterBase gravity_compensator_; + 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; diff --git a/ros/src/force_torque_sensor.cpp b/ros/src/force_torque_sensor.cpp index a3a2eb6..8472578 100644 --- a/ros/src/force_torque_sensor.cpp +++ b/ros/src/force_torque_sensor.cpp @@ -54,7 +54,7 @@ ****************************************************************/ #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/params"} +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(); @@ -152,7 +152,7 @@ ForceTorqueSensor::ForceTorqueSensor(ros::NodeHandle& nh) : nh_(nh), calibration low_pass_filter_.configure("LowPassFilter", nh_); gravity_compensator_.configure("GravityCompensation", nh_); threshold_filter_.configure("ThresholdFilter", nh_); - + //Median Filter if(nh_.hasParam("MovingMeanFilter")) { useMovingMean = true; From a174b64476545ddb8e1676385db4372add5a97d7 Mon Sep 17 00:00:00 2001 From: Andreea Tulbure Date: Thu, 30 Nov 2017 10:48:58 +0100 Subject: [PATCH 53/53] changes for dynamic filter loarding --- config/sensor_configuration.yaml | 8 ++------ ros/include/ati_force_torque/force_torque_sensor.h | 8 ++++---- 2 files changed, 6 insertions(+), 10 deletions(-) diff --git a/config/sensor_configuration.yaml b/config/sensor_configuration.yaml index cff4b0e..4e8e49f 100644 --- a/config/sensor_configuration.yaml +++ b/config/sensor_configuration.yaml @@ -10,11 +10,6 @@ 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 - -#Filters: -# name: ThresholdFilter -# type: iirob_filters/ThresholdFilterWrench -# params: {linear_threshold: 2.5, angular_threshold: 0.3} ThresholdFilter: name: ThresholdFilter @@ -35,7 +30,7 @@ GravityCompensation: name: GravityCompensation type: iirob_filters/GravityCompensatorWrench params: {world_frame: "base_link"} - + Publish: sensor_data: true low_pass: true @@ -43,3 +38,4 @@ Publish: transformed_data: true gravity_compensated: true threshold_filtered: true + diff --git a/ros/include/ati_force_torque/force_torque_sensor.h b/ros/include/ati_force_torque/force_torque_sensor.h index be7d3aa..06cade5 100644 --- a/ros/include/ati_force_torque/force_torque_sensor.h +++ b/ros/include/ati_force_torque/force_torque_sensor.h @@ -196,10 +196,10 @@ class ForceTorqueSensor geometry_msgs::Wrench m_calibOffset; - iirob_filters::IIrobFilterChain low_pass_filter_; - iirob_filters::IIrobFilterChain moving_mean_filter_; - iirob_filters::IIrobFilterChain threshold_filter_; - iirob_filters::IIrobFilterChain gravity_compensator_; + 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;