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 
39 #include <tf2_eigen/tf2_eigen.h>
41 
42 #include <limits>
43 
44 planning_scene_monitor::CurrentStateMonitor::CurrentStateMonitor(const robot_model::RobotModelConstPtr& robot_model,
45  const std::shared_ptr<tf2_ros::Buffer>& tf_buffer)
46  : CurrentStateMonitor(robot_model, tf_buffer, ros::NodeHandle())
47 {
48 }
49 
50 planning_scene_monitor::CurrentStateMonitor::CurrentStateMonitor(const robot_model::RobotModelConstPtr& robot_model,
51  const std::shared_ptr<tf2_ros::Buffer>& tf_buffer,
52  const ros::NodeHandle& nh)
53  : nh_(nh)
54  , tf_buffer_(tf_buffer)
55  , robot_model_(robot_model)
56  , robot_state_(robot_model)
57  , state_monitor_started_(false)
58  , copy_dynamics_(false)
59  , error_(std::numeric_limits<double>::epsilon())
60 {
61  robot_state_.setToDefaultValues();
62 }
63 
65 {
67 }
68 
70 {
71  boost::mutex::scoped_lock slock(state_update_lock_);
72  robot_state::RobotState* result = new robot_state::RobotState(robot_state_);
73  return robot_state::RobotStatePtr(result);
74 }
75 
77 {
78  boost::mutex::scoped_lock slock(state_update_lock_);
79  return current_state_time_;
80 }
81 
82 std::pair<robot_state::RobotStatePtr, ros::Time>
84 {
85  boost::mutex::scoped_lock slock(state_update_lock_);
86  robot_state::RobotState* result = new robot_state::RobotState(robot_state_);
87  return std::make_pair(robot_state::RobotStatePtr(result), current_state_time_);
88 }
89 
91 {
92  std::map<std::string, double> m;
93  boost::mutex::scoped_lock slock(state_update_lock_);
94  const double* pos = robot_state_.getVariablePositions();
95  const std::vector<std::string>& names = robot_state_.getVariableNames();
96  for (std::size_t i = 0; i < names.size(); ++i)
97  m[names[i]] = pos[i];
98  return m;
99 }
100 
102 {
103  boost::mutex::scoped_lock slock(state_update_lock_);
104  const double* pos = robot_state_.getVariablePositions();
105  upd.setVariablePositions(pos);
106  if (copy_dynamics_)
107  {
108  if (robot_state_.hasVelocities())
109  {
110  const double* vel = robot_state_.getVariableVelocities();
111  upd.setVariableVelocities(vel);
112  }
113  if (robot_state_.hasAccelerations())
114  {
115  const double* acc = robot_state_.getVariableAccelerations();
116  upd.setVariableAccelerations(acc);
117  }
118  if (robot_state_.hasEffort())
119  {
120  const double* eff = robot_state_.getVariableEffort();
121  upd.setVariableEffort(eff);
122  }
123  }
124 }
125 
127 {
128  if (fn)
129  update_callbacks_.push_back(fn);
130 }
131 
133 {
134  update_callbacks_.clear();
135 }
136 
137 void planning_scene_monitor::CurrentStateMonitor::startStateMonitor(const std::string& joint_states_topic)
138 {
140  {
141  joint_time_.clear();
142  if (joint_states_topic.empty())
143  ROS_ERROR("The joint states topic cannot be an empty string");
144  else
146  if (tf_buffer_ && !robot_model_->getMultiDOFJointModels().empty())
147  {
148  tf_connection_.reset(new TFConnection(
149  tf_buffer_->_addTransformsChangedListener(boost::bind(&CurrentStateMonitor::tfCallback, this))));
150  }
151  state_monitor_started_ = true;
153  ROS_DEBUG("Listening to joint states on topic '%s'", nh_.resolveName(joint_states_topic).c_str());
154  }
155 }
156 
158 {
159  return state_monitor_started_;
160 }
161 
163 {
165  {
167  if (tf_buffer_ && tf_connection_)
168  {
169  tf_buffer_->_removeTransformsChangedListener(*tf_connection_);
170  tf_connection_.reset();
171  }
172  ROS_DEBUG("No longer listening for joint states");
173  state_monitor_started_ = false;
174  }
175 }
176 
178 {
181  else
182  return "";
183 }
184 
186 {
187  bool result = true;
188  const std::vector<const moveit::core::JointModel*>& joints = robot_model_->getActiveJointModels();
189  boost::mutex::scoped_lock slock(state_update_lock_);
190  for (const moveit::core::JointModel* joint : joints)
191  if (joint_time_.find(joint) == joint_time_.end())
192  {
193  if (!joint->isPassive() && !joint->getMimic())
194  {
195  ROS_DEBUG("Joint '%s' has never been updated", joint->getName().c_str());
196  result = false;
197  }
198  }
199  return result;
200 }
201 
202 bool planning_scene_monitor::CurrentStateMonitor::haveCompleteState(std::vector<std::string>& missing_states) const
203 {
204  bool result = true;
205  const std::vector<const moveit::core::JointModel*>& joints = robot_model_->getActiveJointModels();
206  boost::mutex::scoped_lock slock(state_update_lock_);
207  for (const moveit::core::JointModel* joint : joints)
208  if (joint_time_.find(joint) == joint_time_.end())
209  if (!joint->isPassive() && !joint->getMimic())
210  {
211  missing_states.push_back(joint->getName());
212  result = false;
213  }
214  return result;
215 }
216 
218 {
219  bool result = true;
220  const std::vector<const moveit::core::JointModel*>& joints = robot_model_->getActiveJointModels();
221  ros::Time now = ros::Time::now();
222  ros::Time old = now - age;
223  boost::mutex::scoped_lock slock(state_update_lock_);
224  for (const moveit::core::JointModel* joint : joints)
225  {
226  if (joint->isPassive() || joint->getMimic())
227  continue;
228  std::map<const moveit::core::JointModel*, ros::Time>::const_iterator it = joint_time_.find(joint);
229  if (it == joint_time_.end())
230  {
231  ROS_DEBUG("Joint '%s' has never been updated", joint->getName().c_str());
232  result = false;
233  }
234  else if (it->second < old)
235  {
236  ROS_DEBUG("Joint '%s' was last updated %0.3lf seconds ago (older than the allowed %0.3lf seconds)",
237  joint->getName().c_str(), (now - it->second).toSec(), age.toSec());
238  result = false;
239  }
240  }
241  return result;
242 }
243 
245  std::vector<std::string>& missing_states) const
246 {
247  bool result = true;
248  const std::vector<const moveit::core::JointModel*>& joints = robot_model_->getActiveJointModels();
249  ros::Time now = ros::Time::now();
250  ros::Time old = now - age;
251  boost::mutex::scoped_lock slock(state_update_lock_);
252  for (const moveit::core::JointModel* joint : joints)
253  {
254  if (joint->isPassive() || joint->getMimic())
255  continue;
256  std::map<const moveit::core::JointModel*, ros::Time>::const_iterator it = joint_time_.find(joint);
257  if (it == joint_time_.end())
258  {
259  ROS_DEBUG("Joint '%s' has never been updated", joint->getName().c_str());
260  missing_states.push_back(joint->getName());
261  result = false;
262  }
263  else if (it->second < old)
264  {
265  ROS_DEBUG("Joint '%s' was last updated %0.3lf seconds ago (older than the allowed %0.3lf seconds)",
266  joint->getName().c_str(), (now - it->second).toSec(), age.toSec());
267  missing_states.push_back(joint->getName());
268  result = false;
269  }
270  }
271  return result;
272 }
273 
275 {
277  ros::WallDuration elapsed(0, 0);
278  ros::WallDuration timeout(wait_time);
279 
280  boost::mutex::scoped_lock lock(state_update_lock_);
281  while (current_state_time_ < t)
282  {
283  state_update_condition_.wait_for(lock, boost::chrono::nanoseconds((timeout - elapsed).toNSec()));
284  elapsed = ros::WallTime::now() - start;
285  if (elapsed > timeout)
286  {
287  ROS_INFO_STREAM("Didn't received robot state (joint angles) with recent timestamp within "
288  << wait_time << " seconds.\n"
289  << "Check clock synchronization if your are running ROS across multiple machines!");
290  return false;
291  }
292  }
293  return true;
294 }
295 
297 {
298  double slept_time = 0.0;
299  double sleep_step_s = std::min(0.05, wait_time / 10.0);
300  ros::Duration sleep_step(sleep_step_s);
301  while (!haveCompleteState() && slept_time < wait_time)
302  {
303  sleep_step.sleep();
304  slept_time += sleep_step_s;
305  }
306  return haveCompleteState();
307 }
308 
309 bool planning_scene_monitor::CurrentStateMonitor::waitForCompleteState(const std::string& group, double wait_time) const
310 {
311  if (waitForCompleteState(wait_time))
312  return true;
313  bool ok = true;
314 
315  // check to see if we have a fully known state for the joints we want to record
316  std::vector<std::string> missing_joints;
317  if (!haveCompleteState(missing_joints))
318  {
319  const moveit::core::JointModelGroup* jmg = robot_model_->getJointModelGroup(group);
320  if (jmg)
321  {
322  std::set<std::string> mj;
323  mj.insert(missing_joints.begin(), missing_joints.end());
324  const std::vector<std::string>& names = jmg->getJointModelNames();
325  bool ok = true;
326  for (std::size_t i = 0; ok && i < names.size(); ++i)
327  if (mj.find(names[i]) != mj.end())
328  ok = false;
329  }
330  else
331  ok = false;
332  }
333  return ok;
334 }
335 
336 void planning_scene_monitor::CurrentStateMonitor::jointStateCallback(const sensor_msgs::JointStateConstPtr& joint_state)
337 {
338  if (joint_state->name.size() != joint_state->position.size())
339  {
340  ROS_ERROR_THROTTLE(1, "State monitor received invalid joint state (number of joint names does not match number of "
341  "positions)");
342  return;
343  }
344  bool update = false;
345 
346  {
347  boost::mutex::scoped_lock _(state_update_lock_);
348  // read the received values, and update their time stamps
349  std::size_t n = joint_state->name.size();
350  current_state_time_ = joint_state->header.stamp;
351  for (std::size_t i = 0; i < n; ++i)
352  {
353  const moveit::core::JointModel* jm = robot_model_->getJointModel(joint_state->name[i]);
354  if (!jm)
355  continue;
356  // ignore fixed joints, multi-dof joints (they should not even be in the message)
357  if (jm->getVariableCount() != 1)
358  continue;
359 
360  joint_time_[jm] = joint_state->header.stamp;
361 
362  if (robot_state_.getJointPositions(jm)[0] != joint_state->position[i])
363  {
364  update = true;
365  robot_state_.setJointPositions(jm, &(joint_state->position[i]));
366 
367  // continuous joints wrap, so we don't modify them (even if they are outside bounds!)
369  if (static_cast<const moveit::core::RevoluteJointModel*>(jm)->isContinuous())
370  continue;
371 
372  const robot_model::VariableBounds& b =
373  jm->getVariableBounds()[0]; // only one variable in the joint, so we get its bounds
374 
375  // if the read variable is 'almost' within bounds (up to error_ difference), then consider it to be within
376  // bounds
377  if (joint_state->position[i] < b.min_position_ && joint_state->position[i] >= b.min_position_ - error_)
378  robot_state_.setJointPositions(jm, &b.min_position_);
379  else if (joint_state->position[i] > b.max_position_ && joint_state->position[i] <= b.max_position_ + error_)
380  robot_state_.setJointPositions(jm, &b.max_position_);
381  }
382 
383  // optionally copy velocities and effort
384  if (copy_dynamics_)
385  {
386  // update joint velocities
387  if (joint_state->name.size() == joint_state->velocity.size() &&
388  (!robot_state_.hasVelocities() || robot_state_.getJointVelocities(jm)[0] != joint_state->velocity[i]))
389  {
390  update = true;
391  robot_state_.setJointVelocities(jm, &(joint_state->velocity[i]));
392  }
393 
394  // update joint efforts
395  if (joint_state->name.size() == joint_state->effort.size() &&
396  (!robot_state_.hasEffort() || robot_state_.getJointEffort(jm)[0] != joint_state->effort[i]))
397  {
398  update = true;
399  robot_state_.setJointEfforts(jm, &(joint_state->effort[i]));
400  }
401  }
402  }
403  }
404 
405  // callbacks, if needed
406  if (update)
407  for (std::size_t i = 0; i < update_callbacks_.size(); ++i)
408  update_callbacks_[i](joint_state);
409 
410  // notify waitForCurrentState() *after* potential update callbacks
411  state_update_condition_.notify_all();
412 }
413 
415 {
416  // read multi-dof joint states from TF, if needed
417  const std::vector<const moveit::core::JointModel*>& multi_dof_joints = robot_model_->getMultiDOFJointModels();
418 
419  bool update = false;
420  bool changes = false;
421  {
422  boost::mutex::scoped_lock _(state_update_lock_);
423 
424  for (size_t i = 0; i < multi_dof_joints.size(); i++)
425  {
426  const moveit::core::JointModel* joint = multi_dof_joints[i];
427  const std::string& child_frame = joint->getChildLinkModel()->getName();
428  const std::string& parent_frame =
429  joint->getParentLinkModel() ? joint->getParentLinkModel()->getName() : robot_model_->getModelFrame();
430 
431  ros::Time latest_common_time;
432  geometry_msgs::TransformStamped transf;
433  try
434  {
435  transf = tf_buffer_->lookupTransform(parent_frame, child_frame, ros::Time(0.0));
436  latest_common_time = transf.header.stamp;
437  }
438  catch (tf2::TransformException& ex)
439  {
440  ROS_WARN_STREAM_ONCE("Unable to update multi-DOF joint '"
441  << joint->getName() << "': Failure to lookup transform between '" << parent_frame.c_str()
442  << "' and '" << child_frame.c_str() << "' with TF exception: " << ex.what());
443  continue;
444  }
445 
446  // allow update if time is more recent or if it is a static transform (time = 0)
447  if (latest_common_time <= joint_time_[joint] && latest_common_time > ros::Time(0))
448  continue;
449  joint_time_[joint] = latest_common_time;
450 
451  std::vector<double> new_values(joint->getStateSpaceDimension());
452  const robot_model::LinkModel* link = joint->getChildLinkModel();
453  if (link->jointOriginTransformIsIdentity())
454  joint->computeVariablePositions(tf2::transformToEigen(transf), new_values.data());
455  else
456  joint->computeVariablePositions(link->getJointOriginTransform().inverse() * tf2::transformToEigen(transf),
457  new_values.data());
458 
459  if (joint->distance(new_values.data(), robot_state_.getJointPositions(joint)) > 1e-5)
460  {
461  changes = true;
462  }
463 
464  robot_state_.setJointPositions(joint, new_values.data());
465  update = true;
466  }
467  }
468 
469  // callbacks, if needed
470  if (changes)
471  {
472  // stub joint state: multi-dof joints are not modelled in the message,
473  // but we should still trigger the update callbacks
474  sensor_msgs::JointStatePtr joint_state(new sensor_msgs::JointState);
475  for (std::size_t i = 0; i < update_callbacks_.size(); ++i)
476  update_callbacks_[i](joint_state);
477  }
478 
479  if (update)
480  {
481  // notify waitForCurrentState() *after* potential update callbacks
482  state_update_condition_.notify_all();
483  }
484 }
void setToCurrentState(robot_state::RobotState &upd) const
Set the state upd to the current state maintained by this class.
void stopStateMonitor()
Stop monitoring the "joint_states" topic.
bool haveCompleteState() const
Query whether we have joint state information for all DOFs in the kinematic model.
std::string getTopic() const
ros::Time getCurrentStateTime() const
Get the time stamp for the current state.
ROSCPP_DECL void start()
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_
bool isActive() const
Check if the state monitor is started.
Monitors the joint_states topic and tf to maintain the current state of the robot.
#define ROS_ERROR(...)
robot_model::RobotModelConstPtr robot_model_
const std::vector< std::string > & getJointModelNames() const
#define ROS_DEBUG(...)
CurrentStateMonitor(const robot_model::RobotModelConstPtr &robot_model, const std::shared_ptr< tf2_ros::Buffer > &tf_buffer)
Constructor.
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.
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,...)
virtual void computeVariablePositions(const Eigen::Isometry3d &transform, double *joint_values) const=0
std::size_t getVariableCount() const
double epsilon
ROSCPP_DECL bool ok()
std::string resolveName(const std::string &name, bool remap=true) const
const LinkModel * getChildLinkModel() const
const std::string & getName() const
std::shared_ptr< TFConnection > tf_connection_
std::shared_ptr< tf2_ros::Buffer > tf_buffer_
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...
const VariableBounds & getVariableBounds(const std::string &variable) const
#define ROS_INFO_STREAM(args)
static WallTime now()
robot_state::RobotStatePtr getCurrentState() const
Get the current state.
bool waitForCompleteState(double wait_time) const
Wait for at most wait_time seconds until the complete robot state is known.
#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()
bool sleep() const
boost::function< void(const sensor_msgs::JointStateConstPtr &joint_state)> JointStateUpdateCallback
std::map< std::string, double > getCurrentStateValues() const
Get the current state values as a map from joint names to joint state values.
const LinkModel * getParentLinkModel() const
JointType getType() const
std::string getMonitoredTopic() const
Get the name of the topic being monitored. Returns an empty string if the monitor is inactive...
void addUpdateCallback(const JointStateUpdateCallback &fn)
Add a function that will be called whenever the joint state is updated.
virtual unsigned int getStateSpaceDimension() const=0
std::pair< robot_state::RobotStatePtr, ros::Time > getCurrentStateAndTime() const
Get the current state and its time stamp.
Eigen::Isometry3d transformToEigen(const geometry_msgs::Transform &t)
const std::string & getName() const


planning
Author(s): Ioan Sucan , Sachin Chitta
autogenerated on Thu Jan 14 2021 03:59:21