Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043 #include <ati_force_torque/force_torque_sensor_sim.h>
00044
00045 ForceTorqueSensorSim::ForceTorqueSensorSim(ros::NodeHandle& nh) : nh_(nh), pub_params_{nh.getNamespace()+"/Publish"}, node_params_{nh.getNamespace()+"/Node"}
00046 {
00047 std::cout<<"ForceTorqueSensorSim"<<std::endl;
00048 pub_params_.fromParamServer();
00049 node_params_.fromParamServer();
00050 transform_frame_ = node_params_.transform_frame;
00051
00052 is_pub_sensor_data_=pub_params_.sensor_data;
00053 if(is_pub_sensor_data_){
00054 sensor_data_pub_ = nh_.advertise<geometry_msgs::WrenchStamped>("sensor_data", 1);
00055 }
00056 is_pub_transformed_data_ = pub_params_.transformed_data;
00057 if(is_pub_transformed_data_){
00058 transformed_data_pub_ = nh_.advertise<geometry_msgs::WrenchStamped>("transformed_data", 1);
00059 }
00060 ftUpdateTimer_ = nh.createTimer(ros::Rate(node_params_.ft_pub_freq), &ForceTorqueSensorSim::updateFTData, this, false, false);
00061 ftPullTimer_ = nh.createTimer(ros::Rate(node_params_.ft_pull_freq), &ForceTorqueSensorSim::pullFTData, this, false, false);
00062 p_tfBuffer = new tf2_ros::Buffer();
00063 p_tfListener = new tf2_ros::TransformListener(*p_tfBuffer, true);
00064 init_sensor();
00065 ftUpdateTimer_.start();
00066 }
00067 void ForceTorqueSensorSim::init_sensor() {
00068 force_input_subscriber = nh_.subscribe("/cmd_force", 1, &ForceTorqueSensorSim::subscribeData, this);
00069 ftPullTimer_.start();
00070 }
00071
00072 void ForceTorqueSensorSim::subscribeData(const geometry_msgs::Twist::ConstPtr& msg){
00073 joystick_data.wrench.force.x = msg->linear.x;
00074 joystick_data.wrench.force.y = msg->linear.y;
00075 joystick_data.wrench.force.z = msg->linear.z;
00076 joystick_data.wrench.torque.z = msg->angular.z;
00077 joystick_data.wrench.torque.x = msg->angular.x;;
00078 joystick_data.wrench.torque.y = msg->angular.y;
00079 }
00080
00081 void ForceTorqueSensorSim::pullFTData(const ros::TimerEvent &event)
00082 {
00083 if(is_pub_sensor_data_)
00084 sensor_data_pub_.publish(joystick_data);
00085 }
00086 bool ForceTorqueSensorSim::transform_wrench(std::string goal_frame, std::string source_frame, geometry_msgs::Wrench wrench, geometry_msgs::Wrench *transformed)
00087 {
00088 geometry_msgs::TransformStamped transform;
00089 geometry_msgs::Vector3Stamped temp_vector_in, temp_vector_out;
00090
00091 try
00092 {
00093 transform = p_tfBuffer->lookupTransform(goal_frame, source_frame, ros::Time(0));
00094 _num_transform_errors = 0;
00095 }
00096 catch (tf2::TransformException ex)
00097 {
00098 if (_num_transform_errors%100 == 0){
00099 ROS_ERROR("%s", ex.what());
00100 }
00101 _num_transform_errors++;
00102 return false;
00103 }
00104
00105 temp_vector_in.vector = wrench.force;
00106 tf2::doTransform(temp_vector_in, temp_vector_out, transform);
00107 transformed->force = temp_vector_out.vector;
00108
00109 temp_vector_in.vector = wrench.torque;
00110 tf2::doTransform(temp_vector_in, temp_vector_out, transform);
00111 transformed->torque = temp_vector_out.vector;
00112
00113 return true;
00114 }
00115 void ForceTorqueSensorSim::filterFTData(){
00116 transformed_data.header.stamp = ros::Time::now();
00117 transformed_data.header.frame_id = node_params_.transform_frame;
00118 transform_wrench(node_params_.transform_frame, sensor_frame_, joystick_data.wrench, &transformed_data.wrench);
00119 threshold_filtered_force = transformed_data;
00120 if(is_pub_transformed_data_)
00121 transformed_data_pub_.publish(threshold_filtered_force);
00122 }