link_joints.cpp

Go to the documentation of this file.

#include <ros/ros.h>
#include <string>

//messages
#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";

//a ros subscriber (will be instantiated later on)
ros::Subscriber sub;
//a ros publisher (will be instantiated later on)
ros::Publisher pub;

void callback(const sr_robot_msgs::joints_dataConstPtr& msg)
{
  //loop on all the sendupdate messages received (if > 0)
  int msg_length = msg->joints_list_length;
  if( msg_length == 0)
    {
      ROS_WARN("Received empty message.");
      return;
    }

  //OK, not empty => read the data
  for(unsigned short index_msg=0; index_msg < msg_length; ++index_msg)
    {
      //get the sensor name
      std::string sensor_name = msg->joints_list[index_msg].joint_name;

      if(sensor_name.compare(parent_name) == 0)
        {
          //get the position (to be set as the target of the child joint)
          float target = msg->joints_list[index_msg].joint_position;

          //form a sendupdate msg.
          sr_robot_msgs::sendupdate msg;
          std::vector<sr_robot_msgs::joint> jointVector;

          //fill the message
          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;

          //publish the message
          pub.publish(msg);

          return;
        }
    }

}

int main(int argc, char** argv)
{
  //init the ros node
  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);

  //subscribe until interrupted.
  while( ros::ok() )
    ros::spin();

  return 0;
}