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


planning
Author(s): Ioan Sucan , Sachin Chitta
autogenerated on Sat Jan 18 2025 03:36:46