hand_commander.cpp

Go to the documentation of this file.

#include <sr_hand/hand_commander.hpp>
#include <pr2_mechanism_msgs/ListControllers.h>
#include <sr_robot_msgs/sendupdate.h>
#include <std_msgs/Float64.h>


namespace shadowrobot
{

const double HandCommander::TIMEOUT_TO_DETECT_CONTROLLER_MANAGER = 3.0;

HandCommander::HandCommander():
    hand_type(shadowhandRosLib::UNKNOWN),
    ethercat_controllers_found(false)
{
  //We use the presence of the pr2_controller_manager/list_controllers service to detect that the hand is ethercat
  if(ros::service::waitForService("pr2_controller_manager/list_controllers", ros::Duration(TIMEOUT_TO_DETECT_CONTROLLER_MANAGER)))
  {
    hand_type = shadowhandRosLib::ETHERCAT;
    initializeEthercatHand();
    ROS_INFO("HandCommander library: ETHERCAT hand detected");
  }
  else
  {
    hand_type = shadowhandRosLib::CAN;
    sr_hand_target_pub = node_.advertise<sr_robot_msgs::sendupdate>("/srh/sendupdate", 2);
    ROS_INFO("HandCommander library: CAN hand detected");
  }
}

HandCommander::~HandCommander()
{
}

void HandCommander::initializeEthercatHand()
{
  sr_hand_target_pub_map.clear();

  ros::ServiceClient controller_list_client = node_.serviceClient<pr2_mechanism_msgs::ListControllers>("pr2_controller_manager/list_controllers");

  pr2_mechanism_msgs::ListControllers controller_list;
  std::string controlled_joint_name;

  controller_list_client.call(controller_list);

  for (size_t i=0;i<controller_list.response.controllers.size() ;i++ )
  {
    if(controller_list.response.state[i]=="running")
    {
      std::string controller = controller_list.response.controllers[i];
      if (node_.getParam("/"+controller+"/joint", controlled_joint_name))
      {
        ROS_DEBUG("controller %d:%s controls joint %s\n",
            (int)i,controller.c_str(),controlled_joint_name.c_str());
        sr_hand_target_pub_map[controlled_joint_name]
            = node_.advertise<std_msgs::Float64>(controller+"/command", 2);
        ethercat_controllers_found = true;
      }
    }
  }

}

void HandCommander::sendCommands(std::vector<sr_robot_msgs::joint> joint_vector)
{
  if(hand_type == shadowhandRosLib::ETHERCAT)
  {
    if(!ethercat_controllers_found)
    {
      initializeEthercatHand();
      // No controllers we found so bail out
      if (!ethercat_controllers_found)
          return;
    }
    for(size_t i = 0; i < joint_vector.size(); ++i)
    {
      std_msgs::Float64 target;
      target.data = joint_vector.at(i).joint_target * M_PI/180.0;
      sr_hand_target_pub_map[joint_vector.at(i).joint_name].publish(target);
    }
  }
  else
  {
    sr_robot_msgs::sendupdate sendupdate_msg;
    sendupdate_msg.sendupdate_length = joint_vector.size();
    sendupdate_msg.sendupdate_list = joint_vector;

    sr_hand_target_pub.publish(sendupdate_msg);
  }
}

}