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 }
106 
108 {
109  if (fn)
110  update_callbacks_.push_back(fn);
111 }
112 
114 {
115  update_callbacks_.clear();
116 }
117 
118 void planning_scene_monitor::CurrentStateMonitor::startStateMonitor(const std::string& joint_states_topic)
119 {
121  {
122  joint_time_.clear();
123  if (joint_states_topic.empty())
124  ROS_ERROR("The joint states topic cannot be an empty string");
125  else
127  if (tf_ && robot_model_->getMultiDOFJointModels().size() > 0)
128  {
129  tf_connection_.reset(
130  new TFConnection(tf_->addTransformsChangedListener(boost::bind(&CurrentStateMonitor::tfCallback, this))));
131  }
132  state_monitor_started_ = true;
134  ROS_DEBUG("Listening to joint states on topic '%s'", nh_.resolveName(joint_states_topic).c_str());
135  }
136 }
137 
139 {
140  return state_monitor_started_;
141 }
142 
144 {
146  {
148  if (tf_ && tf_connection_)
149  {
150  tf_->removeTransformsChangedListener(*tf_connection_);
151  tf_connection_.reset();
152  }
153  ROS_DEBUG("No longer listening for joint states");
154  state_monitor_started_ = false;
155  }
156 }
157 
159 {
162  else
163  return "";
164 }
165 
167 {
168  bool result = true;
169  const std::vector<const moveit::core::JointModel*>& joints = robot_model_->getActiveJointModels();
170  boost::mutex::scoped_lock slock(state_update_lock_);
171  for (const moveit::core::JointModel* joint : joints)
172  if (joint_time_.find(joint) == joint_time_.end())
173  {
174  if (!joint->isPassive() && !joint->getMimic())
175  {
176  ROS_DEBUG("Joint '%s' has never been updated", joint->getName().c_str());
177  result = false;
178  }
179  }
180  return result;
181 }
182 
183 bool planning_scene_monitor::CurrentStateMonitor::haveCompleteState(std::vector<std::string>& missing_states) const
184 {
185  bool result = true;
186  const std::vector<const moveit::core::JointModel*>& joints = robot_model_->getActiveJointModels();
187  boost::mutex::scoped_lock slock(state_update_lock_);
188  for (const moveit::core::JointModel* joint : joints)
189  if (joint_time_.find(joint) == joint_time_.end())
190  if (!joint->isPassive() && !joint->getMimic())
191  {
192  missing_states.push_back(joint->getName());
193  result = false;
194  }
195  return result;
196 }
197 
199 {
200  bool result = true;
201  const std::vector<const moveit::core::JointModel*>& joints = robot_model_->getActiveJointModels();
202  ros::Time now = ros::Time::now();
203  ros::Time old = now - age;
204  boost::mutex::scoped_lock slock(state_update_lock_);
205  for (const moveit::core::JointModel* joint : joints)
206  {
207  if (joint->isPassive() || joint->getMimic())
208  continue;
209  std::map<const moveit::core::JointModel*, ros::Time>::const_iterator it = joint_time_.find(joint);
210  if (it == joint_time_.end())
211  {
212  ROS_DEBUG("Joint '%s' has never been updated", joint->getName().c_str());
213  result = false;
214  }
215  else if (it->second < old)
216  {
217  ROS_DEBUG("Joint '%s' was last updated %0.3lf seconds ago (older than the allowed %0.3lf seconds)",
218  joint->getName().c_str(), (now - it->second).toSec(), age.toSec());
219  result = false;
220  }
221  }
222  return result;
223 }
224 
226  std::vector<std::string>& missing_states) const
227 {
228  bool result = true;
229  const std::vector<const moveit::core::JointModel*>& joints = robot_model_->getActiveJointModels();
230  ros::Time now = ros::Time::now();
231  ros::Time old = now - age;
232  boost::mutex::scoped_lock slock(state_update_lock_);
233  for (const moveit::core::JointModel* joint : joints)
234  {
235  if (joint->isPassive() || joint->getMimic())
236  continue;
237  std::map<const moveit::core::JointModel*, ros::Time>::const_iterator it = joint_time_.find(joint);
238  if (it == joint_time_.end())
239  {
240  ROS_DEBUG("Joint '%s' has never been updated", joint->getName().c_str());
241  missing_states.push_back(joint->getName());
242  result = false;
243  }
244  else if (it->second < old)
245  {
246  ROS_DEBUG("Joint '%s' was last updated %0.3lf seconds ago (older than the allowed %0.3lf seconds)",
247  joint->getName().c_str(), (now - it->second).toSec(), age.toSec());
248  missing_states.push_back(joint->getName());
249  result = false;
250  }
251  }
252  return result;
253 }
254 
256 {
258  ros::WallDuration elapsed(0, 0);
259  ros::WallDuration timeout(wait_time);
260 
261  boost::mutex::scoped_lock lock(state_update_lock_);
262  while (current_state_time_ < t)
263  {
264  state_update_condition_.wait_for(lock, boost::chrono::nanoseconds((timeout - elapsed).toNSec()));
265  elapsed = ros::WallTime::now() - start;
266  if (elapsed > timeout)
267  {
268  ROS_INFO_STREAM("Didn't received robot state (joint angles) with recent timestamp within "
269  << wait_time << " seconds.\n"
270  << "Check clock synchronization if your are running ROS across multiple machines!");
271  return false;
272  }
273  }
274  return true;
275 }
276 
278 {
279  double slept_time = 0.0;
280  double sleep_step_s = std::min(0.05, wait_time / 10.0);
281  ros::Duration sleep_step(sleep_step_s);
282  while (!haveCompleteState() && slept_time < wait_time)
283  {
284  sleep_step.sleep();
285  slept_time += sleep_step_s;
286  }
287  return haveCompleteState();
288 }
290 {
291  waitForCompleteState(wait_time);
292 }
293 
294 bool planning_scene_monitor::CurrentStateMonitor::waitForCompleteState(const std::string& group, double wait_time) const
295 {
296  if (waitForCompleteState(wait_time))
297  return true;
298  bool ok = true;
299 
300  // check to see if we have a fully known state for the joints we want to record
301  std::vector<std::string> missing_joints;
302  if (!haveCompleteState(missing_joints))
303  {
304  const moveit::core::JointModelGroup* jmg = robot_model_->getJointModelGroup(group);
305  if (jmg)
306  {
307  std::set<std::string> mj;
308  mj.insert(missing_joints.begin(), missing_joints.end());
309  const std::vector<std::string>& names = jmg->getJointModelNames();
310  bool ok = true;
311  for (std::size_t i = 0; ok && i < names.size(); ++i)
312  if (mj.find(names[i]) != mj.end())
313  ok = false;
314  }
315  else
316  ok = false;
317  }
318  return ok;
319 }
320 
321 bool planning_scene_monitor::CurrentStateMonitor::waitForCurrentState(const std::string& group, double wait_time) const
322 {
323  waitForCompleteState(group, wait_time);
324 }
325 
326 void planning_scene_monitor::CurrentStateMonitor::jointStateCallback(const sensor_msgs::JointStateConstPtr& joint_state)
327 {
328  if (joint_state->name.size() != joint_state->position.size())
329  {
330  ROS_ERROR_THROTTLE(1, "State monitor received invalid joint state (number of joint names does not match number of "
331  "positions)");
332  return;
333  }
334  bool update = false;
335 
336  {
337  boost::mutex::scoped_lock _(state_update_lock_);
338  // read the received values, and update their time stamps
339  std::size_t n = joint_state->name.size();
340  current_state_time_ = joint_state->header.stamp;
341  for (std::size_t i = 0; i < n; ++i)
342  {
343  const moveit::core::JointModel* jm = robot_model_->getJointModel(joint_state->name[i]);
344  if (!jm)
345  continue;
346  // ignore fixed joints, multi-dof joints (they should not even be in the message)
347  if (jm->getVariableCount() != 1)
348  continue;
349 
350  joint_time_[jm] = joint_state->header.stamp;
351 
352  if (robot_state_.getJointPositions(jm)[0] != joint_state->position[i])
353  {
354  update = true;
355  robot_state_.setJointPositions(jm, &(joint_state->position[i]));
356 
357  // optionally copy velocities and effort
358  if (copy_dynamics_)
359  {
360  // check if velocities exist
361  if (joint_state->name.size() == joint_state->velocity.size())
362  {
363  robot_state_.setJointVelocities(jm, &(joint_state->velocity[i]));
364 
365  // check if effort exist. assume they are not useful if no velocities were passed in
366  if (joint_state->name.size() == joint_state->effort.size())
367  {
368  robot_state_.setJointEfforts(jm, &(joint_state->effort[i]));
369  }
370  }
371  }
372 
373  // continuous joints wrap, so we don't modify them (even if they are outside bounds!)
375  if (static_cast<const moveit::core::RevoluteJointModel*>(jm)->isContinuous())
376  continue;
377 
378  const robot_model::VariableBounds& b =
379  jm->getVariableBounds()[0]; // only one variable in the joint, so we get its bounds
380 
381  // if the read variable is 'almost' within bounds (up to error_ difference), then consider it to be within
382  // bounds
383  if (joint_state->position[i] < b.min_position_ && joint_state->position[i] >= b.min_position_ - error_)
384  robot_state_.setJointPositions(jm, &b.min_position_);
385  else if (joint_state->position[i] > b.max_position_ && joint_state->position[i] <= b.max_position_ + error_)
386  robot_state_.setJointPositions(jm, &b.max_position_);
387  }
388  }
389  }
390 
391  // callbacks, if needed
392  if (update)
393  for (std::size_t i = 0; i < update_callbacks_.size(); ++i)
394  update_callbacks_[i](joint_state);
395 
396  // notify waitForCurrentState() *after* potential update callbacks
398 }
399 
401 {
402  // read multi-dof joint states from TF, if needed
403  const std::vector<const moveit::core::JointModel*>& multi_dof_joints = robot_model_->getMultiDOFJointModels();
404 
405  bool update = false;
406  bool changes = false;
407  {
408  boost::mutex::scoped_lock _(state_update_lock_);
409 
410  for (size_t i = 0; i < multi_dof_joints.size(); i++)
411  {
412  const moveit::core::JointModel* joint = multi_dof_joints[i];
413  const std::string& child_frame = joint->getChildLinkModel()->getName();
414  const std::string& parent_frame =
415  joint->getParentLinkModel() ? joint->getParentLinkModel()->getName() : robot_model_->getModelFrame();
416 
417  ros::Time latest_common_time;
418  std::string err;
419  if (tf_->getLatestCommonTime(parent_frame, child_frame, latest_common_time, &err) != tf::NO_ERROR)
420  {
421  ROS_WARN_STREAM_THROTTLE(1, "Unable to update multi-DOF joint '"
422  << joint->getName() << "': TF has no common time between '"
423  << parent_frame.c_str() << "' and '" << child_frame.c_str() << "': " << err);
424  continue;
425  }
426 
427  // allow update if time is more recent or if it is a static transform (time = 0)
428  if (latest_common_time <= joint_time_[joint] && latest_common_time > ros::Time(0))
429  continue;
430 
431  tf::StampedTransform transf;
432  try
433  {
434  tf_->lookupTransform(parent_frame, child_frame, latest_common_time, transf);
435  }
436  catch (tf::TransformException& ex)
437  {
438  ROS_ERROR_STREAM_THROTTLE(1, "Unable to update multi-dof joint '" << joint->getName()
439  << "'. TF exception: " << ex.what());
440  continue;
441  }
442  joint_time_[joint] = latest_common_time;
443 
444  Eigen::Affine3d eigen_transf;
445  tf::transformTFToEigen(transf, eigen_transf);
446 
447  double new_values[joint->getStateSpaceDimension()];
448  joint->computeVariablePositions(eigen_transf, new_values);
449 
450  if (joint->distance(new_values, robot_state_.getJointPositions(joint)) > 1e-5)
451  {
452  changes = true;
453  }
454 
455  robot_state_.setJointPositions(joint, new_values);
456  update = true;
457  }
458  }
459 
460  // callbacks, if needed
461  if (changes)
462  {
463  // stub joint state: multi-dof joints are not modelled in the message,
464  // but we should still trigger the update callbacks
465  sensor_msgs::JointStatePtr joint_state(new sensor_msgs::JointState);
466  for (std::size_t i = 0; i < update_callbacks_.size(); ++i)
467  update_callbacks_[i](joint_state);
468  }
469 
470  if (update)
471  {
472  // notify waitForCurrentState() *after* potential update callbacks
474  }
475 }
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_
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.
#define ROS_ERROR_THROTTLE(rate,...)
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)
std::string getTopic() const
double epsilon
const std::vector< std::string > & getJointModelNames() const
std::shared_ptr< TFConnection > tf_connection_
#define ROS_WARN_STREAM_THROTTLE(rate, args)
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.
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
#define ROS_ERROR_STREAM_THROTTLE(rate, args)
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 Sat Apr 21 2018 02:55:20