current_state_monitor.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2011, 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 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 
00035 /* Author: Ioan Sucan */
00036 
00037 #include <moveit/planning_scene_monitor/current_state_monitor.h>
00038 #include <tf_conversions/tf_eigen.h>
00039 #include <limits>
00040 
00041 planning_scene_monitor::CurrentStateMonitor::CurrentStateMonitor(const robot_model::RobotModelConstPtr& robot_model,
00042                                                                  const boost::shared_ptr<tf::Transformer>& tf)
00043   : CurrentStateMonitor(robot_model, tf, ros::NodeHandle())
00044 {
00045 }
00046 
00047 planning_scene_monitor::CurrentStateMonitor::CurrentStateMonitor(const robot_model::RobotModelConstPtr& robot_model,
00048                                                                  const boost::shared_ptr<tf::Transformer>& tf,
00049                                                                  ros::NodeHandle nh)
00050   : nh_(nh)
00051   , tf_(tf)
00052   , robot_model_(robot_model)
00053   , robot_state_(robot_model)
00054   , state_monitor_started_(false)
00055   , error_(std::numeric_limits<float>::epsilon())
00056 {
00057   robot_state_.setToDefaultValues();
00058 }
00059 
00060 planning_scene_monitor::CurrentStateMonitor::~CurrentStateMonitor()
00061 {
00062   stopStateMonitor();
00063 }
00064 
00065 robot_state::RobotStatePtr planning_scene_monitor::CurrentStateMonitor::getCurrentState() const
00066 {
00067   boost::mutex::scoped_lock slock(state_update_lock_);
00068   robot_state::RobotState* result = new robot_state::RobotState(robot_state_);
00069   return robot_state::RobotStatePtr(result);
00070 }
00071 
00072 ros::Time planning_scene_monitor::CurrentStateMonitor::getCurrentStateTime() const
00073 {
00074   boost::mutex::scoped_lock slock(state_update_lock_);
00075   return current_state_time_;
00076 }
00077 
00078 std::pair<robot_state::RobotStatePtr, ros::Time>
00079 planning_scene_monitor::CurrentStateMonitor::getCurrentStateAndTime() const
00080 {
00081   boost::mutex::scoped_lock slock(state_update_lock_);
00082   robot_state::RobotState* result = new robot_state::RobotState(robot_state_);
00083   return std::make_pair(robot_state::RobotStatePtr(result), current_state_time_);
00084 }
00085 
00086 std::map<std::string, double> planning_scene_monitor::CurrentStateMonitor::getCurrentStateValues() const
00087 {
00088   std::map<std::string, double> m;
00089   boost::mutex::scoped_lock slock(state_update_lock_);
00090   const double* pos = robot_state_.getVariablePositions();
00091   const std::vector<std::string>& names = robot_state_.getVariableNames();
00092   for (std::size_t i = 0; i < names.size(); ++i)
00093     m[names[i]] = pos[i];
00094   return m;
00095 }
00096 
00097 void planning_scene_monitor::CurrentStateMonitor::setToCurrentState(robot_state::RobotState& upd) const
00098 {
00099   boost::mutex::scoped_lock slock(state_update_lock_);
00100   const double* pos = robot_state_.getVariablePositions();
00101   upd.setVariablePositions(pos);
00102 }
00103 
00104 void planning_scene_monitor::CurrentStateMonitor::addUpdateCallback(const JointStateUpdateCallback& fn)
00105 {
00106   if (fn)
00107     update_callbacks_.push_back(fn);
00108 }
00109 
00110 void planning_scene_monitor::CurrentStateMonitor::clearUpdateCallbacks()
00111 {
00112   update_callbacks_.clear();
00113 }
00114 
00115 void planning_scene_monitor::CurrentStateMonitor::startStateMonitor(const std::string& joint_states_topic)
00116 {
00117   if (!state_monitor_started_ && robot_model_)
00118   {
00119     joint_time_.clear();
00120     if (joint_states_topic.empty())
00121       ROS_ERROR("The joint states topic cannot be an empty string");
00122     else
00123       joint_state_subscriber_ = nh_.subscribe(joint_states_topic, 25, &CurrentStateMonitor::jointStateCallback, this);
00124     state_monitor_started_ = true;
00125     monitor_start_time_ = ros::Time::now();
00126     ROS_DEBUG("Listening to joint states on topic '%s'", nh_.resolveName(joint_states_topic).c_str());
00127   }
00128 }
00129 
00130 bool planning_scene_monitor::CurrentStateMonitor::isActive() const
00131 {
00132   return state_monitor_started_;
00133 }
00134 
00135 void planning_scene_monitor::CurrentStateMonitor::stopStateMonitor()
00136 {
00137   if (state_monitor_started_)
00138   {
00139     joint_state_subscriber_.shutdown();
00140     ROS_DEBUG("No longer listening for joint states");
00141     state_monitor_started_ = false;
00142   }
00143 }
00144 
00145 std::string planning_scene_monitor::CurrentStateMonitor::getMonitoredTopic() const
00146 {
00147   if (joint_state_subscriber_)
00148     return joint_state_subscriber_.getTopic();
00149   else
00150     return "";
00151 }
00152 
00153 bool planning_scene_monitor::CurrentStateMonitor::isPassiveOrMimicDOF(const std::string& dof) const
00154 {
00155   if (robot_model_->hasJointModel(dof))
00156   {
00157     if (robot_model_->getJointModel(dof)->isPassive() || robot_model_->getJointModel(dof)->getMimic())
00158       return true;
00159   }
00160   else
00161   {
00162     // check if this DOF is part of a multi-dof passive joint
00163     std::size_t slash = dof.find_last_of("/");
00164     if (slash != std::string::npos)
00165     {
00166       std::string joint_name = dof.substr(0, slash);
00167       if (robot_model_->hasJointModel(joint_name))
00168         if (robot_model_->getJointModel(joint_name)->isPassive() || robot_model_->getJointModel(joint_name)->getMimic())
00169           return true;
00170     }
00171   }
00172   return false;
00173 }
00174 
00175 bool planning_scene_monitor::CurrentStateMonitor::haveCompleteState() const
00176 {
00177   bool result = true;
00178   const std::vector<std::string>& dof = robot_model_->getVariableNames();
00179   boost::mutex::scoped_lock slock(state_update_lock_);
00180   for (std::size_t i = 0; i < dof.size(); ++i)
00181     if (joint_time_.find(dof[i]) == joint_time_.end())
00182     {
00183       if (!isPassiveOrMimicDOF(dof[i]))
00184       {
00185         ROS_DEBUG("Joint variable '%s' has never been updated", dof[i].c_str());
00186         result = false;
00187       }
00188     }
00189   return result;
00190 }
00191 
00192 bool planning_scene_monitor::CurrentStateMonitor::haveCompleteState(std::vector<std::string>& missing_states) const
00193 {
00194   bool result = true;
00195   const std::vector<std::string>& dof = robot_model_->getVariableNames();
00196   boost::mutex::scoped_lock slock(state_update_lock_);
00197   for (std::size_t i = 0; i < dof.size(); ++i)
00198     if (joint_time_.find(dof[i]) == joint_time_.end())
00199       if (!isPassiveOrMimicDOF(dof[i]))
00200       {
00201         missing_states.push_back(dof[i]);
00202         result = false;
00203       }
00204   return result;
00205 }
00206 
00207 bool planning_scene_monitor::CurrentStateMonitor::haveCompleteState(const ros::Duration& age) const
00208 {
00209   bool result = true;
00210   const std::vector<std::string>& dof = robot_model_->getVariableNames();
00211   ros::Time now = ros::Time::now();
00212   ros::Time old = now - age;
00213   boost::mutex::scoped_lock slock(state_update_lock_);
00214   for (std::size_t i = 0; i < dof.size(); ++i)
00215   {
00216     if (isPassiveOrMimicDOF(dof[i]))
00217       continue;
00218     std::map<std::string, ros::Time>::const_iterator it = joint_time_.find(dof[i]);
00219     if (it == joint_time_.end())
00220     {
00221       ROS_DEBUG("Joint variable '%s' has never been updated", dof[i].c_str());
00222       result = false;
00223     }
00224     else if (it->second < old)
00225     {
00226       ROS_DEBUG("Joint variable '%s' was last updated %0.3lf seconds ago (older than the allowed %0.3lf seconds)",
00227                 dof[i].c_str(), (now - it->second).toSec(), age.toSec());
00228       result = false;
00229     }
00230   }
00231   return result;
00232 }
00233 
00234 bool planning_scene_monitor::CurrentStateMonitor::haveCompleteState(const ros::Duration& age,
00235                                                                     std::vector<std::string>& missing_states) const
00236 {
00237   bool result = true;
00238   const std::vector<std::string>& dof = robot_model_->getVariableNames();
00239   ros::Time now = ros::Time::now();
00240   ros::Time old = now - age;
00241   boost::mutex::scoped_lock slock(state_update_lock_);
00242   for (std::size_t i = 0; i < dof.size(); ++i)
00243   {
00244     if (isPassiveOrMimicDOF(dof[i]))
00245       continue;
00246     std::map<std::string, ros::Time>::const_iterator it = joint_time_.find(dof[i]);
00247     if (it == joint_time_.end())
00248     {
00249       ROS_DEBUG("Joint variable '%s' has never been updated", dof[i].c_str());
00250       missing_states.push_back(dof[i]);
00251       result = false;
00252     }
00253     else if (it->second < old)
00254     {
00255       ROS_DEBUG("Joint variable '%s' was last updated %0.3lf seconds ago (older than the allowed %0.3lf seconds)",
00256                 dof[i].c_str(), (now - it->second).toSec(), age.toSec());
00257       missing_states.push_back(dof[i]);
00258       result = false;
00259     }
00260   }
00261   return result;
00262 }
00263 
00264 bool planning_scene_monitor::CurrentStateMonitor::waitForCurrentState(const ros::Time t, double wait_time) const
00265 {
00266   ros::WallTime timeout = ros::WallTime::now() + ros::WallDuration(wait_time);
00267   ros::WallDuration busywait(0.1);
00268 
00269   boost::mutex::scoped_lock lock(state_update_lock_);
00270   while (current_state_time_ < t)
00271   {
00272     lock.unlock();
00273     busywait.sleep();
00274     if (ros::WallTime::now() >= timeout)
00275       return false;
00276     lock.lock();
00277   }
00278   return true;
00279 }
00280 
00281 bool planning_scene_monitor::CurrentStateMonitor::waitForCurrentState(double wait_time) const
00282 {
00283   double slept_time = 0.0;
00284   double sleep_step_s = std::min(0.05, wait_time / 10.0);
00285   ros::Duration sleep_step(sleep_step_s);
00286   while (!haveCompleteState() && slept_time < wait_time)
00287   {
00288     sleep_step.sleep();
00289     slept_time += sleep_step_s;
00290   }
00291   return haveCompleteState();
00292 }
00293 
00294 bool planning_scene_monitor::CurrentStateMonitor::waitForCurrentState(const std::string& group, double wait_time) const
00295 {
00296   if (waitForCurrentState(wait_time))
00297     return true;
00298   bool ok = true;
00299 
00300   // check to see if we have a fully known state for the joints we want to record
00301   std::vector<std::string> missing_joints;
00302   if (!haveCompleteState(missing_joints))
00303   {
00304     const robot_model::JointModelGroup* jmg = robot_model_->getJointModelGroup(group);
00305     if (jmg)
00306     {
00307       std::set<std::string> mj;
00308       mj.insert(missing_joints.begin(), missing_joints.end());
00309       const std::vector<std::string>& names = jmg->getJointModelNames();
00310       bool ok = true;
00311       for (std::size_t i = 0; ok && i < names.size(); ++i)
00312         if (mj.find(names[i]) != mj.end())
00313           ok = false;
00314     }
00315     else
00316       ok = false;
00317   }
00318   return ok;
00319 }
00320 
00321 void planning_scene_monitor::CurrentStateMonitor::jointStateCallback(const sensor_msgs::JointStateConstPtr& joint_state)
00322 {
00323   if (joint_state->name.size() != joint_state->position.size())
00324   {
00325     ROS_ERROR_THROTTLE(1, "State monitor received invalid joint state (number of joint names does not match number of "
00326                           "positions)");
00327     return;
00328   }
00329   bool update = false;
00330 
00331   {
00332     boost::mutex::scoped_lock _(state_update_lock_);
00333     // read the received values, and update their time stamps
00334     std::size_t n = joint_state->name.size();
00335     current_state_time_ = joint_state->header.stamp;
00336     for (std::size_t i = 0; i < n; ++i)
00337     {
00338       const robot_model::JointModel* jm = robot_model_->getJointModel(joint_state->name[i]);
00339       if (!jm)
00340         continue;
00341       // ignore fixed joints, multi-dof joints (they should not even be in the message)
00342       if (jm->getVariableCount() != 1)
00343         continue;
00344 
00345       joint_time_[joint_state->name[i]] = joint_state->header.stamp;
00346 
00347       if (robot_state_.getJointPositions(jm)[0] != joint_state->position[i])
00348       {
00349         update = true;
00350         robot_state_.setJointPositions(jm, &(joint_state->position[i]));
00351 
00352         // continuous joints wrap, so we don't modify them (even if they are outside bounds!)
00353         if (jm->getType() == robot_model::JointModel::REVOLUTE)
00354           if (static_cast<const robot_model::RevoluteJointModel*>(jm)->isContinuous())
00355             continue;
00356 
00357         const robot_model::VariableBounds& b =
00358             jm->getVariableBounds()[0];  // only one variable in the joint, so we get its bounds
00359 
00360         // if the read variable is 'almost' within bounds (up to error_ difference), then consider it to be within
00361         // bounds
00362         if (joint_state->position[i] < b.min_position_ && joint_state->position[i] >= b.min_position_ - error_)
00363           robot_state_.setJointPositions(jm, &b.min_position_);
00364         else if (joint_state->position[i] > b.max_position_ && joint_state->position[i] <= b.max_position_ + error_)
00365           robot_state_.setJointPositions(jm, &b.max_position_);
00366       }
00367     }
00368 
00369     // read root transform, if needed
00370     if (tf_ && (robot_model_->getRootJoint()->getType() == robot_model::JointModel::PLANAR ||
00371                 robot_model_->getRootJoint()->getType() == robot_model::JointModel::FLOATING))
00372     {
00373       const std::string& child_frame = robot_model_->getRootLink()->getName();
00374       const std::string& parent_frame = robot_model_->getModelFrame();
00375 
00376       std::string err;
00377       ros::Time tm;
00378       tf::StampedTransform transf;
00379       bool ok = false;
00380       if (tf_->getLatestCommonTime(parent_frame, child_frame, tm, &err) == tf::NO_ERROR)
00381       {
00382         try
00383         {
00384           tf_->lookupTransform(parent_frame, child_frame, tm, transf);
00385           ok = true;
00386         }
00387         catch (tf::TransformException& ex)
00388         {
00389           ROS_ERROR_THROTTLE(1, "Unable to lookup transform from %s to %s.  Exception: %s", parent_frame.c_str(),
00390                              child_frame.c_str(), ex.what());
00391         }
00392       }
00393       else
00394         ROS_DEBUG_THROTTLE(1, "Unable to lookup transform from %s to %s: no common time.", parent_frame.c_str(),
00395                            child_frame.c_str());
00396       if (ok && last_tf_update_ != tm)
00397       {
00398         update = true;
00399         last_tf_update_ = tm;
00400         const std::vector<std::string>& vars = robot_model_->getRootJoint()->getVariableNames();
00401         for (std::size_t j = 0; j < vars.size(); ++j)
00402           joint_time_[vars[j]] = tm;
00403         Eigen::Affine3d eigen_transf;
00404         tf::transformTFToEigen(transf, eigen_transf);
00405         robot_state_.setJointPositions(robot_model_->getRootJoint(), eigen_transf);
00406       }
00407     }
00408   }
00409 
00410   // callbacks, if needed
00411   if (update)
00412     for (std::size_t i = 0; i < update_callbacks_.size(); ++i)
00413       update_callbacks_[i](joint_state);
00414 }


planning
Author(s): Ioan Sucan , Sachin Chitta
autogenerated on Fri Dec 14 2018 03:32:30