link_joints.cpp

Go to the documentation of this file.

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

//messages
#include <std_msgs/Float64.h>
#include <sr_robot_msgs/JointControllerState.h>
#include <pr2_controllers_msgs/JointControllerState.h>

std::string parent_name = "ffj3";
std::string child_name  = "mfj3";
// If you use the simulated hand (in gazebo) use the mixed controllers
std::string controller_type = "_mixed_position_velocity_controller";
// If you use the real hand, generally use the position controller (comment the previous line and uncomment the following)
//std::string controller_type = "_position_controller";

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

// If you use the simulated hand (in gazebo) use the mixed controllers
void callback(const sr_robot_msgs::JointControllerStateConstPtr& msg)
// If you use the real hand, generally use the position controller (comment the previous line and uncomment the following)
//void callback(const pr2_controllers_msgs::JointControllerStateConstPtr& msg)
{
  //publish the message
  std_msgs::Float64 command;
  command.data = msg->set_point;
  pub.publish(command);

  return;
}

int main(int argc, char** argv)
{
  //init the ros node
  ros::init(argc, argv, "link_joints_example");
  ros::NodeHandle node;

  sub = node.subscribe("sh_" + parent_name + controller_type + "/state", 2,  callback);

  pub = node.advertise<std_msgs::Float64>("sh_" + child_name + controller_type + "/command", 2);

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

  return 0;
}