sr_publisher.cpp

Go to the documentation of this file.

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

// messages
#include <sr_robot_msgs/joints_data.h>
#include <sr_robot_msgs/joint.h>
#include <sensor_msgs/JointState.h>

// generic C/C++ include
#include <vector>
#include <string>
#include <sstream>

#include "sr_hand/sr_publisher.h"

namespace shadowrobot
{
//    CONSTRUCTOR/DESTRUCTOR   //
  SRPublisher::SRPublisher(boost::shared_ptr<SRArticulatedRobot> sh) :
          n_tilde("~"), publish_rate(0.0)
  {
    sr_articulated_robot = sh;

    // set publish frequency
    double publish_freq;
    n_tilde.param("publish_frequency", publish_freq, 50.0);
    publish_rate = Rate(publish_freq);

    // publishes JointState messages for the robot_state_publisher
    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 + "position/joint_states";
    sr_jointstate_pos_pub = node.advertise<sensor_msgs::JointState>(full_topic, 2);
    full_topic = prefix + "target/joint_states";
    sr_jointstate_target_pub = node.advertise<sensor_msgs::JointState>(full_topic, 2);

    // publishes standard joints data (pos, targets, temp, current, ...)
    full_topic = prefix + "shadowhand_data";
    sr_pub = node.advertise<sr_robot_msgs::joints_data>(full_topic, 2);
  }

  SRPublisher::~SRPublisher()
  {
    // if( shadowhand != NULL )
    // delete shadowhand;
  }

//       PUBLISH METHOD        //
  void SRPublisher::publish()
  {
    SRArticulatedRobot::JointsMap joints_map = sr_articulated_robot->getAllJointsData();

    sr_robot_msgs::joints_data msg;
    std::vector<sr_robot_msgs::joint> jointVector;

    sensor_msgs::JointState jointstate_pos_msg;
    sensor_msgs::JointState jointstate_target_msg;

    ros::Time now = ros::Time::now();
    jointstate_pos_msg.header.stamp = now;
    jointstate_target_msg.header.stamp = now;

    for (SRArticulatedRobot::JointsMap::const_iterator it = joints_map.begin(); it != joints_map.end(); ++it)
    {
      sr_robot_msgs::joint joint;
      JointData currentData = it->second;

      // compute the angular velocity of the joint
      if (currentData.last_pos_time.toSec() != 0.0)
      {
        currentData.velocity = (currentData.position - currentData.last_pos);
        currentData.velocity /= (now - currentData.last_pos_time).toSec();
        ROS_DEBUG("Velocity = (%f - %f)/(%f) = %f", currentData.position, currentData.last_pos,
                  (now - currentData.last_pos_time).toSec(), currentData.velocity);
      }

      joint.joint_name = it->first;
      jointstate_pos_msg.name.push_back(it->first);
      jointstate_target_msg.name.push_back(it->first);

      jointstate_target_msg.position.push_back(toRad(currentData.target));
      jointstate_target_msg.velocity.push_back(0.0);
      jointstate_target_msg.effort.push_back(0.0);

      jointstate_pos_msg.position.push_back(toRad(currentData.position));
      jointstate_pos_msg.velocity.push_back(currentData.velocity);
      jointstate_pos_msg.effort.push_back(currentData.force);

      joint.joint_position = currentData.position;
      joint.joint_target = currentData.target;
      joint.joint_torque = currentData.force;
      joint.joint_temperature = currentData.temperature;
      joint.joint_current = currentData.current;

      // update data for the velocity
      currentData.last_pos_time = now;
      currentData.last_pos = currentData.position;

      sr_articulated_robot->joints_map_mutex.lock();
      sr_articulated_robot->joints_map[it->first] = JointData(currentData);
      sr_articulated_robot->joints_map_mutex.unlock();

      ROS_DEBUG("last_pos_time[%s] = %f / %f", it->first.c_str(), currentData.last_pos_time.toSec(),
                joints_map[it->first].last_pos_time.toSec());

      jointVector.push_back(joint);
    }

    msg.joints_list_length = jointVector.size();
    msg.joints_list = jointVector;

    // publish standard data (pos, target, current, temp, force, ...)
    sr_pub.publish(msg);
    // publish JointState position message
    sr_jointstate_pos_pub.publish(jointstate_pos_msg);
    // publish JointState target message
    sr_jointstate_target_pub.publish(jointstate_target_msg);

    ros::spinOnce();
    publish_rate.sleep();
  }

}  // namespace shadowrobot