planning_scene_monitor.h
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 
37 #ifndef MOVEIT_PLANNING_SCENE_MONITOR_PLANNING_SCENE_MONITOR_
38 #define MOVEIT_PLANNING_SCENE_MONITOR_PLANNING_SCENE_MONITOR_
39 
40 #include <ros/ros.h>
41 #include <tf/tf.h>
42 #include <tf/message_filter.h>
50 #include <boost/noncopyable.hpp>
51 #include <boost/thread/shared_mutex.hpp>
52 #include <boost/thread/recursive_mutex.hpp>
53 #include <memory>
54 
55 namespace planning_scene_monitor
56 {
57 MOVEIT_CLASS_FORWARD(PlanningSceneMonitor);
58 
62 class PlanningSceneMonitor : private boost::noncopyable
63 {
64 public:
66  {
69 
72 
75 
79 
82  };
83 
85  static const std::string DEFAULT_JOINT_STATES_TOPIC; // "/joint_states"
86 
88  static const std::string DEFAULT_ATTACHED_COLLISION_OBJECT_TOPIC; // "/attached_collision_object"
89 
91  static const std::string DEFAULT_COLLISION_OBJECT_TOPIC; // "/collision_object"
92 
95  static const std::string DEFAULT_PLANNING_SCENE_WORLD_TOPIC; // "/planning_scene_world"
96 
98  static const std::string DEFAULT_PLANNING_SCENE_TOPIC; // "/planning_scene"
99 
101  static const std::string DEFAULT_PLANNING_SCENE_SERVICE; // "/get_planning_scene"
102 
105  static const std::string MONITORED_PLANNING_SCENE_TOPIC; // "monitored_planning_scene"
106 
112  PlanningSceneMonitor(const std::string& robot_description,
114  const std::string& name = "");
115 
121  PlanningSceneMonitor(const robot_model_loader::RobotModelLoaderPtr& rml,
123  const std::string& name = "");
124 
131  PlanningSceneMonitor(const planning_scene::PlanningScenePtr& scene, const std::string& robot_description,
133  const std::string& name = "");
134 
141  PlanningSceneMonitor(const planning_scene::PlanningScenePtr& scene,
142  const robot_model_loader::RobotModelLoaderPtr& rml,
144  const std::string& name = "");
145 
147 
149  const std::string& getName() const
150  {
151  return monitor_name_;
152  }
153 
155  const robot_model_loader::RobotModelLoaderPtr& getRobotModelLoader() const
156  {
157  return rm_loader_;
158  }
159 
160  const robot_model::RobotModelConstPtr& getRobotModel() const
161  {
162  return robot_model_;
163  }
164 
179  const planning_scene::PlanningScenePtr& getPlanningScene()
180  {
181  return scene_;
182  }
183 
187  const planning_scene::PlanningSceneConstPtr& getPlanningScene() const
188  {
189  return scene_const_;
190  }
191 
196  bool updatesScene(const planning_scene::PlanningSceneConstPtr& scene) const;
197 
202  bool updatesScene(const planning_scene::PlanningScenePtr& scene) const;
203 
206  const std::string& getRobotDescription() const
207  {
208  return robot_description_;
209  }
210 
212  double getDefaultRobotPadding() const
213  {
214  return default_robot_padd_;
215  }
216 
218  double getDefaultRobotScale() const
219  {
220  return default_robot_scale_;
221  }
222 
224  double getDefaultObjectPadding() const
225  {
226  return default_object_padd_;
227  }
228 
231  {
232  return default_attached_padd_;
233  }
234 
237  {
238  return tf_;
239  }
240 
246  void monitorDiffs(bool flag);
247 
252  const std::string& planning_scene_topic = MONITORED_PLANNING_SCENE_TOPIC);
253 
256 
258  void setPlanningScenePublishingFrequency(double hz);
259 
262  {
264  }
265 
268  const CurrentStateMonitorPtr& getStateMonitor() const
269  {
270  return current_state_monitor_;
271  }
272 
273  CurrentStateMonitorPtr& getStateMonitorNonConst()
274  {
275  return current_state_monitor_;
276  }
277 
283  void updateFrameTransforms();
284 
288  void startStateMonitor(const std::string& joint_states_topic = DEFAULT_JOINT_STATES_TOPIC,
289  const std::string& attached_objects_topic = DEFAULT_ATTACHED_COLLISION_OBJECT_TOPIC);
290 
292  void stopStateMonitor();
293 
298 
304  void setStateUpdateFrequency(double hz);
305 
307  double getStateUpdateFrequency() const
308  {
309  if (!dt_state_update_.isZero())
310  return 1.0 / dt_state_update_.toSec();
311  else
312  return 0.0;
313  }
314 
318  void startSceneMonitor(const std::string& scene_topic = DEFAULT_PLANNING_SCENE_TOPIC);
319 
326  bool requestPlanningSceneState(const std::string& service_name = DEFAULT_PLANNING_SCENE_SERVICE);
327 
329  void stopSceneMonitor();
330 
337  void startWorldGeometryMonitor(const std::string& collision_objects_topic = DEFAULT_COLLISION_OBJECT_TOPIC,
338  const std::string& planning_scene_world_topic = DEFAULT_PLANNING_SCENE_WORLD_TOPIC,
339  const bool load_octomap_monitor = true);
340 
343 
345  void addUpdateCallback(const boost::function<void(SceneUpdateType)>& fn);
346 
348  void clearUpdateCallbacks();
349 
351  void getMonitoredTopics(std::vector<std::string>& topics) const;
352 
355  {
356  return last_update_time_;
357  }
358 
359  void publishDebugInformation(bool flag);
360 
362  void triggerSceneUpdateEvent(SceneUpdateType update_type);
363 
369  bool waitForCurrentRobotState(const ros::Time& t, double wait_time = 1.);
370 
372  void lockSceneRead();
373 
375  void unlockSceneRead();
376 
379  void lockSceneWrite();
380 
383  void unlockSceneWrite();
384 
385  void clearOctomap();
386 
387  // Called to update the planning scene with a new message.
388  bool newPlanningSceneMessage(const moveit_msgs::PlanningScene& scene);
389 
390 protected:
394  void initialize(const planning_scene::PlanningScenePtr& scene);
395 
397  void configureCollisionMatrix(const planning_scene::PlanningScenePtr& scene);
398 
401 
403  void collisionObjectCallback(const moveit_msgs::CollisionObjectConstPtr& obj);
404 
406  void collisionObjectFailTFCallback(const moveit_msgs::CollisionObjectConstPtr& obj,
408 
410  void newPlanningSceneWorldCallback(const moveit_msgs::PlanningSceneWorldConstPtr& world);
411 
413  void octomapUpdateCallback();
414 
416  void attachObjectCallback(const moveit_msgs::AttachedCollisionObjectConstPtr& obj);
417 
419  void currentStateAttachedBodyUpdateCallback(robot_state::AttachedBody* attached_body, bool just_attached);
420 
422  void currentWorldObjectUpdateCallback(const collision_detection::World::ObjectConstPtr& object,
424 
427 
430  void excludeWorldObjectFromOctree(const collision_detection::World::ObjectConstPtr& obj);
431  void includeWorldObjectInOctree(const collision_detection::World::ObjectConstPtr& obj);
432 
435  void excludeAttachedBodyFromOctree(const robot_state::AttachedBody* attached_body);
436  void includeAttachedBodyInOctree(const robot_state::AttachedBody* attached_body);
437 
438  bool getShapeTransformCache(const std::string& target_frame, const ros::Time& target_time,
440 
442  std::string monitor_name_;
443 
444  planning_scene::PlanningScenePtr scene_;
445  planning_scene::PlanningSceneConstPtr scene_const_;
446  planning_scene::PlanningScenePtr parent_scene_;
447  boost::shared_mutex scene_update_mutex_;
450 
454  std::string robot_description_;
455 
465  std::map<std::string, double> default_robot_link_padd_;
467  std::map<std::string, double> default_robot_link_scale_;
468 
469  // variables for planning scene publishing
471  std::unique_ptr<boost::thread> publish_planning_scene_;
476 
477  // subscribe to various sources of data
480 
482 
483  std::unique_ptr<message_filters::Subscriber<moveit_msgs::CollisionObject> > collision_object_subscriber_;
484  std::unique_ptr<tf::MessageFilter<moveit_msgs::CollisionObject> > collision_object_filter_;
485 
486  // include a octomap monitor
487  std::unique_ptr<occupancy_map_monitor::OccupancyMapMonitor> octomap_monitor_;
488 
489  // include a current state monitor
490  CurrentStateMonitorPtr current_state_monitor_;
491 
492  typedef std::map<const robot_model::LinkModel*,
493  std::vector<std::pair<occupancy_map_monitor::ShapeHandle, std::size_t> > >
495  typedef std::map<const robot_state::AttachedBody*,
496  std::vector<std::pair<occupancy_map_monitor::ShapeHandle, std::size_t> > >
498  typedef std::map<std::string, std::vector<std::pair<occupancy_map_monitor::ShapeHandle, const Eigen::Affine3d*> > >
500 
504  mutable boost::recursive_mutex shape_handles_lock_;
505 
507  boost::recursive_mutex update_lock_;
508  std::vector<boost::function<void(SceneUpdateType)> > update_callbacks_;
509 
511 private:
512  void getUpdatedFrameTransforms(std::vector<geometry_msgs::TransformStamped>& transforms);
513 
514  // publish planning scene update diffs (runs in its own thread)
515  void scenePublishingThread();
516 
517  // called by current_state_monitor_ when robot state (as monitored on joint state topic) changes
518  void onStateUpdate(const sensor_msgs::JointStateConstPtr& joint_state);
519 
520  // called by state_update_timer_ when a state update it pending
522 
523  // Callback for a new planning scene msg
524  void newPlanningSceneCallback(const moveit_msgs::PlanningSceneConstPtr& scene);
525 
526  // Lock for state_update_pending_ and dt_state_update_
527  boost::mutex state_pending_mutex_;
528 
530  // This field is protected by state_pending_mutex_
531  volatile bool state_update_pending_;
532 
534  // This field is protected by state_pending_mutex_
536 
538  // Setting this to a non-zero value resolves issues when the sensor data is
539  // arriving so fast that it is preceding the transform state.
541 
543  // Check if last_state_update_ is true and if so call updateSceneWithCurrentState()
544  // Not safe to access from callback functions.
546 
548  // Only access this from callback functions (and constructor)
550 
551  robot_model_loader::RobotModelLoaderPtr rm_loader_;
552  robot_model::RobotModelConstPtr robot_model_;
553 
555 
558 };
559 
582 {
583 public:
584  LockedPlanningSceneRO(const PlanningSceneMonitorPtr& planning_scene_monitor)
585  : planning_scene_monitor_(planning_scene_monitor)
586  {
587  initialize(true);
588  }
589 
590  const PlanningSceneMonitorPtr& getPlanningSceneMonitor()
591  {
592  return planning_scene_monitor_;
593  }
594 
595  operator bool() const
596  {
597  return planning_scene_monitor_ && planning_scene_monitor_->getPlanningScene();
598  }
599 
600  operator const planning_scene::PlanningSceneConstPtr&() const
601  {
602  return static_cast<const PlanningSceneMonitor*>(planning_scene_monitor_.get())->getPlanningScene();
603  }
604 
605  const planning_scene::PlanningSceneConstPtr& operator->() const
606  {
607  return static_cast<const PlanningSceneMonitor*>(planning_scene_monitor_.get())->getPlanningScene();
608  }
609 
610 protected:
611  LockedPlanningSceneRO(const PlanningSceneMonitorPtr& planning_scene_monitor, bool read_only)
612  : planning_scene_monitor_(planning_scene_monitor)
613  {
614  initialize(read_only);
615  }
616 
617  void initialize(bool read_only)
618  {
619  if (planning_scene_monitor_)
620  lock_.reset(new SingleUnlock(planning_scene_monitor_.get(), read_only));
621  }
622 
624 
625  // we use this struct so that lock/unlock are called only once
626  // even if the LockedPlanningScene instance is copied around
628  {
630  : planning_scene_monitor_(planning_scene_monitor), read_only_(read_only)
631  {
632  if (read_only)
633  planning_scene_monitor_->lockSceneRead();
634  else
635  planning_scene_monitor_->lockSceneWrite();
636  }
638  {
639  if (read_only_)
640  planning_scene_monitor_->unlockSceneRead();
641  else
642  planning_scene_monitor_->unlockSceneWrite();
643  }
646  };
647 
648  PlanningSceneMonitorPtr planning_scene_monitor_;
649  SingleUnlockPtr lock_;
650 };
651 
674 {
675 public:
676  LockedPlanningSceneRW(const PlanningSceneMonitorPtr& planning_scene_monitor)
677  : LockedPlanningSceneRO(planning_scene_monitor, false)
678  {
679  }
680 
681  operator const planning_scene::PlanningScenePtr&()
682  {
683  return planning_scene_monitor_->getPlanningScene();
684  }
685 
686  const planning_scene::PlanningScenePtr& operator->()
687  {
688  return planning_scene_monitor_->getPlanningScene();
689  }
690 };
691 }
692 
693 #endif
std::map< const robot_state::AttachedBody *, std::vector< std::pair< occupancy_map_monitor::ShapeHandle, std::size_t > > > AttachedBodyShapeHandles
void startSceneMonitor(const std::string &scene_topic=DEFAULT_PLANNING_SCENE_TOPIC)
Start the scene monitor.
boost::recursive_mutex update_lock_
lock access to update_callbacks_
std::map< ShapeHandle, Eigen::Affine3d, std::less< ShapeHandle >, Eigen::aligned_allocator< std::pair< const ShapeHandle, Eigen::Affine3d > > > ShapeTransformCache
void clearUpdateCallbacks()
Clear the functions to be called when an update to the scene is received.
double getDefaultRobotPadding() const
Get the default robot padding.
const PlanningSceneMonitorPtr & getPlanningSceneMonitor()
void getUpdatedFrameTransforms(std::vector< geometry_msgs::TransformStamped > &transforms)
std::map< const robot_model::LinkModel *, std::vector< std::pair< occupancy_map_monitor::ShapeHandle, std::size_t > > > LinkShapeHandles
The geometry of the scene was updated. This includes receiving new octomaps, collision objects...
robot_model_loader::RobotModelLoaderPtr rm_loader_
void lockSceneWrite()
Lock the scene for writing (only one thread can lock for writing and no other thread can lock for rea...
static const std::string DEFAULT_PLANNING_SCENE_SERVICE
The name of the service used by default for requesting full planning scene state. ...
void onStateUpdate(const sensor_msgs::JointStateConstPtr &joint_state)
ros::Time last_robot_motion_time_
Last time the state was updated.
void addUpdateCallback(const boost::function< void(SceneUpdateType)> &fn)
Add a function to be called when an update to the scene is received.
const robot_model_loader::RobotModelLoaderPtr & getRobotModelLoader() const
Get the user kinematic model loader.
const robot_model::RobotModelConstPtr & getRobotModel() const
ros::Duration shape_transform_cache_lookup_wait_time_
the amount of time to wait when looking up transforms
void octomapUpdateCallback()
Callback for octomap updates.
void configureDefaultPadding()
Configure the default padding.
void triggerSceneUpdateEvent(SceneUpdateType update_type)
This function is called every time there is a change to the planning scene.
void stopPublishingPlanningScene()
Stop publishing the maintained planning scene.
void lockSceneRead()
Lock the scene for reading (multiple threads can lock for reading at the same time) ...
double getDefaultRobotScale() const
Get the default robot scaling.
double getPlanningScenePublishingFrequency() const
Get the maximum frequency at which planning scenes are published (Hz)
void collisionObjectCallback(const moveit_msgs::CollisionObjectConstPtr &obj)
Callback for a new collision object msg.
void unlockSceneWrite()
Lock the scene from writing (only one thread can lock for writing and no other thread can lock for re...
PlanningSceneMonitor(const std::string &robot_description, const boost::shared_ptr< tf::Transformer > &tf=boost::shared_ptr< tf::Transformer >(), const std::string &name="")
Constructor.
The state in the monitored scene was updated.
std::unique_ptr< message_filters::Subscriber< moveit_msgs::CollisionObject > > collision_object_subscriber_
void setStateUpdateFrequency(double hz)
Update the scene using the monitored state at a specified frequency, in Hz. This function has an effe...
void currentStateAttachedBodyUpdateCallback(robot_state::AttachedBody *attached_body, bool just_attached)
Callback for a change for an attached object of the current state of the planning scene...
bool updatesScene(const planning_scene::PlanningSceneConstPtr &scene) const
Return true if the scene scene can be updated directly or indirectly by this monitor. This function will return true if the pointer of the scene is the same as the one maintained, or if a parent of the scene is the one maintained.
const planning_scene::PlanningScenePtr & getPlanningScene()
Avoid this function! Returns an unsafe pointer to the current planning scene.
LockedPlanningSceneRO(const PlanningSceneMonitorPtr &planning_scene_monitor)
const planning_scene::PlanningSceneConstPtr & getPlanningScene() const
Avoid this function! Returns an unsafe pointer to the current planning scene.
void excludeAttachedBodyFromOctree(const robot_state::AttachedBody *attached_body)
const planning_scene::PlanningSceneConstPtr & operator->() const
std::unique_ptr< boost::thread > publish_planning_scene_
const CurrentStateMonitorPtr & getStateMonitor() const
Get the stored instance of the stored current state monitor.
void getMonitoredTopics(std::vector< std::string > &topics) const
Get the topic names that the monitor is listening to.
void updateFrameTransforms()
Update the transforms for the frames that are not part of the kinematic model using tf...
ros::WallTime last_robot_state_update_wall_time_
Last time the state was updated from current_state_monitor_.
std::unique_ptr< tf::MessageFilter< moveit_msgs::CollisionObject > > collision_object_filter_
double getStateUpdateFrequency() const
Get the maximum frequency (Hz) at which the current state of the planning scene is updated...
void startWorldGeometryMonitor(const std::string &collision_objects_topic=DEFAULT_COLLISION_OBJECT_TOPIC, const std::string &planning_scene_world_topic=DEFAULT_PLANNING_SCENE_WORLD_TOPIC, const bool load_octomap_monitor=true)
Start listening for objects in the world, the collision map and attached collision objects...
double getDefaultObjectPadding() const
Get the default object padding.
std::unique_ptr< occupancy_map_monitor::OccupancyMapMonitor > octomap_monitor_
PlanningSceneMonitor Subscribes to the topic planning_scene.
std::map< std::string, double > default_robot_link_scale_
default robot link scale
std::map< std::string, std::vector< std::pair< occupancy_map_monitor::ShapeHandle, const Eigen::Affine3d * > > > CollisionBodyShapeHandles
bool waitForCurrentRobotState(const ros::Time &t, double wait_time=1.)
Wait for robot state to become more recent than time t.
ros::NodeHandle nh_
Last time the robot has moved.
void initialize(const planning_scene::PlanningScenePtr &scene)
Initialize the planning scene monitor.
static const std::string DEFAULT_PLANNING_SCENE_TOPIC
The name of the topic used by default for receiving full planning scenes or planning scene diffs...
void updateSceneWithCurrentState()
Update the scene using the monitored state. This function is automatically called when an update to t...
const planning_scene::PlanningScenePtr & operator->()
ros::WallTimer state_update_timer_
timer for state updates.
double getDefaultAttachedObjectPadding() const
Get the default attached padding.
void startStateMonitor(const std::string &joint_states_topic=DEFAULT_JOINT_STATES_TOPIC, const std::string &attached_objects_topic=DEFAULT_ATTACHED_COLLISION_OBJECT_TOPIC)
Start the current state monitor.
void configureCollisionMatrix(const planning_scene::PlanningScenePtr &scene)
Configure the collision matrix for a particular scene.
boost::condition_variable_any new_scene_update_condition_
boost::shared_ptr< tf::Transformer > tf_
void startPublishingPlanningScene(SceneUpdateType event, const std::string &planning_scene_topic=MONITORED_PLANNING_SCENE_TOPIC)
Start publishing the maintained planning scene. The first message set out is a complete planning scen...
LockedPlanningSceneRO(const PlanningSceneMonitorPtr &planning_scene_monitor, bool read_only)
action
This is a convenience class for obtaining access to an instance of a locked PlanningScene.
void unlockSceneRead()
Unlock the scene from reading (multiple threads can lock for reading at the same time) ...
void stateUpdateTimerCallback(const ros::WallTimerEvent &event)
ros::Time last_update_time_
mutex for stored scene
static const std::string DEFAULT_COLLISION_OBJECT_TOPIC
The name of the topic used by default for receiving collision objects in the world.
MOVEIT_CLASS_FORWARD(CurrentStateMonitor)
std::vector< boost::function< void(SceneUpdateType)> > update_callbacks_
void collisionObjectFailTFCallback(const moveit_msgs::CollisionObjectConstPtr &obj, tf::filter_failure_reasons::FilterFailureReason reason)
Callback for a new collision object msg that failed to pass the TF filter.
boost::shared_mutex scene_update_mutex_
if diffs are monitored, this is the pointer to the parent scene
void monitorDiffs(bool flag)
By default, the maintained planning scene does not reason about diffs. When the flag passed in is tru...
planning_scene::PlanningScenePtr parent_scene_
bool getShapeTransformCache(const std::string &target_frame, const ros::Time &target_time, occupancy_map_monitor::ShapeTransformCache &cache) const
void stopWorldGeometryMonitor()
Stop the world geometry monitor.
This is used to load the collision plugin.
void excludeWorldObjectFromOctree(const collision_detection::World::ObjectConstPtr &obj)
bool requestPlanningSceneState(const std::string &service_name=DEFAULT_PLANNING_SCENE_SERVICE)
Request planning scene state using a service call.
void includeWorldObjectInOctree(const collision_detection::World::ObjectConstPtr &obj)
The maintained set of fixed transforms in the monitored scene was updated.
double default_attached_padd_
default attached padding
std::string monitor_name_
The name of this scene monitor.
bool newPlanningSceneMessage(const moveit_msgs::PlanningScene &scene)
const std::string & getRobotDescription() const
Get the stored robot description.
const std::string & getName() const
Get the name of this monitor.
void currentWorldObjectUpdateCallback(const collision_detection::World::ObjectConstPtr &object, collision_detection::World::Action action)
Callback for a change in the world maintained by the planning scene.
void setPlanningScenePublishingFrequency(double hz)
Set the maximum frequency at which planning scenes are being published.
static const std::string DEFAULT_ATTACHED_COLLISION_OBJECT_TOPIC
The name of the topic used by default for attached collision objects.
std::map< std::string, double > default_robot_link_padd_
default robot link padding
LockedPlanningSceneRW(const PlanningSceneMonitorPtr &planning_scene_monitor)
SingleUnlock(PlanningSceneMonitor *planning_scene_monitor, bool read_only)
volatile bool state_update_pending_
True when we need to update the RobotState from current_state_monitor_.
void includeAttachedBodyInOctree(const robot_state::AttachedBody *attached_body)
void newPlanningSceneCallback(const moveit_msgs::PlanningSceneConstPtr &scene)
const boost::shared_ptr< tf::Transformer > & getTFClient() const
Get the instance of the TF client that was passed to the constructor of this class.
This is a convenience class for obtaining access to an instance of a locked PlanningScene.
void attachObjectCallback(const moveit_msgs::AttachedCollisionObjectConstPtr &obj)
Callback for a new attached object msg.
planning_scene::PlanningSceneConstPtr scene_const_
void newPlanningSceneWorldCallback(const moveit_msgs::PlanningSceneWorldConstPtr &world)
Callback for a new planning scene world.
static const std::string DEFAULT_JOINT_STATES_TOPIC
The name of the topic used by default for receiving joint states.
ros::WallDuration dt_state_update_
the amount of time to wait in between updates to the robot state
collision_detection::CollisionPluginLoader collision_loader_
const ros::Time & getLastUpdateTime() const
Return the time when the last update was made to the planning scene (by any monitor) ...


planning
Author(s): Ioan Sucan , Sachin Chitta
autogenerated on Sat Apr 21 2018 02:55:20