mech_stats_publisher.hpp
Go to the documentation of this file.
00001 /*
00002  * mech_stats_publisher.hpp
00003  *
00004  *  Created on: 15 April 2014
00005  *      Author: Manos Nikolaidis
00006  *
00007  * Software License Agreement (BSD License)
00008  *
00009  *  Copyright (c) 2014, Shadow Robot Company Ltd.
00010  *  All rights reserved.
00011  *
00012  *  Redistribution and use in source and binary forms, with or without
00013  *  modification, are permitted provided that the following conditions
00014  *  are met:
00015  *
00016  *   * Redistributions of source code must retain the above copyright
00017  *     notice, this list of conditions and the following disclaimer.
00018  *   * Redistributions in binary form must reproduce the above
00019  *     copyright notice, this list of conditions and the following
00020  *     disclaimer in the documentation and/or other materials provided
00021  *     with the distribution.
00022  *   * Neither the name of the Willow Garage nor the names of its
00023  *     contributors may be used to endorse or promote products derived
00024  *     from this software without specific prior written permission.
00025  *
00026  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00027  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00028  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00029  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00030  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00031  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00032  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00033  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00034  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00035  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00036  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00037  *  POSSIBILITY OF SUCH DAMAGE.
00038  *********************************************************************/
00039 
00040 #ifndef ROS_ETHERCAT_MODEL_MECH_STATS_PUBLISHER_HPP_
00041 #define ROS_ETHERCAT_MODEL_MECH_STATS_PUBLISHER_HPP_
00042 
00043 #include <ros_ethercat_model/robot_state.hpp>
00044 #include <pr2_mechanism_msgs/MechanismStatistics.h>
00045 #include <pr2_mechanism_msgs/JointStatistics.h>
00046 #include <pr2_mechanism_msgs/ActuatorStatistics.h>
00047 
00048 using std::string;
00049 using std::vector;
00050 using boost::unordered_map;
00051 using boost::ptr_vector;
00052 using boost::ptr_unordered_map;
00053 using ros::Duration;
00054 using ros::Time;
00055 using ros::NodeHandle;
00056 using realtime_tools::RealtimePublisher;
00057 using pr2_mechanism_msgs::MechanismStatistics;
00058 using pr2_mechanism_msgs::JointStatistics;
00059 using pr2_mechanism_msgs::ActuatorStatistics;
00060 using ros_ethercat_model::JointState;
00061 using ros_ethercat_model::Actuator;
00062 using ros_ethercat_model::ActuatorState;
00063 using ros_ethercat_model::RobotState;
00064 using ros_ethercat_model::Transmission;
00065 
00066 class MechStatsPublisher
00067 {
00068 public:
00069   MechStatsPublisher(NodeHandle &nh, RobotState &state) :
00070     state_(state),
00071     pub_mech_stats_(nh, "mechanism_statistics", 1),
00072     last_published_mechanism_stats_(Time::now())
00073   {
00074     pub_mech_stats_.msg_.joint_statistics.resize(state_.joint_states_.size());
00075     pub_mech_stats_.msg_.actuator_statistics.resize(state_.transmissions_.size());
00076 
00077     double publish_rate_mechanism_stats;
00078     nh.param("mechanism_statistics_publish_rate", publish_rate_mechanism_stats, 1.0);
00079     publish_period_mechanism_stats_ = Duration(1.0 / fmax(0.000001, publish_rate_mechanism_stats));
00080   }
00081   void publish()
00082   {
00083     Time now = Time::now();
00084     if (now > last_published_mechanism_stats_ + publish_period_mechanism_stats_)
00085     {
00086       if (pub_mech_stats_.trylock())
00087       {
00088         while (last_published_mechanism_stats_ + publish_period_mechanism_stats_ < now)
00089           last_published_mechanism_stats_ = last_published_mechanism_stats_ + publish_period_mechanism_stats_;
00090 
00091         ptr_unordered_map<string, JointState>::iterator jin = state_.joint_states_.begin();
00092         vector<JointStatistics>::iterator jout = pub_mech_stats_.msg_.joint_statistics.begin();
00093         while (jin != state_.joint_states_.end() &&
00094                jout != pub_mech_stats_.msg_.joint_statistics.end())
00095         {
00096           int type = jin->second->joint_->type;
00097           if (type != urdf::Joint::REVOLUTE && type != urdf::Joint::CONTINUOUS && type != urdf::Joint::PRISMATIC)
00098             continue;
00099           jout->timestamp = now;
00100           jout->name = jin->second->joint_->name;
00101           jout->position = jin->second->position_;
00102           jout->velocity = jin->second->velocity_;
00103           jout->measured_effort = jin->second->measured_effort_;
00104           jout->commanded_effort = jin->second->commanded_effort_;
00105           jout->is_calibrated = jin->second->calibrated_;
00106           jout->violated_limits = jin->second->joint_statistics_.violated_limits_;
00107           jout->min_position = jin->second->joint_statistics_.min_position_;
00108           jout->max_position = jin->second->joint_statistics_.max_position_;
00109           jout->max_abs_velocity = jin->second->joint_statistics_.max_abs_velocity_;
00110           jout->max_abs_effort = jin->second->joint_statistics_.max_abs_effort_;
00111           jin->second->joint_statistics_.reset();
00112           ++jin;
00113           ++jout;
00114         }
00115 
00116         ptr_vector<Transmission>::iterator tin = state_.transmissions_.begin();
00117         vector<ActuatorStatistics>::iterator aout = pub_mech_stats_.msg_.actuator_statistics.begin();
00118         while (tin != state_.transmissions_.end() &&
00119                aout != pub_mech_stats_.msg_.actuator_statistics.end())
00120         {
00121           aout->timestamp = now;
00122           aout->name = tin->actuator_->name_;
00123           aout->position = tin->actuator_->state_.position_;
00124           aout->device_id = tin->actuator_->state_.device_id_;
00125           aout->velocity = tin->actuator_->state_.velocity_;
00126           aout->is_enabled = true;
00127           aout->last_commanded_current = tin->actuator_->state_.last_commanded_current_;
00128           aout->last_measured_current = tin->actuator_->state_.last_measured_current_;
00129           aout->last_commanded_effort = tin->actuator_->state_.last_commanded_effort_;
00130           aout->last_measured_effort = tin->actuator_->state_.last_measured_effort_;
00131           aout->motor_voltage = tin->actuator_->state_.motor_voltage_;
00132           ++tin;
00133           ++aout;
00134         }
00135 
00136         pub_mech_stats_.msg_.header.stamp = Time::now();
00137         pub_mech_stats_.unlockAndPublish();
00138       }
00139     }
00140   }
00141 
00142   RobotState &state_;
00143   RealtimePublisher<MechanismStatistics> pub_mech_stats_;
00144   Duration publish_period_mechanism_stats_;
00145   Time last_published_mechanism_stats_;
00146 };
00147 #endif


ros_ethercat_model
Author(s): Manos Nikolaidis
autogenerated on Thu Aug 27 2015 14:47:12