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


planning
Author(s): Ioan Sucan , Sachin Chitta
autogenerated on Tue Jun 8 2021 02:53:10