sr_subscriber.cpp

Go to the documentation of this file.

#include <iostream>
#include <string>
#include <vector>

#include "sr_hand/sr_subscriber.h"
#include <boost/algorithm/string.hpp>
#include <sr_utilities/sr_math_utils.hpp>

using std::vector;
using std::string;

namespace shadowrobot
{

//    CONSTRUCTOR/DESTRUCTOR   //

  SRSubscriber::SRSubscriber(boost::shared_ptr<SRArticulatedRobot> sr_art_robot) :
          n_tilde("~")
  {
    sr_articulated_robot = sr_art_robot;
    // Initialize the subscribers
    SRSubscriber::init();
  }

  SRSubscriber::~SRSubscriber()
  {
  }

  void SRSubscriber::init()
  {
    // subscribe to sendupdate topic, set sendupdateCallback function
    std::string prefix;
    std::string searched_param;
    n_tilde.searchParam("shadowhand_prefix", searched_param);
    n_tilde.param(searched_param, prefix, std::string());
    std::string full_topic = prefix + "sendupdate";

    sendupdate_sub = node.subscribe(full_topic, 2, &SRSubscriber::sendupdateCallback, this);

    // subscribe to contrlr topic, set contrlrCallback function
    full_topic = prefix + "contrlr";
    contrlr_sub = node.subscribe(full_topic, 2, &SRSubscriber::contrlrCallback, this);

    // subscribe to config topic, set configCallback function
    full_topic = prefix + "config";
    config_sub = node.subscribe(full_topic, 2, &SRSubscriber::configCallback, this);

    // subscribe to the new ethercat like topics: one topic per joint
    for (SRArticulatedRobot::JointsMap::iterator joint = sr_articulated_robot->joints_map.begin();
         joint != sr_articulated_robot->joints_map.end(); ++joint)
    {
      controllers_sub.push_back(node.subscribe<std_msgs::Float64>(
              "sh_" + boost::to_lower_copy(joint->first + "_position_controller/command"), 2,
              boost::bind(&SRSubscriber::cmd_callback, this, _1, joint->first)));
    }
  }

//         CALLBACK            //
  void SRSubscriber::sendupdateCallback(const sr_robot_msgs::sendupdateConstPtr &msg)
  {
    // loop on all the sendupdate messages received (if > 0)
    int sendupdate_length = msg->sendupdate_length;
    if (sendupdate_length == 0)
    {
      ROS_DEBUG("Received empty sendupdate command.");
      return;
    }
    // OK, not empty => loop to process all the sendupdate messages
    for (uint16_t index_msg = 0; index_msg < msg->sendupdate_length; ++index_msg)
    {
      float target = msg->sendupdate_list[index_msg].joint_target;
      string sensor_name = msg->sendupdate_list[index_msg].joint_name;

      ROS_DEBUG("Received sendupdate Command [%s : %f]", sensor_name.c_str(), target);
      sr_articulated_robot->sendupdate(sensor_name, static_cast<double>(target));
    }
  }

  void SRSubscriber::cmd_callback(const std_msgs::Float64ConstPtr &msg, std::string &joint_name)
  {
    // converting to degrees as the old can interface was expecting degrees
    sr_articulated_robot->sendupdate(joint_name, sr_math_utils::to_degrees(msg->data));
  }

  void SRSubscriber::contrlrCallback(const sr_robot_msgs::contrlrConstPtr &msg)
  {
    vector<string> list_of_parameters = msg->list_of_parameters;

    JointControllerData ctrl_data;

    // parses all the parameters transmitted in the msg
    for (unsigned int index_param = 0; index_param < msg->length_of_list; ++index_param)
    {
      // split the string (around ":")
      vector<string> splitted_string;
      boost::split(splitted_string, msg->list_of_parameters[index_param], boost::is_any_of(":"));

      Parameters param;
      param.name = splitted_string[0];
      param.value = splitted_string[1];

      ctrl_data.data.push_back(param);
    }

    sr_articulated_robot->setContrl(msg->contrlr_name, ctrl_data);
  }

  void SRSubscriber::configCallback(const sr_robot_msgs::configConstPtr &msg)
  {
    ROS_ERROR("Configuration command callback not implemented yet");
  }

}  // namespace shadowrobot