sr_publisher.cpp
Go to the documentation of this file.
00001 
00030 //ROS include
00031 #include <ros/ros.h>
00032 
00033 //messages
00034 #include <sr_robot_msgs/joints_data.h>
00035 #include <sr_robot_msgs/joint.h>
00036 #include <sensor_msgs/JointState.h>
00037 
00038 //generic C/C++ include
00039 #include <vector>
00040 #include <string>
00041 #include <sstream>
00042 
00043 #include "sr_hand/sr_publisher.h"
00044 
00045 using namespace ros;
00046 using namespace shadowrobot;
00047 
00048 namespace shadowrobot
00049 {
00051 //    CONSTRUCTOR/DESTRUCTOR   //
00053 SRPublisher::SRPublisher( boost::shared_ptr<SRArticulatedRobot> sh ) :
00054     n_tilde("~"), publish_rate(0.0)
00055 {
00056     sr_articulated_robot = sh;
00057 
00058     // set publish frequency
00059     double publish_freq;
00060     n_tilde.param("publish_frequency", publish_freq, 50.0);
00061     publish_rate = Rate(publish_freq);
00062 
00063     //publishes JointState messages for the robot_state_publisher
00064     std::string prefix;
00065     std::string searched_param;
00066     n_tilde.searchParam("shadowhand_prefix", searched_param);
00067     n_tilde.param(searched_param, prefix, std::string());
00068     std::string full_topic = prefix + "position/joint_states";
00069     sr_jointstate_pos_pub = node.advertise<sensor_msgs::JointState> (full_topic, 2);
00070     full_topic = prefix + "target/joint_states";
00071     sr_jointstate_target_pub = node.advertise<sensor_msgs::JointState> (full_topic, 2);
00072 
00073     //publishes standard joints data (pos, targets, temp, current, ...)
00074     full_topic = prefix + "shadowhand_data";
00075     sr_pub = node.advertise<sr_robot_msgs::joints_data> (full_topic, 2);
00076 }
00077 
00078 SRPublisher::~SRPublisher()
00079 {
00080     //if( shadowhand != NULL )
00081     // delete shadowhand;
00082 }
00083 
00085 //       PUBLISH METHOD        //
00087 void SRPublisher::publish()
00088 {
00089     SRArticulatedRobot::JointsMap joints_map = sr_articulated_robot->getAllJointsData();
00090 
00091     sr_robot_msgs::joints_data msg;
00092     std::vector<sr_robot_msgs::joint> jointVector;
00093 
00094     sensor_msgs::JointState jointstate_pos_msg;
00095     sensor_msgs::JointState jointstate_target_msg;
00096 
00097     ros::Time now = ros::Time::now();
00098     jointstate_pos_msg.header.stamp = now; 
00099     jointstate_target_msg.header.stamp = now;
00100 
00101     for( SRArticulatedRobot::JointsMap::const_iterator it = joints_map.begin(); it != joints_map.end(); ++it )
00102     {
00103         sr_robot_msgs::joint joint;
00104         JointData currentData = it->second;
00105 
00106         //compute the angular velocity of the joint
00107         if(currentData.last_pos_time.toSec() != 0.0)
00108         {
00109             currentData.velocity = (currentData.position - currentData.last_pos);
00110             currentData.velocity /= (now - currentData.last_pos_time).toSec();
00111             ROS_DEBUG("Velocity = (%f - %f)/(%f) = %f", currentData.position, currentData.last_pos, (now - currentData.last_pos_time).toSec(), currentData.velocity);
00112         }
00113 
00114         joint.joint_name = it->first;
00115         jointstate_pos_msg.name.push_back(it->first);
00116         jointstate_target_msg.name.push_back(it->first);
00117 
00118         jointstate_target_msg.position.push_back(toRad(currentData.target));
00119         jointstate_target_msg.velocity.push_back(0.0);
00120         jointstate_target_msg.effort.push_back(0.0);
00121 
00122         jointstate_pos_msg.position.push_back(toRad(currentData.position));
00123         jointstate_pos_msg.velocity.push_back(currentData.velocity);
00124         jointstate_pos_msg.effort.push_back(currentData.force);
00125 
00126         joint.joint_position = currentData.position;
00127         joint.joint_target = currentData.target;
00128         joint.joint_torque = currentData.force;
00129         joint.joint_temperature = currentData.temperature;
00130         joint.joint_current = currentData.current;
00131 
00132         //update data for the velocity
00133         currentData.last_pos_time = now;
00134         currentData.last_pos = currentData.position;
00135 
00136         sr_articulated_robot->joints_map_mutex.lock();
00137         sr_articulated_robot->joints_map[it->first] = JointData(currentData);
00138         sr_articulated_robot->joints_map_mutex.unlock();
00139 
00140         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());
00141 
00142         jointVector.push_back(joint);
00143     }
00144 
00145     msg.joints_list_length = jointVector.size();
00146     msg.joints_list = jointVector;
00147 
00148     //publish standard data (pos, target, current, temp, force, ...)
00149     sr_pub.publish(msg);
00150     //publish JointState position message
00151     sr_jointstate_pos_pub.publish(jointstate_pos_msg);
00152     //publish JointState target message
00153     sr_jointstate_target_pub.publish(jointstate_target_msg);
00154 
00155     ros::spinOnce();
00156     publish_rate.sleep();
00157 }
00158 
00159 }// end namespace
00160 
00161 


sr_hand
Author(s): Ugo Cupcic
autogenerated on Fri Aug 28 2015 13:08:51