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 <ros/callback_queue.h>
42 #include <tf2_ros/buffer.h>
49 #include <moveit_msgs/GetPlanningScene.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,
113  const std::shared_ptr<tf2_ros::Buffer>& tf_buffer = std::shared_ptr<tf2_ros::Buffer>(),
114  const std::string& name = "");
115 
121  PlanningSceneMonitor(const robot_model_loader::RobotModelLoaderPtr& rml,
122  const std::shared_ptr<tf2_ros::Buffer>& tf_buffer = std::shared_ptr<tf2_ros::Buffer>(),
123  const std::string& name = "");
124 
131  PlanningSceneMonitor(const planning_scene::PlanningScenePtr& scene, const std::string& robot_description,
132  const std::shared_ptr<tf2_ros::Buffer>& tf_buffer = std::shared_ptr<tf2_ros::Buffer>(),
133  const std::string& name = "");
134 
141  PlanningSceneMonitor(const planning_scene::PlanningScenePtr& scene,
142  const robot_model_loader::RobotModelLoaderPtr& rml,
143  const std::shared_ptr<tf2_ros::Buffer>& tf_buffer = std::shared_ptr<tf2_ros::Buffer>(),
144  const std::string& name = "");
145 
155  PlanningSceneMonitor(const planning_scene::PlanningScenePtr& scene,
156  const robot_model_loader::RobotModelLoaderPtr& rml, const ros::NodeHandle& nh,
157  const std::shared_ptr<tf2_ros::Buffer>& tf_buffer = std::shared_ptr<tf2_ros::Buffer>(),
158  const std::string& name = "");
159 
161 
163  const std::string& getName() const
164  {
165  return monitor_name_;
166  }
167 
169  const robot_model_loader::RobotModelLoaderPtr& getRobotModelLoader() const
170  {
171  return rm_loader_;
172  }
173 
174  const robot_model::RobotModelConstPtr& getRobotModel() const
175  {
176  return robot_model_;
177  }
178 
193  const planning_scene::PlanningScenePtr& getPlanningScene()
194  {
195  return scene_;
196  }
197 
201  const planning_scene::PlanningSceneConstPtr& getPlanningScene() const
202  {
203  return scene_const_;
204  }
205 
210  bool updatesScene(const planning_scene::PlanningSceneConstPtr& scene) const;
211 
216  bool updatesScene(const planning_scene::PlanningScenePtr& scene) const;
217 
220  const std::string& getRobotDescription() const
221  {
222  return robot_description_;
223  }
224 
226  double getDefaultRobotPadding() const
227  {
228  return default_robot_padd_;
229  }
230 
232  double getDefaultRobotScale() const
233  {
234  return default_robot_scale_;
235  }
236 
238  double getDefaultObjectPadding() const
239  {
240  return default_object_padd_;
241  }
242 
245  {
246  return default_attached_padd_;
247  }
248 
250  const std::shared_ptr<tf2_ros::Buffer>& getTFClient() const
251  {
252  return tf_buffer_;
253  }
254 
260  void monitorDiffs(bool flag);
261 
266  const std::string& planning_scene_topic = MONITORED_PLANNING_SCENE_TOPIC);
267 
270 
272  void setPlanningScenePublishingFrequency(double hz);
273 
276  {
278  }
279 
282  const CurrentStateMonitorPtr& getStateMonitor() const
283  {
284  return current_state_monitor_;
285  }
286 
287  CurrentStateMonitorPtr& getStateMonitorNonConst()
288  {
289  return current_state_monitor_;
290  }
291 
297  void updateFrameTransforms();
298 
302  void startStateMonitor(const std::string& joint_states_topic = DEFAULT_JOINT_STATES_TOPIC,
303  const std::string& attached_objects_topic = DEFAULT_ATTACHED_COLLISION_OBJECT_TOPIC);
304 
306  void stopStateMonitor();
307 
312 
318  void setStateUpdateFrequency(double hz);
319 
321  double getStateUpdateFrequency() const
322  {
323  if (!dt_state_update_.isZero())
324  return 1.0 / dt_state_update_.toSec();
325  else
326  return 0.0;
327  }
328 
332  void startSceneMonitor(const std::string& scene_topic = DEFAULT_PLANNING_SCENE_TOPIC);
333 
341  bool requestPlanningSceneState(const std::string& service_name = DEFAULT_PLANNING_SCENE_SERVICE);
342 
350  void providePlanningSceneService(const std::string& service_name = DEFAULT_PLANNING_SCENE_SERVICE);
351 
353  void stopSceneMonitor();
354 
363  void startWorldGeometryMonitor(const std::string& collision_objects_topic = DEFAULT_COLLISION_OBJECT_TOPIC,
364  const std::string& planning_scene_world_topic = DEFAULT_PLANNING_SCENE_WORLD_TOPIC,
365  const bool load_octomap_monitor = true);
366 
369 
371  void addUpdateCallback(const boost::function<void(SceneUpdateType)>& fn);
372 
374  void clearUpdateCallbacks();
375 
377  void getMonitoredTopics(std::vector<std::string>& topics) const;
378 
381  {
382  return last_update_time_;
383  }
384 
385  void publishDebugInformation(bool flag);
386 
388  void triggerSceneUpdateEvent(SceneUpdateType update_type);
389 
395  bool waitForCurrentRobotState(const ros::Time& t, double wait_time = 1.);
396 
398  void lockSceneRead();
399 
401  void unlockSceneRead();
402 
405  void lockSceneWrite();
406 
409  void unlockSceneWrite();
410 
411  void clearOctomap();
412 
413  // Called to update the planning scene with a new message.
414  bool newPlanningSceneMessage(const moveit_msgs::PlanningScene& scene);
415 
416 protected:
420  void initialize(const planning_scene::PlanningScenePtr& scene);
421 
423  void configureCollisionMatrix(const planning_scene::PlanningScenePtr& scene);
424 
427 
429  void collisionObjectCallback(const moveit_msgs::CollisionObjectConstPtr& obj);
430 
432  void newPlanningSceneWorldCallback(const moveit_msgs::PlanningSceneWorldConstPtr& world);
433 
435  void octomapUpdateCallback();
436 
438  void attachObjectCallback(const moveit_msgs::AttachedCollisionObjectConstPtr& obj);
439 
441  void currentStateAttachedBodyUpdateCallback(robot_state::AttachedBody* attached_body, bool just_attached);
442 
444  void currentWorldObjectUpdateCallback(const collision_detection::World::ObjectConstPtr& object,
446 
449 
452  void excludeWorldObjectFromOctree(const collision_detection::World::ObjectConstPtr& obj);
453  void includeWorldObjectInOctree(const collision_detection::World::ObjectConstPtr& obj);
454 
457  void excludeAttachedBodyFromOctree(const robot_state::AttachedBody* attached_body);
458  void includeAttachedBodyInOctree(const robot_state::AttachedBody* attached_body);
459 
460  bool getShapeTransformCache(const std::string& target_frame, const ros::Time& target_time,
462 
464  std::string monitor_name_;
465 
466  planning_scene::PlanningScenePtr scene_;
467  planning_scene::PlanningSceneConstPtr scene_const_;
468  planning_scene::PlanningScenePtr parent_scene_;
469  boost::shared_mutex scene_update_mutex_;
472 
476  std::shared_ptr<ros::AsyncSpinner> spinner_;
477 
478  std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
479 
480  std::string robot_description_;
481 
491  std::map<std::string, double> default_robot_link_padd_;
493  std::map<std::string, double> default_robot_link_scale_;
494 
495  // variables for planning scene publishing
497  std::unique_ptr<boost::thread> publish_planning_scene_;
501  boost::condition_variable_any new_scene_update_condition_;
502 
503  // subscribe to various sources of data
506 
509 
510  // provide an optional service to get the full planning scene state
511  // this is used by MoveGroup and related application nodes
513 
514  // include a octomap monitor
515  std::unique_ptr<occupancy_map_monitor::OccupancyMapMonitor> octomap_monitor_;
516 
517  // include a current state monitor
518  CurrentStateMonitorPtr current_state_monitor_;
519 
520  typedef std::map<const robot_model::LinkModel*,
521  std::vector<std::pair<occupancy_map_monitor::ShapeHandle, std::size_t> > >
523  typedef std::map<const robot_state::AttachedBody*,
524  std::vector<std::pair<occupancy_map_monitor::ShapeHandle, std::size_t> > >
526  typedef std::map<std::string, std::vector<std::pair<occupancy_map_monitor::ShapeHandle, const Eigen::Isometry3d*> > >
528 
532  mutable boost::recursive_mutex shape_handles_lock_;
533 
535  boost::recursive_mutex update_lock_;
536  std::vector<boost::function<void(SceneUpdateType)> > update_callbacks_;
537 
539 private:
540  void getUpdatedFrameTransforms(std::vector<geometry_msgs::TransformStamped>& transforms);
541 
542  // publish planning scene update diffs (runs in its own thread)
543  void scenePublishingThread();
544 
545  // called by current_state_monitor_ when robot state (as monitored on joint state topic) changes
546  void onStateUpdate(const sensor_msgs::JointStateConstPtr& joint_state);
547 
548  // called by state_update_timer_ when a state update it pending
550 
551  // Callback for a new planning scene msg
552  void newPlanningSceneCallback(const moveit_msgs::PlanningSceneConstPtr& scene);
553 
554  // Callback for requesting the full planning scene via service
555  bool getPlanningSceneServiceCallback(moveit_msgs::GetPlanningScene::Request& req,
556  moveit_msgs::GetPlanningScene::Response& res);
557 
558  // Lock for state_update_pending_ and dt_state_update_
559  boost::mutex state_pending_mutex_;
560 
562  // This field is protected by state_pending_mutex_
563  volatile bool state_update_pending_;
564 
566  // This field is protected by state_pending_mutex_
568 
570  // Setting this to a non-zero value resolves issues when the sensor data is
571  // arriving so fast that it is preceding the transform state.
573 
575  // Check if last_state_update_ is true and if so call updateSceneWithCurrentState()
576  // Not safe to access from callback functions.
578 
580  // Only access this from callback functions (and constructor)
582 
583  robot_model_loader::RobotModelLoaderPtr rm_loader_;
584  robot_model::RobotModelConstPtr robot_model_;
585 
587 
590 };
591 
614 {
615 public:
616  LockedPlanningSceneRO(const PlanningSceneMonitorPtr& planning_scene_monitor)
617  : planning_scene_monitor_(planning_scene_monitor)
618  {
619  initialize(true);
620  }
621 
622  const PlanningSceneMonitorPtr& getPlanningSceneMonitor()
623  {
624  return planning_scene_monitor_;
625  }
626 
627  operator bool() const
628  {
629  return planning_scene_monitor_ && planning_scene_monitor_->getPlanningScene();
630  }
631 
632  operator const planning_scene::PlanningSceneConstPtr&() const
633  {
634  return static_cast<const PlanningSceneMonitor*>(planning_scene_monitor_.get())->getPlanningScene();
635  }
636 
637  const planning_scene::PlanningSceneConstPtr& operator->() const
638  {
639  return static_cast<const PlanningSceneMonitor*>(planning_scene_monitor_.get())->getPlanningScene();
640  }
641 
642 protected:
643  LockedPlanningSceneRO(const PlanningSceneMonitorPtr& planning_scene_monitor, bool read_only)
644  : planning_scene_monitor_(planning_scene_monitor)
645  {
646  initialize(read_only);
647  }
648 
649  void initialize(bool read_only)
650  {
651  if (planning_scene_monitor_)
652  lock_.reset(new SingleUnlock(planning_scene_monitor_.get(), read_only));
653  }
654 
656 
657  // we use this struct so that lock/unlock are called only once
658  // even if the LockedPlanningScene instance is copied around
660  {
662  : planning_scene_monitor_(planning_scene_monitor), read_only_(read_only)
663  {
664  if (read_only)
665  planning_scene_monitor_->lockSceneRead();
666  else
667  planning_scene_monitor_->lockSceneWrite();
668  }
670  {
671  if (read_only_)
672  planning_scene_monitor_->unlockSceneRead();
673  else
674  planning_scene_monitor_->unlockSceneWrite();
675  }
678  };
679 
680  PlanningSceneMonitorPtr planning_scene_monitor_;
681  SingleUnlockPtr lock_;
682 };
683 
706 {
707 public:
708  LockedPlanningSceneRW(const PlanningSceneMonitorPtr& planning_scene_monitor)
709  : LockedPlanningSceneRO(planning_scene_monitor, false)
710  {
711  }
712 
713  operator const planning_scene::PlanningScenePtr&()
714  {
715  return planning_scene_monitor_->getPlanningScene();
716  }
717 
718  const planning_scene::PlanningScenePtr& operator->()
719  {
720  return planning_scene_monitor_->getPlanningScene();
721  }
722 };
723 }
724 
725 #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 (ROS topic-based)
boost::recursive_mutex update_lock_
lock access to update_callbacks_
double getDefaultRobotPadding() const
Get the default robot padding.
void clearUpdateCallbacks()
Clear the functions to be called when an update to the scene is received.
const PlanningSceneMonitorPtr & getPlanningSceneMonitor()
void getUpdatedFrameTransforms(std::vector< geometry_msgs::TransformStamped > &transforms)
const planning_scene::PlanningSceneConstPtr & getPlanningScene() const
Avoid this function! Returns an unsafe pointer to the current planning scene.
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_
std::shared_ptr< tf2_ros::Buffer > tf_buffer_
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)
double getDefaultRobotScale() const
Get the default robot scaling.
const std::string & getName() const
Get the name of this monitor.
const CurrentStateMonitorPtr & getStateMonitor() const
Get the stored instance of the stored current state monitor.
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.
PlanningSceneMonitor(const std::string &robot_description, const std::shared_ptr< tf2_ros::Buffer > &tf_buffer=std::shared_ptr< tf2_ros::Buffer >(), const std::string &name="")
Constructor.
ros::Duration shape_transform_cache_lookup_wait_time_
the amount of time to wait when looking up transforms
void octomapUpdateCallback()
Callback for octomap updates.
const robot_model::RobotModelConstPtr & getRobotModel() const
void configureDefaultPadding()
Configure the default padding.
std::shared_ptr< ros::AsyncSpinner > spinner_
const std::shared_ptr< tf2_ros::Buffer > & getTFClient() const
Get the instance of the TF client that was passed to the constructor of this class.
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 getDefaultAttachedObjectPadding() const
Get the default attached padding.
geometry_msgs::TransformStamped t
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.
std::map< ShapeHandle, Eigen::Isometry3d, std::less< ShapeHandle >, Eigen::aligned_allocator< std::pair< const ShapeHandle, Eigen::Isometry3d > > > ShapeTransformCache
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...
The state in the monitored scene was updated.
void setStateUpdateFrequency(double hz)
Update the scene using the monitored state at a specified frequency, in Hz. This function has an effe...
bool getPlanningSceneServiceCallback(moveit_msgs::GetPlanningScene::Request &req, moveit_msgs::GetPlanningScene::Response &res)
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...
const planning_scene::PlanningScenePtr & getPlanningScene()
Avoid this function! Returns an unsafe pointer to the current planning scene.
LockedPlanningSceneRO(const PlanningSceneMonitorPtr &planning_scene_monitor)
void excludeAttachedBodyFromOctree(const robot_state::AttachedBody *attached_body)
std::map< std::string, std::vector< std::pair< occupancy_map_monitor::ShapeHandle, const Eigen::Isometry3d * > > > CollisionBodyShapeHandles
std::unique_ptr< boost::thread > publish_planning_scene_
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_.
MOVEIT_STRUCT_FORWARD(CollisionGeometryData)
const planning_scene::PlanningSceneConstPtr & operator->() const
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 the OccupancyMapMonitor and listening for:
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
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->()
void getMonitoredTopics(std::vector< std::string > &topics) const
Get the topic names that the monitor is listening to.
ros::WallTimer state_update_timer_
timer for state updates.
double getStateUpdateFrequency() const
Get the maximum frequency (Hz) at which the current state of the planning scene is updated...
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_
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
void providePlanningSceneService(const std::string &service_name=DEFAULT_PLANNING_SCENE_SERVICE)
Create an optional service for getting the complete planning scene This is useful for satisfying the ...
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) ...
double getPlanningScenePublishingFrequency() const
Get the maximum frequency at which planning scenes are published (Hz)
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_
const robot_model_loader::RobotModelLoaderPtr & getRobotModelLoader() const
Get the user kinematic model loader.
double getDefaultObjectPadding() const
Get the default object padding.
const std::string & getRobotDescription() const
Get the stored robot description.
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_
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 a full planning scene state using a service call Be careful not to use this in conjunction wi...
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)
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)
const ros::Time & getLastUpdateTime() const
Return the time when the last update was made to the planning scene (by any monitor) ...
void newPlanningSceneCallback(const moveit_msgs::PlanningSceneConstPtr &scene)
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_
bool getShapeTransformCache(const std::string &target_frame, const ros::Time &target_time, occupancy_map_monitor::ShapeTransformCache &cache) const


planning
Author(s): Ioan Sucan , Sachin Chitta
autogenerated on Thu Apr 2 2020 03:50:53