link_joints.cpp

Go to the documentation of this file.

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

// messages
#include <std_msgs/Float64.h>
#include <control_msgs/JointControllerState.h>

std::string parent_name = "rh_ffj3";
std::string child_name = "rh_mfj3";
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;

void callback(const control_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;
}