rq_sensor.cpp
Go to the documentation of this file.
00001 /* Software License Agreement (BSD License)
00002 *
00003 * Copyright (c) 2014, Robotiq, Inc.
00004 * All rights reserved.
00005 *
00006 * Redistribution and use in source and binary forms, with or without
00007 * modification, are permitted provided that the following conditions
00008 * are met:
00009 *
00010 * * Redistributions of source code must retain the above copyright
00011 * notice, this list of conditions and the following disclaimer.
00012 * * Redistributions in binary form must reproduce the above
00013 * copyright notice, this list of conditions and the following
00014 * disclaimer in the documentation and/or other materials provided
00015 * with the distribution.
00016 * * Neither the name of Robotiq, Inc. nor the names of its
00017 * contributors may be used to endorse or promote products derived
00018 * from this software without specific prior written permission.
00019 *
00020 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00021 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00022 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00023 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00024 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00025 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00026 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00027 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00028 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00029 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00030 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00031 * POSSIBILITY OF SUCH DAMAGE.
00032 *
00033 * Copyright (c) 2014, Robotiq, Inc
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);//Attend 1 seconde.
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         //If we can't initialize, we return an error
00156         ret = rq_sensor_state(max_retries_);
00157         if(ret == -1)
00158         {
00159                 wait_for_other_connection();
00160         }
00161 
00162         //Reads basic info on the sensor
00163         ret = rq_sensor_state(max_retries_);
00164         if(ret == -1)
00165         {
00166                 wait_for_other_connection();
00167         }
00168 
00169         //Starts the stream
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         //std_msgs::String msg;
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                                 //compose WrenchStamped Msg
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 }


robotiq_force_torque_sensor
Author(s): Jonathan Savoie
autogenerated on Thu Jun 6 2019 17:58:03