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
00043 #include <string.h>
00044 #include <stdio.h>
00045
00046 #include "ros/ros.h"
00047 #include "std_msgs/String.h"
00048 #include "geometry_msgs/WrenchStamped.h"
00049 #include "robotiq_force_torque_sensor/rq_sensor_state.h"
00050 #include "robotiq_force_torque_sensor/ft_sensor.h"
00051 #include "robotiq_force_torque_sensor/sensor_accessor.h"
00052
00053 static void decode_message_and_do(INT_8 const * const buff, INT_8 * const ret);
00054 static void wait_for_other_connection(void);
00055 static int max_retries_(100);
00056
00057 ros::Publisher sensor_pub_acc;
00058
00064 static void decode_message_and_do(INT_8 const * const buff, INT_8 * const ret)
00065 {
00066 INT_8 get_or_set[3];
00067 INT_8 nom_var[4];
00068
00069 if(buff == NULL || strlen(buff) != 7)
00070 {
00071 return;
00072 }
00073
00074 strncpy(get_or_set, &buff[0], 3);
00075 strncpy(nom_var, &buff[4], strlen(buff) -3);
00076
00077 if(strstr(get_or_set, "GET"))
00078 {
00079 rq_state_get_command(nom_var, ret);
00080 }
00081 else if(strstr(get_or_set, "SET"))
00082 {
00083 if(strstr(nom_var, "ZRO"))
00084 {
00085 rq_state_do_zero_force_flag();
00086 strcpy(ret,"Done");
00087 }
00088 }
00089 }
00090
00091 bool receiverCallback(robotiq_force_torque_sensor::sensor_accessor::Request& req,
00092 robotiq_force_torque_sensor::sensor_accessor::Response& res)
00093 {
00094 ROS_INFO("I heard: [%s]",req.command.c_str());
00095 INT_8 buffer[512];
00096 decode_message_and_do((char*)req.command.c_str(), buffer);
00097 res.res = buffer;
00098 ROS_INFO("I send: [%s]", res.res.c_str());
00099 return true;
00100 }
00101
00106 static void wait_for_other_connection(void)
00107 {
00108 INT_8 ret;
00109
00110 while(ros::ok())
00111 {
00112 ROS_INFO("Waiting for sensor connection...");
00113 usleep(1000000);
00114
00115 ret = rq_sensor_state(max_retries_);
00116 if(ret == 0)
00117 {
00118 ROS_INFO("Sensor connected!");
00119 break;
00120 }
00121
00122 ros::spinOnce();
00123 }
00124 }
00125
00131 static robotiq_force_torque_sensor::ft_sensor get_data(void)
00132 {
00133 robotiq_force_torque_sensor::ft_sensor msgStream;
00134
00135 msgStream.Fx = rq_state_get_received_data(0);
00136 msgStream.Fy = rq_state_get_received_data(1);
00137 msgStream.Fz = rq_state_get_received_data(2);
00138 msgStream.Mx = rq_state_get_received_data(3);
00139 msgStream.My = rq_state_get_received_data(4);
00140 msgStream.Mz = rq_state_get_received_data(5);
00141
00142 return msgStream;
00143 }
00144
00145 int main(int argc, char **argv)
00146 {
00147 ros::init(argc, argv, "robotiq_force_torque_sensor");
00148 ros::NodeHandle n;
00149 ros::param::param<int>("~max_retries", max_retries_, 100);
00150
00151 INT_8 bufStream[512];
00152 robotiq_force_torque_sensor::ft_sensor msgStream;
00153 INT_8 ret;
00154
00155
00156 ret = rq_sensor_state(max_retries_);
00157 if(ret == -1)
00158 {
00159 wait_for_other_connection();
00160 }
00161
00162
00163 ret = rq_sensor_state(max_retries_);
00164 if(ret == -1)
00165 {
00166 wait_for_other_connection();
00167 }
00168
00169
00170 ret = rq_sensor_state(max_retries_);
00171 if(ret == -1)
00172 {
00173 wait_for_other_connection();
00174 }
00175
00176 ros::Publisher sensor_pub = n.advertise<robotiq_force_torque_sensor::ft_sensor>("robotiq_force_torque_sensor", 512);
00177 ros::Publisher wrench_pub = n.advertise<geometry_msgs::WrenchStamped>("robotiq_force_torque_wrench", 512);
00178 ros::ServiceServer service = n.advertiseService("robotiq_force_torque_sensor_acc", receiverCallback);
00179
00180
00181 geometry_msgs::WrenchStamped wrenchMsg;
00182 ros::param::param<std::string>("~frame_id", wrenchMsg.header.frame_id, "robotiq_force_torque_frame_id");
00183
00184 ROS_INFO("Starting Sensor");
00185 while(ros::ok())
00186 {
00187 ret = rq_sensor_state(max_retries_);
00188
00189 if(ret == -1)
00190 {
00191 wait_for_other_connection();
00192 }
00193
00194 if(rq_sensor_get_current_state() == RQ_STATE_RUN)
00195 {
00196 strcpy(bufStream,"");
00197 msgStream = get_data();
00198
00199 if(rq_state_got_new_message())
00200 {
00201 sensor_pub.publish(msgStream);
00202
00203
00204 wrenchMsg.header.stamp = ros::Time::now();
00205 wrenchMsg.wrench.force.x = msgStream.Fx;
00206 wrenchMsg.wrench.force.y = msgStream.Fy;
00207 wrenchMsg.wrench.force.z = msgStream.Fz;
00208 wrenchMsg.wrench.torque.x = msgStream.Mx;
00209 wrenchMsg.wrench.torque.y = msgStream.My;
00210 wrenchMsg.wrench.torque.z = msgStream.Mz;
00211 wrench_pub.publish(wrenchMsg);
00212 }
00213 }
00214
00215 ros::spinOnce();
00216 }
00217 return 0;
00218 }