link_joints.cpp
#include <ros/ros.h>
#include <string>
#include <sr_robot_msgs/joints_data.h>
#include <sr_robot_msgs/joint.h>
#include <sr_robot_msgs/sendupdate.h>
std::string parent_name = "FFJ3";
std::string child_name = "MFJ3";
ros::Subscriber sub;
ros::Publisher pub;
void callback(const sr_robot_msgs::joints_dataConstPtr& msg)
{
int msg_length = msg->joints_list_length;
if( msg_length == 0)
{
ROS_WARN("Received empty message.");
return;
}
for(unsigned short index_msg=0; index_msg < msg_length; ++index_msg)
{
std::string sensor_name = msg->joints_list[index_msg].joint_name;
if(sensor_name.compare(parent_name) == 0)
{
float target = msg->joints_list[index_msg].joint_position;
sr_robot_msgs::sendupdate msg;
std::vector<sr_robot_msgs::joint> jointVector;
sr_robot_msgs::joint joint;
joint.joint_name = child_name;
joint.joint_target = target;
jointVector.push_back(joint);
msg.sendupdate_length = jointVector.size();
msg.sendupdate_list = jointVector;
pub.publish(msg);
return;
}
}
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "link_joints_example");
ros::NodeHandle node;
sub = node.subscribe("/srh/shadowhand_data", 2, callback);
pub = node.advertise<sr_robot_msgs::sendupdate>("/srh/sendupdate", 2);
while( ros::ok() )
ros::spin();
return 0;
}