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 
39 #include <limits>
40 
41 planning_scene_monitor::CurrentStateMonitor::CurrentStateMonitor(const robot_model::RobotModelConstPtr& robot_model,
43  : CurrentStateMonitor(robot_model, tf, ros::NodeHandle())
44 {
45 }
46 
47 planning_scene_monitor::CurrentStateMonitor::CurrentStateMonitor(const robot_model::RobotModelConstPtr& robot_model,
49  ros::NodeHandle nh)
50  : nh_(nh)
51  , tf_(tf)
52  , robot_model_(robot_model)
53  , robot_state_(robot_model)
54  , state_monitor_started_(false)
55  , copy_dynamics_(false)
56  , error_(std::numeric_limits<double>::epsilon())
57 {
58  robot_state_.setToDefaultValues();
59 }
60 
62 {
64 }
65 
67 {
68  boost::mutex::scoped_lock slock(state_update_lock_);
69  robot_state::RobotState* result = new robot_state::RobotState(robot_state_);
70  return robot_state::RobotStatePtr(result);
71 }
72 
74 {
75  boost::mutex::scoped_lock slock(state_update_lock_);
76  return current_state_time_;
77 }
78 
79 std::pair<robot_state::RobotStatePtr, ros::Time>
81 {
82  boost::mutex::scoped_lock slock(state_update_lock_);
83  robot_state::RobotState* result = new robot_state::RobotState(robot_state_);
84  return std::make_pair(robot_state::RobotStatePtr(result), current_state_time_);
85 }
86 
88 {
89  std::map<std::string, double> m;
90  boost::mutex::scoped_lock slock(state_update_lock_);
91  const double* pos = robot_state_.getVariablePositions();
92  const std::vector<std::string>& names = robot_state_.getVariableNames();
93  for (std::size_t i = 0; i < names.size(); ++i)
94  m[names[i]] = pos[i];
95  return m;
96 }
97 
99 {
100  boost::mutex::scoped_lock slock(state_update_lock_);
101  const double* pos = robot_state_.getVariablePositions();
102  upd.setVariablePositions(pos);
103 }
104 
106 {
107  if (fn)
108  update_callbacks_.push_back(fn);
109 }
110 
112 {
113  update_callbacks_.clear();
114 }
115 
116 void planning_scene_monitor::CurrentStateMonitor::startStateMonitor(const std::string& joint_states_topic)
117 {
119  {
120  joint_time_.clear();
121  if (joint_states_topic.empty())
122  ROS_ERROR("The joint states topic cannot be an empty string");
123  else
125  state_monitor_started_ = true;
127  ROS_DEBUG("Listening to joint states on topic '%s'", nh_.resolveName(joint_states_topic).c_str());
128  }
129 }
130 
132 {
133  return state_monitor_started_;
134 }
135 
137 {
139  {
141  ROS_DEBUG("No longer listening o joint states");
142  state_monitor_started_ = false;
143  }
144 }
145 
147 {
150  else
151  return "";
152 }
153 
155 {
156  if (robot_model_->hasJointModel(dof))
157  {
158  if (robot_model_->getJointModel(dof)->isPassive() || robot_model_->getJointModel(dof)->getMimic())
159  return true;
160  }
161  else
162  {
163  // check if this DOF is part of a multi-dof passive joint
164  std::size_t slash = dof.find_last_of("/");
165  if (slash != std::string::npos)
166  {
167  std::string joint_name = dof.substr(0, slash);
168  if (robot_model_->hasJointModel(joint_name))
169  if (robot_model_->getJointModel(joint_name)->isPassive() || robot_model_->getJointModel(joint_name)->getMimic())
170  return true;
171  }
172  }
173  return false;
174 }
175 
177 {
178  bool result = true;
179  const std::vector<std::string>& dof = robot_model_->getVariableNames();
180  boost::mutex::scoped_lock slock(state_update_lock_);
181  for (std::size_t i = 0; i < dof.size(); ++i)
182  if (joint_time_.find(dof[i]) == joint_time_.end())
183  {
184  if (!isPassiveOrMimicDOF(dof[i]))
185  {
186  ROS_DEBUG("Joint variable '%s' has never been updated", dof[i].c_str());
187  result = false;
188  }
189  }
190  return result;
191 }
192 
193 bool planning_scene_monitor::CurrentStateMonitor::haveCompleteState(std::vector<std::string>& missing_states) const
194 {
195  bool result = true;
196  const std::vector<std::string>& dof = robot_model_->getVariableNames();
197  boost::mutex::scoped_lock slock(state_update_lock_);
198  for (std::size_t i = 0; i < dof.size(); ++i)
199  if (joint_time_.find(dof[i]) == joint_time_.end())
200  if (!isPassiveOrMimicDOF(dof[i]))
201  {
202  missing_states.push_back(dof[i]);
203  result = false;
204  }
205  return result;
206 }
207 
209 {
210  bool result = true;
211  const std::vector<std::string>& dof = robot_model_->getVariableNames();
212  ros::Time now = ros::Time::now();
213  ros::Time old = now - age;
214  boost::mutex::scoped_lock slock(state_update_lock_);
215  for (std::size_t i = 0; i < dof.size(); ++i)
216  {
217  if (isPassiveOrMimicDOF(dof[i]))
218  continue;
219  std::map<std::string, ros::Time>::const_iterator it = joint_time_.find(dof[i]);
220  if (it == joint_time_.end())
221  {
222  ROS_DEBUG("Joint variable '%s' has never been updated", dof[i].c_str());
223  result = false;
224  }
225  else if (it->second < old)
226  {
227  ROS_DEBUG("Joint variable '%s' was last updated %0.3lf seconds ago (older than the allowed %0.3lf seconds)",
228  dof[i].c_str(), (now - it->second).toSec(), age.toSec());
229  result = false;
230  }
231  }
232  return result;
233 }
234 
236  std::vector<std::string>& missing_states) const
237 {
238  bool result = true;
239  const std::vector<std::string>& dof = robot_model_->getVariableNames();
240  ros::Time now = ros::Time::now();
241  ros::Time old = now - age;
242  boost::mutex::scoped_lock slock(state_update_lock_);
243  for (std::size_t i = 0; i < dof.size(); ++i)
244  {
245  if (isPassiveOrMimicDOF(dof[i]))
246  continue;
247  std::map<std::string, ros::Time>::const_iterator it = joint_time_.find(dof[i]);
248  if (it == joint_time_.end())
249  {
250  ROS_DEBUG("Joint variable '%s' has never been updated", dof[i].c_str());
251  missing_states.push_back(dof[i]);
252  result = false;
253  }
254  else if (it->second < old)
255  {
256  ROS_DEBUG("Joint variable '%s' was last updated %0.3lf seconds ago (older than the allowed %0.3lf seconds)",
257  dof[i].c_str(), (now - it->second).toSec(), age.toSec());
258  missing_states.push_back(dof[i]);
259  result = false;
260  }
261  }
262  return result;
263 }
264 
266 {
268  ros::WallDuration elapsed(0, 0);
269  ros::WallDuration timeout(wait_time);
270 
271  boost::mutex::scoped_lock lock(state_update_lock_);
272  while (current_state_time_ < t)
273  {
274  state_update_condition_.wait_for(lock, boost::chrono::nanoseconds((timeout - elapsed).toNSec()));
275  elapsed = ros::WallTime::now() - start;
276  if (elapsed > timeout)
277  return false;
278  }
279  return true;
280 }
281 
283 {
284  double slept_time = 0.0;
285  double sleep_step_s = std::min(0.05, wait_time / 10.0);
286  ros::Duration sleep_step(sleep_step_s);
287  while (!haveCompleteState() && slept_time < wait_time)
288  {
289  sleep_step.sleep();
290  slept_time += sleep_step_s;
291  }
292  return haveCompleteState();
293 }
295 {
296  waitForCompleteState(wait_time);
297 }
298 
299 bool planning_scene_monitor::CurrentStateMonitor::waitForCompleteState(const std::string& group, double wait_time) const
300 {
301  if (waitForCompleteState(wait_time))
302  return true;
303  bool ok = true;
304 
305  // check to see if we have a fully known state for the joints we want to record
306  std::vector<std::string> missing_joints;
307  if (!haveCompleteState(missing_joints))
308  {
309  const robot_model::JointModelGroup* jmg = robot_model_->getJointModelGroup(group);
310  if (jmg)
311  {
312  std::set<std::string> mj;
313  mj.insert(missing_joints.begin(), missing_joints.end());
314  const std::vector<std::string>& names = jmg->getJointModelNames();
315  bool ok = true;
316  for (std::size_t i = 0; ok && i < names.size(); ++i)
317  if (mj.find(names[i]) != mj.end())
318  ok = false;
319  }
320  else
321  ok = false;
322  }
323  return ok;
324 }
325 
326 bool planning_scene_monitor::CurrentStateMonitor::waitForCurrentState(const std::string& group, double wait_time) const
327 {
328  waitForCompleteState(group, wait_time);
329 }
330 
331 void planning_scene_monitor::CurrentStateMonitor::jointStateCallback(const sensor_msgs::JointStateConstPtr& joint_state)
332 {
333  if (joint_state->name.size() != joint_state->position.size())
334  {
335  ROS_ERROR_THROTTLE(1, "State monitor received invalid joint state (number of joint names does not match number of "
336  "positions)");
337  return;
338  }
339  bool update = false;
340 
341  {
342  boost::mutex::scoped_lock _(state_update_lock_);
343  // read the received values, and update their time stamps
344  std::size_t n = joint_state->name.size();
345  current_state_time_ = joint_state->header.stamp;
346  for (std::size_t i = 0; i < n; ++i)
347  {
348  const robot_model::JointModel* jm = robot_model_->getJointModel(joint_state->name[i]);
349  if (!jm)
350  continue;
351  // ignore fixed joints, multi-dof joints (they should not even be in the message)
352  if (jm->getVariableCount() != 1)
353  continue;
354 
355  joint_time_[joint_state->name[i]] = joint_state->header.stamp;
356 
357  if (robot_state_.getJointPositions(jm)[0] != joint_state->position[i])
358  {
359  update = true;
360  robot_state_.setJointPositions(jm, &(joint_state->position[i]));
361 
362  // optionally copy velocities and effort
363  if (copy_dynamics_)
364  {
365  // check if velocities exist
366  if (joint_state->name.size() == joint_state->velocity.size())
367  {
368  robot_state_.setJointVelocities(jm, &(joint_state->velocity[i]));
369 
370  // check if effort exist. assume they are not useful if no velocities were passed in
371  if (joint_state->name.size() == joint_state->effort.size())
372  {
373  robot_state_.setJointEfforts(jm, &(joint_state->effort[i]));
374  }
375  }
376  }
377 
378  // continuous joints wrap, so we don't modify them (even if they are outside bounds!)
379  if (jm->getType() == robot_model::JointModel::REVOLUTE)
380  if (static_cast<const robot_model::RevoluteJointModel*>(jm)->isContinuous())
381  continue;
382 
383  const robot_model::VariableBounds& b =
384  jm->getVariableBounds()[0]; // only one variable in the joint, so we get its bounds
385 
386  // if the read variable is 'almost' within bounds (up to error_ difference), then consider it to be within
387  // bounds
388  if (joint_state->position[i] < b.min_position_ && joint_state->position[i] >= b.min_position_ - error_)
389  robot_state_.setJointPositions(jm, &b.min_position_);
390  else if (joint_state->position[i] > b.max_position_ && joint_state->position[i] <= b.max_position_ + error_)
391  robot_state_.setJointPositions(jm, &b.max_position_);
392  }
393  }
394 
395  // read root transform, if needed
396  if (tf_ && (robot_model_->getRootJoint()->getType() == robot_model::JointModel::PLANAR ||
397  robot_model_->getRootJoint()->getType() == robot_model::JointModel::FLOATING))
398  {
399  const std::string& child_frame = robot_model_->getRootLink()->getName();
400  const std::string& parent_frame = robot_model_->getModelFrame();
401 
402  std::string err;
403  ros::Time tm;
404  tf::StampedTransform transf;
405  bool ok = false;
406  if (tf_->getLatestCommonTime(parent_frame, child_frame, tm, &err) == tf::NO_ERROR)
407  {
408  try
409  {
410  tf_->lookupTransform(parent_frame, child_frame, tm, transf);
411  ok = true;
412  }
413  catch (tf::TransformException& ex)
414  {
415  ROS_ERROR_THROTTLE(1, "Unable to lookup transform from %s to %s. Exception: %s", parent_frame.c_str(),
416  child_frame.c_str(), ex.what());
417  }
418  }
419  else
420  ROS_DEBUG_THROTTLE(1, "Unable to lookup transform from %s to %s: no common time.", parent_frame.c_str(),
421  child_frame.c_str());
422  if (ok && last_tf_update_ != tm)
423  {
424  update = true;
425  last_tf_update_ = tm;
426  const std::vector<std::string>& vars = robot_model_->getRootJoint()->getVariableNames();
427  for (std::size_t j = 0; j < vars.size(); ++j)
428  joint_time_[vars[j]] = tm;
429  Eigen::Affine3d eigen_transf;
430  tf::transformTFToEigen(transf, eigen_transf);
431  robot_state_.setJointPositions(robot_model_->getRootJoint(), eigen_transf);
432  }
433  }
434  }
435 
436  // callbacks, if needed
437  if (update)
438  for (std::size_t i = 0; i < update_callbacks_.size(); ++i)
439  update_callbacks_[i](joint_state);
440 
441  // notify waitForCurrentState() *after* potential update callbacks
443 }
void notify_all() BOOST_NOEXCEPT
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.
#define ROS_DEBUG_THROTTLE(rate,...)
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
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...
std::vector< JointStateUpdateCallback > update_callbacks_
bool isPassiveOrMimicDOF(const std::string &dof) const
void jointStateCallback(const sensor_msgs::JointStateConstPtr &joint_state)
std::string getTopic() const
double epsilon
bool waitForCompleteState(double wait_time) const
Wait for at most wait_time seconds until the complete robot state is known.
static WallTime now()
bool haveCompleteState() const
Query whether we have joint state information for all DOFs in the kinematic model.
std::map< std::string, ros::Time > joint_time_
void clearUpdateCallbacks()
Clear the functions to be called when an update to the joint state is received.
static Time now()
void transformTFToEigen(const tf::Transform &t, Eigen::Isometry3d &e)
boost::function< void(const sensor_msgs::JointStateConstPtr &joint_state)> JointStateUpdateCallback
ros::Time getCurrentStateTime() const
Get the time stamp for the current state.
#define ROS_ERROR(...)
void addUpdateCallback(const JointStateUpdateCallback &fn)
Add a function that will be called whenever the joint state is updated.
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 Sun Jan 21 2018 03:55:10