00001
00056 #include <ros/ros.h>
00057 #include <string>
00058
00059
00060 #include <sr_robot_msgs/joints_data.h>
00061 #include <sr_robot_msgs/joint.h>
00062 #include <sr_robot_msgs/sendupdate.h>
00063
00065 std::string parent_name = "FFJ3";
00067 std::string child_name = "MFJ3";
00068
00069
00070 ros::Subscriber sub;
00071
00072 ros::Publisher pub;
00073
00080 void callback(const sr_robot_msgs::joints_dataConstPtr& msg)
00081 {
00082
00083 int msg_length = msg->joints_list_length;
00084 if( msg_length == 0)
00085 {
00086 ROS_WARN("Received empty message.");
00087 return;
00088 }
00089
00090
00091 for(unsigned short index_msg=0; index_msg < msg_length; ++index_msg)
00092 {
00093
00094 std::string sensor_name = msg->joints_list[index_msg].joint_name;
00095
00100 if(sensor_name.compare(parent_name) == 0)
00101 {
00102
00103 float target = msg->joints_list[index_msg].joint_position;
00104
00105
00106 sr_robot_msgs::sendupdate msg;
00107 std::vector<sr_robot_msgs::joint> jointVector;
00108
00109
00110 sr_robot_msgs::joint joint;
00111 joint.joint_name = child_name;
00112 joint.joint_target = target;
00113 jointVector.push_back(joint);
00114
00115 msg.sendupdate_length = jointVector.size();
00116 msg.sendupdate_list = jointVector;
00117
00118
00119 pub.publish(msg);
00120
00121 return;
00122 }
00123 }
00124
00125 }
00126
00135 int main(int argc, char** argv)
00136 {
00137
00138 ros::init(argc, argv, "link_joints_example");
00139 ros::NodeHandle node;
00140
00146 sub = node.subscribe("/srh/shadowhand_data", 2, callback);
00147
00152 pub = node.advertise<sr_robot_msgs::sendupdate>("/srh/sendupdate", 2);
00153
00154
00155 while( ros::ok() )
00156 ros::spin();
00157
00158 return 0;
00159 }