joint_state_monitor.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2008, Willow Garage, Inc.
00005 *  All rights reserved.
00006 *
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 *
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of the Willow Garage nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 *
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
00033 *********************************************************************/
00034 
00037 #include <planning_environment/monitors/joint_state_monitor.h>
00038 #include <angles/angles.h>
00039 #include <sstream>
00040 
00041 planning_environment::JointStateMonitor::JointStateMonitor()
00042 {
00043   first_time_ = true;  
00044   std::string urdf_xml,full_urdf_xml;
00045   root_handle_.param("urdf_xml",urdf_xml,std::string("robot_description"));
00046 
00047   if(!root_handle_.getParam(urdf_xml,full_urdf_xml))
00048   {
00049     ROS_ERROR("Could not load the xml from parameter server: %s\n", urdf_xml.c_str());
00050     active_ = false;
00051   }
00052   else
00053   {
00054     robot_model_.initString(full_urdf_xml);
00055     active_ = true;
00056     joint_state_subscriber_ =  root_handle_.subscribe("joint_states", 1, &planning_environment::JointStateMonitor::jointStateCallback, this);
00057     ROS_INFO("Joint state monitor active");
00058   }
00059 }
00060 
00061 void planning_environment::JointStateMonitor::stop(void)
00062 {
00063   if (!active_)
00064     return;    
00065   joint_state_subscriber_.shutdown();    
00066   active_ = false;
00067 }
00068 
00069 void planning_environment::JointStateMonitor::jointStateCallback(const sensor_msgs::JointStateConstPtr &joint_state)
00070 {    
00071   if (!active_)
00072     return;    
00073   ROS_DEBUG("Joint state monitor callback");
00074   if (joint_state->name.size() != joint_state->position.size() || joint_state->name.size() !=joint_state->velocity.size())
00075   {
00076     ROS_ERROR("Planning environment received invalid joint state");
00077     return;
00078   }
00079   boost::mutex::scoped_lock lock(state_mutex_);
00080   if(first_time_)
00081   {
00082     joint_state_.header.frame_id = "base_footprint";
00083     joint_state_.name = joint_state->name;
00084     joint_state_.position.resize(joint_state->position.size());
00085     for(unsigned int i=0; i < joint_state_.name.size(); i++)
00086     {
00087       joint_state_index_[joint_state_.name[i]] = i;
00088 
00089       boost::shared_ptr<const urdf::Joint> joint = robot_model_.getJoint(joint_state_.name[i]);
00090       if(!joint->child_link_name.empty())
00091       {
00092         joint_real_state_index_.push_back(i);
00093       }
00094     }
00095   }
00096   joint_state_.header.stamp = ros::Time::now();
00097   for(unsigned int i=0; i < joint_state->position.size(); i++)
00098   {
00099     boost::shared_ptr<const urdf::Joint> joint = robot_model_.getJoint(joint_state_.name[i]);
00100     if (joint->type == urdf::Joint::CONTINUOUS)
00101       joint_state_.position[i] = angles::normalize_angle(joint_state->position[i]);
00102     else
00103       joint_state_.position[i] = joint_state->position[i];
00104   }
00105   first_time_ = false;
00106   last_update_ = joint_state->header.stamp;
00107 }
00108 
00109 sensor_msgs::JointState planning_environment::JointStateMonitor::getJointState()
00110 {
00111   boost::mutex::scoped_lock lock(state_mutex_);
00112   return joint_state_;
00113 }
00114 
00115 sensor_msgs::JointState planning_environment::JointStateMonitor::getJointStateRealJoints()
00116 {
00117   if (!active_)
00118     return joint_state_;    
00119   boost::mutex::scoped_lock lock(state_mutex_);
00120   sensor_msgs::JointState joint_state;
00121 
00122   unsigned int num_real_joints = joint_real_state_index_.size();
00123   joint_state.header = joint_state_.header;
00124   joint_state.name.resize(num_real_joints);
00125   joint_state.position.resize(num_real_joints);
00126 
00127   for(unsigned int i=0; i< num_real_joints; i++)
00128   {
00129     int index = joint_real_state_index_[i];
00130     joint_state.name[i] = joint_state_.name[index];
00131     joint_state.position[i] = joint_state_.position[index];
00132   }
00133   return joint_state;
00134 }
00135 
00136 
00137 sensor_msgs::JointState planning_environment::JointStateMonitor::getJointState(std::vector<std::string> names)
00138 {
00139   if (!active_)
00140     return joint_state_;    
00141   sensor_msgs::JointState state;
00142   state.position.resize(names.size());
00143   state.name = names;
00144 
00145   boost::mutex::scoped_lock lock(state_mutex_);
00146   for(unsigned int i=0; i < state.name.size(); i++)
00147   {
00148     std::map<std::string, int>::iterator joint_it = joint_state_index_.find(state.name[i]);
00149     if(joint_it == joint_state_index_.end())
00150     {
00151       continue;
00152     }
00153     else
00154     {
00155       int index = joint_it->second;
00156       state.position[i]  = joint_state_.position[index];
00157     }
00158   }
00159   return state;
00160 }


planning_environment
Author(s): Ioan Sucan
autogenerated on Mon Dec 2 2013 12:34:43