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 "ros/ros.h" 00044 #include "std_msgs/String.h" 00045 #include "robotiq_force_torque_sensor/ft_sensor.h" 00046 #include "robotiq_force_torque_sensor/sensor_accessor.h" 00047 00048 #include <sstream> 00049 00050 /*void receiveCallback(const std_msgs::String::ConstPtr& msg) 00051 { 00052 ROS_INFO("I heard: [%s]", msg->data.c_str()); 00053 }*/ 00054 00055 void reCallback(const robotiq_force_torque_sensor::ft_sensor& msg) 00056 { 00057 ROS_INFO("I heard: FX[%f] FY[%f] FZ[%f] MX[%f] MY[%f] MZ[%f]", msg.Fx,msg.Fy,msg.Fz,msg.Mx,msg.My,msg.Mz); 00058 } 00059 00063 int main(int argc, char **argv) 00064 { 00065 00066 ros::init(argc, argv, "rq_test_sensor"); 00067 00068 00069 ros::NodeHandle n; 00070 00071 ros::ServiceClient client = n.serviceClient<robotiq_force_torque_sensor::sensor_accessor>("robotiq_force_torque_sensor_acc"); 00072 ros::Subscriber sub1 = n.subscribe("robotiq_force_torque_sensor",100,reCallback); 00073 00074 robotiq_force_torque_sensor::sensor_accessor srv; 00075 00076 int count = 0; 00077 while (ros::ok()) 00078 { 00079 if(count == 10000000){ 00080 srv.request.command = "SET ZRO"; 00081 if(client.call(srv)){ 00082 ROS_INFO("ret: %s", srv.response.res.c_str()); 00083 } 00084 count = 0; 00085 } 00086 00087 ros::spinOnce(); 00088 00089 ++count; 00090 } 00091 00092 00093 return 0; 00094 }