send_targets.cpp

Go to the documentation of this file.

#include <ros/ros.h>

// we store the publishers in a map.
#include <map>
#include <string>
#include <vector>

// You need to send a Float64 message to the joints.
#include <std_msgs/Float64.h>

class TargetSender
{
public:
  TargetSender()
  {
    static const std::string joints[20] =
    {
      "ffj0", "ffj3", "ffj4",
      "mfj0", "mfj3", "mfj4",
      "rfj0", "rfj3", "rfj4",
      "lfj0", "lfj3", "lfj4", "lfj5",
      "thj1", "thj2", "thj3", "thj4", "thj5",
      "wrj1", "wrj2"
    };

    static const std::string prefix = "/sh_";
    static const std::string suffix = "_position_controller/command";

    /*
     * We now build the map of publishers: there's one topic per joint.
     *  to send a target to a joint, we need to publish on /sh_ffj3_position_controller/command
     *  (replacing ffj3 by the other joint names, or position controller by the controller
     *  you want to use: you can run rostopic list to see which topics are available).
     *
     * The topic type is Float64.
     */
    for (unsigned int index_joint = 0; index_joint < 20; ++index_joint)
    {
      std::string joint_tmp = joints[index_joint];
      publisher_map_[joint_tmp] = node_handle_.advertise<std_msgs::Float64>(prefix + joint_tmp + suffix, 2);
    }
  };

  ~TargetSender()
  {
  };

  bool sendupdate(std::string joint_name, double target)
  {
    // first we find the joint in the map (or return an error if we
    // can't find it)
    std::map<std::string, ros::Publisher>::iterator publisher_iterator;
    publisher_iterator = publisher_map_.find(joint_name);

    if (publisher_iterator == publisher_map_.end())
    {
      ROS_WARN_STREAM("Joint " << joint_name << " not found.");
      return false;
    }

    // now we build the message:
    // the target must be send in RADIANS
    std_msgs::Float64 msg_to_send;
    msg_to_send.data = target * 3.14159 / 360.0;
    publisher_iterator->second.publish(msg_to_send);

    return true;
  };

protected:
  std::map <std::string, ros::Publisher> publisher_map_;
  ros::NodeHandle node_handle_;
};


int main(int argc, char **argv)
{
  // Initialize the ROS node.
  ros::init(argc, argv, "send_targets");

  // Instantiate the target sender
  TargetSender target_sender = TargetSender();

  // Builds the vector of targets
  static const unsigned int length_targets = 1000;
  std::vector<double> targets;

  for (unsigned int i = 0; i < length_targets; ++i)
  {
    double target = static_cast<double>(i) / 1000.0 * 90.0;
    targets.push_back(target);
  }

  // Send the targets until the node is killed.
  unsigned int step = 0;
  while (ros::ok())
  {
    target_sender.sendupdate("ffj3", targets[step % length_targets]);
    usleep(10000);

    ++step;
  }

  return 0;
}

/* For the emacs weenies in the crowd.
Local Variables:
   c-basic-offset: 2
End:
*/