current_state_monitor.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2011, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Ioan Sucan */
36 
38 
40 
41 #include <limits>
42 
43 planning_scene_monitor::CurrentStateMonitor::CurrentStateMonitor(const robot_model::RobotModelConstPtr& robot_model,
45  : CurrentStateMonitor(robot_model, tf, ros::NodeHandle())
46 {
47 }
48 
49 planning_scene_monitor::CurrentStateMonitor::CurrentStateMonitor(const robot_model::RobotModelConstPtr& robot_model,
51  ros::NodeHandle nh)
52  : nh_(nh)
53  , tf_(tf)
54  , robot_model_(robot_model)
55  , robot_state_(robot_model)
56  , state_monitor_started_(false)
57  , copy_dynamics_(false)
58  , error_(std::numeric_limits<double>::epsilon())
59 {
60  robot_state_.setToDefaultValues();
61 }
62 
64 {
66 }
67 
69 {
70  boost::mutex::scoped_lock slock(state_update_lock_);
71  robot_state::RobotState* result = new robot_state::RobotState(robot_state_);
72  return robot_state::RobotStatePtr(result);
73 }
74 
76 {
77  boost::mutex::scoped_lock slock(state_update_lock_);
78  return current_state_time_;
79 }
80 
81 std::pair<robot_state::RobotStatePtr, ros::Time>
83 {
84  boost::mutex::scoped_lock slock(state_update_lock_);
85  robot_state::RobotState* result = new robot_state::RobotState(robot_state_);
86  return std::make_pair(robot_state::RobotStatePtr(result), current_state_time_);
87 }
88 
90 {
91  std::map<std::string, double> m;
92  boost::mutex::scoped_lock slock(state_update_lock_);
93  const double* pos = robot_state_.getVariablePositions();
94  const std::vector<std::string>& names = robot_state_.getVariableNames();
95  for (std::size_t i = 0; i < names.size(); ++i)
96  m[names[i]] = pos[i];
97  return m;
98 }
99 
101 {
102  boost::mutex::scoped_lock slock(state_update_lock_);
103  const double* pos = robot_state_.getVariablePositions();
104  upd.setVariablePositions(pos);
105  if (copy_dynamics_)
106  {
107  if (robot_state_.hasVelocities())
108  {
109  const double* vel = robot_state_.getVariableVelocities();
110  upd.setVariableVelocities(vel);
111  }
112  if (robot_state_.hasAccelerations())
113  {
114  const double* acc = robot_state_.getVariableAccelerations();
115  upd.setVariableAccelerations(acc);
116  }
117  if (robot_state_.hasEffort())
118  {
119  const double* eff = robot_state_.getVariableEffort();
120  upd.setVariableEffort(eff);
121  }
122  }
123 }
124 
126 {
127  if (fn)
128  update_callbacks_.push_back(fn);
129 }
130 
132 {
133  update_callbacks_.clear();
134 }
135 
136 void planning_scene_monitor::CurrentStateMonitor::startStateMonitor(const std::string& joint_states_topic)
137 {
139  {
140  joint_time_.clear();
141  if (joint_states_topic.empty())
142  ROS_ERROR("The joint states topic cannot be an empty string");
143  else
145  if (tf_ && robot_model_->getMultiDOFJointModels().size() > 0)
146  {
147  tf_connection_.reset(
148  new TFConnection(tf_->addTransformsChangedListener(boost::bind(&CurrentStateMonitor::tfCallback, this))));
149  }
150  state_monitor_started_ = true;
152  ROS_DEBUG("Listening to joint states on topic '%s'", nh_.resolveName(joint_states_topic).c_str());
153  }
154 }
155 
157 {
158  return state_monitor_started_;
159 }
160 
162 {
164  {
166  if (tf_ && tf_connection_)
167  {
168  tf_->removeTransformsChangedListener(*tf_connection_);
169  tf_connection_.reset();
170  }
171  ROS_DEBUG("No longer listening for joint states");
172  state_monitor_started_ = false;
173  }
174 }
175 
177 {
180  else
181  return "";
182 }
183 
185 {
186  bool result = true;
187  const std::vector<const moveit::core::JointModel*>& joints = robot_model_->getActiveJointModels();
188  boost::mutex::scoped_lock slock(state_update_lock_);
189  for (const moveit::core::JointModel* joint : joints)
190  if (joint_time_.find(joint) == joint_time_.end())
191  {
192  if (!joint->isPassive() && !joint->getMimic())
193  {
194  ROS_DEBUG("Joint '%s' has never been updated", joint->getName().c_str());
195  result = false;
196  }
197  }
198  return result;
199 }
200 
201 bool planning_scene_monitor::CurrentStateMonitor::haveCompleteState(std::vector<std::string>& missing_states) const
202 {
203  bool result = true;
204  const std::vector<const moveit::core::JointModel*>& joints = robot_model_->getActiveJointModels();
205  boost::mutex::scoped_lock slock(state_update_lock_);
206  for (const moveit::core::JointModel* joint : joints)
207  if (joint_time_.find(joint) == joint_time_.end())
208  if (!joint->isPassive() && !joint->getMimic())
209  {
210  missing_states.push_back(joint->getName());
211  result = false;
212  }
213  return result;
214 }
215 
217 {
218  bool result = true;
219  const std::vector<const moveit::core::JointModel*>& joints = robot_model_->getActiveJointModels();
220  ros::Time now = ros::Time::now();
221  ros::Time old = now - age;
222  boost::mutex::scoped_lock slock(state_update_lock_);
223  for (const moveit::core::JointModel* joint : joints)
224  {
225  if (joint->isPassive() || joint->getMimic())
226  continue;
227  std::map<const moveit::core::JointModel*, ros::Time>::const_iterator it = joint_time_.find(joint);
228  if (it == joint_time_.end())
229  {
230  ROS_DEBUG("Joint '%s' has never been updated", joint->getName().c_str());
231  result = false;
232  }
233  else if (it->second < old)
234  {
235  ROS_DEBUG("Joint '%s' was last updated %0.3lf seconds ago (older than the allowed %0.3lf seconds)",
236  joint->getName().c_str(), (now - it->second).toSec(), age.toSec());
237  result = false;
238  }
239  }
240  return result;
241 }
242 
244  std::vector<std::string>& missing_states) const
245 {
246  bool result = true;
247  const std::vector<const moveit::core::JointModel*>& joints = robot_model_->getActiveJointModels();
248  ros::Time now = ros::Time::now();
249  ros::Time old = now - age;
250  boost::mutex::scoped_lock slock(state_update_lock_);
251  for (const moveit::core::JointModel* joint : joints)
252  {
253  if (joint->isPassive() || joint->getMimic())
254  continue;
255  std::map<const moveit::core::JointModel*, ros::Time>::const_iterator it = joint_time_.find(joint);
256  if (it == joint_time_.end())
257  {
258  ROS_DEBUG("Joint '%s' has never been updated", joint->getName().c_str());
259  missing_states.push_back(joint->getName());
260  result = false;
261  }
262  else if (it->second < old)
263  {
264  ROS_DEBUG("Joint '%s' was last updated %0.3lf seconds ago (older than the allowed %0.3lf seconds)",
265  joint->getName().c_str(), (now - it->second).toSec(), age.toSec());
266  missing_states.push_back(joint->getName());
267  result = false;
268  }
269  }
270  return result;
271 }
272 
274 {
276  ros::WallDuration elapsed(0, 0);
277  ros::WallDuration timeout(wait_time);
278 
279  boost::mutex::scoped_lock lock(state_update_lock_);
280  while (current_state_time_ < t)
281  {
282  state_update_condition_.wait_for(lock, boost::chrono::nanoseconds((timeout - elapsed).toNSec()));
283  elapsed = ros::WallTime::now() - start;
284  if (elapsed > timeout)
285  {
286  ROS_INFO_STREAM("Didn't received robot state (joint angles) with recent timestamp within "
287  << wait_time << " seconds.\n"
288  << "Check clock synchronization if your are running ROS across multiple machines!");
289  return false;
290  }
291  }
292  return true;
293 }
294 
296 {
297  double slept_time = 0.0;
298  double sleep_step_s = std::min(0.05, wait_time / 10.0);
299  ros::Duration sleep_step(sleep_step_s);
300  while (!haveCompleteState() && slept_time < wait_time)
301  {
302  sleep_step.sleep();
303  slept_time += sleep_step_s;
304  }
305  return haveCompleteState();
306 }
308 {
309  return waitForCompleteState(wait_time);
310 }
311 
312 bool planning_scene_monitor::CurrentStateMonitor::waitForCompleteState(const std::string& group, double wait_time) const
313 {
314  if (waitForCompleteState(wait_time))
315  return true;
316  bool ok = true;
317 
318  // check to see if we have a fully known state for the joints we want to record
319  std::vector<std::string> missing_joints;
320  if (!haveCompleteState(missing_joints))
321  {
322  const moveit::core::JointModelGroup* jmg = robot_model_->getJointModelGroup(group);
323  if (jmg)
324  {
325  std::set<std::string> mj;
326  mj.insert(missing_joints.begin(), missing_joints.end());
327  const std::vector<std::string>& names = jmg->getJointModelNames();
328  bool ok = true;
329  for (std::size_t i = 0; ok && i < names.size(); ++i)
330  if (mj.find(names[i]) != mj.end())
331  ok = false;
332  }
333  else
334  ok = false;
335  }
336  return ok;
337 }
338 
339 bool planning_scene_monitor::CurrentStateMonitor::waitForCurrentState(const std::string& group, double wait_time) const
340 {
341  waitForCompleteState(group, wait_time);
342 }
343 
344 void planning_scene_monitor::CurrentStateMonitor::jointStateCallback(const sensor_msgs::JointStateConstPtr& joint_state)
345 {
346  if (joint_state->name.size() != joint_state->position.size())
347  {
348  ROS_ERROR_THROTTLE(1, "State monitor received invalid joint state (number of joint names does not match number of "
349  "positions)");
350  return;
351  }
352  bool update = false;
353 
354  {
355  boost::mutex::scoped_lock _(state_update_lock_);
356  // read the received values, and update their time stamps
357  std::size_t n = joint_state->name.size();
358  current_state_time_ = joint_state->header.stamp;
359  for (std::size_t i = 0; i < n; ++i)
360  {
361  const moveit::core::JointModel* jm = robot_model_->getJointModel(joint_state->name[i]);
362  if (!jm)
363  continue;
364  // ignore fixed joints, multi-dof joints (they should not even be in the message)
365  if (jm->getVariableCount() != 1)
366  continue;
367 
368  joint_time_[jm] = joint_state->header.stamp;
369 
370  if (robot_state_.getJointPositions(jm)[0] != joint_state->position[i])
371  {
372  update = true;
373  robot_state_.setJointPositions(jm, &(joint_state->position[i]));
374 
375  // optionally copy velocities and effort
376  if (copy_dynamics_)
377  {
378  // check if velocities exist
379  if (joint_state->name.size() == joint_state->velocity.size())
380  {
381  robot_state_.setJointVelocities(jm, &(joint_state->velocity[i]));
382 
383  // check if effort exist. assume they are not useful if no velocities were passed in
384  if (joint_state->name.size() == joint_state->effort.size())
385  {
386  robot_state_.setJointEfforts(jm, &(joint_state->effort[i]));
387  }
388  }
389  }
390 
391  // continuous joints wrap, so we don't modify them (even if they are outside bounds!)
393  if (static_cast<const moveit::core::RevoluteJointModel*>(jm)->isContinuous())
394  continue;
395 
396  const robot_model::VariableBounds& b =
397  jm->getVariableBounds()[0]; // only one variable in the joint, so we get its bounds
398 
399  // if the read variable is 'almost' within bounds (up to error_ difference), then consider it to be within
400  // bounds
401  if (joint_state->position[i] < b.min_position_ && joint_state->position[i] >= b.min_position_ - error_)
402  robot_state_.setJointPositions(jm, &b.min_position_);
403  else if (joint_state->position[i] > b.max_position_ && joint_state->position[i] <= b.max_position_ + error_)
404  robot_state_.setJointPositions(jm, &b.max_position_);
405  }
406  }
407  }
408 
409  // callbacks, if needed
410  if (update)
411  for (std::size_t i = 0; i < update_callbacks_.size(); ++i)
412  update_callbacks_[i](joint_state);
413 
414  // notify waitForCurrentState() *after* potential update callbacks
416 }
417 
419 {
420  // read multi-dof joint states from TF, if needed
421  const std::vector<const moveit::core::JointModel*>& multi_dof_joints = robot_model_->getMultiDOFJointModels();
422 
423  bool update = false;
424  bool changes = false;
425  {
426  boost::mutex::scoped_lock _(state_update_lock_);
427 
428  for (size_t i = 0; i < multi_dof_joints.size(); i++)
429  {
430  const moveit::core::JointModel* joint = multi_dof_joints[i];
431  const std::string& child_frame = joint->getChildLinkModel()->getName();
432  const std::string& parent_frame =
433  joint->getParentLinkModel() ? joint->getParentLinkModel()->getName() : robot_model_->getModelFrame();
434 
435  ros::Time latest_common_time;
436  std::string err;
437  if (tf_->getLatestCommonTime(parent_frame, child_frame, latest_common_time, &err) != tf::NO_ERROR)
438  {
439  ROS_WARN_STREAM_ONCE("Unable to update multi-DOF joint '"
440  << joint->getName() << "': TF has no common time between '" << parent_frame.c_str()
441  << "' and '" << child_frame.c_str() << "': " << err);
442  continue;
443  }
444 
445  // allow update if time is more recent or if it is a static transform (time = 0)
446  if (latest_common_time <= joint_time_[joint] && latest_common_time > ros::Time(0))
447  continue;
448 
449  tf::StampedTransform transf;
450  try
451  {
452  tf_->lookupTransform(parent_frame, child_frame, latest_common_time, transf);
453  }
454  catch (tf::TransformException& ex)
455  {
456  ROS_ERROR_STREAM_THROTTLE(1, "Unable to update multi-dof joint '" << joint->getName()
457  << "'. TF exception: " << ex.what());
458  continue;
459  }
460  joint_time_[joint] = latest_common_time;
461 
462  Eigen::Affine3d eigen_transf;
463  tf::transformTFToEigen(transf, eigen_transf);
464 
465  double new_values[joint->getStateSpaceDimension()];
466  const robot_model::LinkModel* link = joint->getChildLinkModel();
467  if (link->jointOriginTransformIsIdentity())
468  joint->computeVariablePositions(eigen_transf, new_values);
469  else
470  joint->computeVariablePositions(link->getJointOriginTransform().inverse(Eigen::Isometry) * eigen_transf,
471  new_values);
472 
473  if (joint->distance(new_values, robot_state_.getJointPositions(joint)) > 1e-5)
474  {
475  changes = true;
476  }
477 
478  robot_state_.setJointPositions(joint, new_values);
479  update = true;
480  }
481  }
482 
483  // callbacks, if needed
484  if (changes)
485  {
486  // stub joint state: multi-dof joints are not modelled in the message,
487  // but we should still trigger the update callbacks
488  sensor_msgs::JointStatePtr joint_state(new sensor_msgs::JointState);
489  for (std::size_t i = 0; i < update_callbacks_.size(); ++i)
490  update_callbacks_[i](joint_state);
491  }
492 
493  if (update)
494  {
495  // notify waitForCurrentState() *after* potential update callbacks
497  }
498 }
const std::string & getName() const
const std::string & getName() const
void notify_all() BOOST_NOEXCEPT
std::size_t getVariableCount() const
void stopStateMonitor()
Stop monitoring the "joint_states" topic.
boost::shared_ptr< tf::Transformer > tf_
#define ROS_ERROR_STREAM_THROTTLE(period, args)
void setToCurrentState(robot_state::RobotState &upd) const
Set the state upd to the current state maintained by this class.
CurrentStateMonitor(const robot_model::RobotModelConstPtr &robot_model, const boost::shared_ptr< tf::Transformer > &tf)
Constructor.
decltype(tf::Transformer().addTransformsChangedListener(boost::function< void(void)>())) typedef TFConnection
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
std::map< const moveit::core::JointModel *, ros::Time > joint_time_
std::string resolveName(const std::string &name, bool remap=true) const
bool waitForCurrentState(const ros::Time t=ros::Time::now(), double wait_time=1.0) const
Wait for at most wait_time seconds (default 1s) for a robot state more recent than t...
bool sleep() const
Monitors the joint_states topic and tf to maintain the current state of the robot.
robot_model::RobotModelConstPtr robot_model_
std::map< std::string, double > getCurrentStateValues() const
Get the current state values as a map from joint names to joint state values.
void update(const std::string &key, const XmlRpc::XmlRpcValue &v)
void startStateMonitor(const std::string &joint_states_topic="joint_states")
Start monitoring joint states on a particular topic.
std::pair< robot_state::RobotStatePtr, ros::Time > getCurrentStateAndTime() const
Get the current state and its time stamp.
std::string getMonitoredTopic() const
Get the name of the topic being monitored. Returns an empty string if the monitor is inactive...
virtual double distance(const double *value1, const double *value2) const =0
std::vector< JointStateUpdateCallback > update_callbacks_
void jointStateCallback(const sensor_msgs::JointStateConstPtr &joint_state)
#define ROS_ERROR_THROTTLE(period,...)
std::string getTopic() const
double epsilon
const std::vector< std::string > & getJointModelNames() const
std::shared_ptr< TFConnection > tf_connection_
bool waitForCompleteState(double wait_time) const
Wait for at most wait_time seconds until the complete robot state is known.
#define ROS_INFO_STREAM(args)
static WallTime now()
const LinkModel * getChildLinkModel() const
bool haveCompleteState() const
Query whether we have joint state information for all DOFs in the kinematic model.
#define ROS_WARN_STREAM_ONCE(args)
void clearUpdateCallbacks()
Clear the functions to be called when an update to the joint state is received.
static Time now()
const LinkModel * getParentLinkModel() const
void transformTFToEigen(const tf::Transform &t, Eigen::Isometry3d &e)
boost::function< void(const sensor_msgs::JointStateConstPtr &joint_state)> JointStateUpdateCallback
const VariableBounds & getVariableBounds(const std::string &variable) const
ros::Time getCurrentStateTime() const
Get the time stamp for the current state.
#define ROS_ERROR(...)
JointType getType() const
virtual void computeVariablePositions(const Eigen::Affine3d &transform, double *joint_values) const =0
void addUpdateCallback(const JointStateUpdateCallback &fn)
Add a function that will be called whenever the joint state is updated.
virtual unsigned int getStateSpaceDimension() const =0
NO_ERROR
bool isActive() const
Check if the state monitor is started.
#define ROS_DEBUG(...)
robot_state::RobotStatePtr getCurrentState() const
Get the current state.


planning
Author(s): Ioan Sucan , Sachin Chitta
autogenerated on Wed Jul 10 2019 04:03:32