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 <boost/noncopyable.hpp>
50 #include <boost/thread/shared_mutex.hpp>
51 #include <boost/thread/recursive_mutex.hpp>
52 #include <memory>
53 
54 namespace planning_scene_monitor
55 {
56 MOVEIT_CLASS_FORWARD(PlanningSceneMonitor);
57 
61 class PlanningSceneMonitor : private boost::noncopyable
62 {
63 public:
65  {
68 
71 
74 
78 
81  };
82 
84  static const std::string DEFAULT_JOINT_STATES_TOPIC; // "/joint_states"
85 
87  static const std::string DEFAULT_ATTACHED_COLLISION_OBJECT_TOPIC; // "/attached_collision_object"
88 
90  static const std::string DEFAULT_COLLISION_OBJECT_TOPIC; // "/collision_object"
91 
94  static const std::string DEFAULT_PLANNING_SCENE_WORLD_TOPIC; // "/planning_scene_world"
95 
97  static const std::string DEFAULT_PLANNING_SCENE_TOPIC; // "/planning_scene"
98 
100  static const std::string DEFAULT_PLANNING_SCENE_SERVICE; // "/get_planning_scene"
101 
104  static const std::string MONITORED_PLANNING_SCENE_TOPIC; // "monitored_planning_scene"
105 
111  PlanningSceneMonitor(const std::string& robot_description,
112  const std::shared_ptr<tf2_ros::Buffer>& tf_buffer = std::shared_ptr<tf2_ros::Buffer>(),
113  const std::string& name = "");
114 
120  PlanningSceneMonitor(const robot_model_loader::RobotModelLoaderPtr& rml,
121  const std::shared_ptr<tf2_ros::Buffer>& tf_buffer = std::shared_ptr<tf2_ros::Buffer>(),
122  const std::string& name = "");
123 
130  PlanningSceneMonitor(const planning_scene::PlanningScenePtr& scene, const std::string& robot_description,
131  const std::shared_ptr<tf2_ros::Buffer>& tf_buffer = std::shared_ptr<tf2_ros::Buffer>(),
132  const std::string& name = "");
133 
140  PlanningSceneMonitor(const planning_scene::PlanningScenePtr& scene,
141  const robot_model_loader::RobotModelLoaderPtr& rml,
142  const std::shared_ptr<tf2_ros::Buffer>& tf_buffer = std::shared_ptr<tf2_ros::Buffer>(),
143  const std::string& name = "");
144 
154  PlanningSceneMonitor(const planning_scene::PlanningScenePtr& scene,
155  const robot_model_loader::RobotModelLoaderPtr& rml, const ros::NodeHandle& nh,
156  const std::shared_ptr<tf2_ros::Buffer>& tf_buffer = std::shared_ptr<tf2_ros::Buffer>(),
157  const std::string& name = "");
158 
160 
162  const std::string& getName() const
163  {
164  return monitor_name_;
165  }
166 
168  const robot_model_loader::RobotModelLoaderPtr& getRobotModelLoader() const
169  {
170  return rm_loader_;
171  }
172 
173  const robot_model::RobotModelConstPtr& getRobotModel() const
174  {
175  return robot_model_;
176  }
177 
192  const planning_scene::PlanningScenePtr& getPlanningScene()
193  {
194  return scene_;
195  }
196 
200  const planning_scene::PlanningSceneConstPtr& getPlanningScene() const
201  {
202  return scene_const_;
203  }
204 
209  bool updatesScene(const planning_scene::PlanningSceneConstPtr& scene) const;
210 
215  bool updatesScene(const planning_scene::PlanningScenePtr& scene) const;
216 
219  const std::string& getRobotDescription() const
220  {
221  return robot_description_;
222  }
223 
225  double getDefaultRobotPadding() const
226  {
227  return default_robot_padd_;
228  }
229 
231  double getDefaultRobotScale() const
232  {
233  return default_robot_scale_;
234  }
235 
237  double getDefaultObjectPadding() const
238  {
239  return default_object_padd_;
240  }
241 
244  {
245  return default_attached_padd_;
246  }
247 
249  const std::shared_ptr<tf2_ros::Buffer>& getTFClient() const
250  {
251  return tf_buffer_;
252  }
253 
259  void monitorDiffs(bool flag);
260 
265  const std::string& planning_scene_topic = MONITORED_PLANNING_SCENE_TOPIC);
266 
269 
271  void setPlanningScenePublishingFrequency(double hz);
272 
275  {
277  }
278 
281  const CurrentStateMonitorPtr& getStateMonitor() const
282  {
283  return current_state_monitor_;
284  }
285 
286  CurrentStateMonitorPtr& getStateMonitorNonConst()
287  {
288  return current_state_monitor_;
289  }
290 
296  void updateFrameTransforms();
297 
301  void startStateMonitor(const std::string& joint_states_topic = DEFAULT_JOINT_STATES_TOPIC,
302  const std::string& attached_objects_topic = DEFAULT_ATTACHED_COLLISION_OBJECT_TOPIC);
303 
305  void stopStateMonitor();
306 
311 
317  void setStateUpdateFrequency(double hz);
318 
320  double getStateUpdateFrequency() const
321  {
322  if (!dt_state_update_.isZero())
323  return 1.0 / dt_state_update_.toSec();
324  else
325  return 0.0;
326  }
327 
331  void startSceneMonitor(const std::string& scene_topic = DEFAULT_PLANNING_SCENE_TOPIC);
332 
339  bool requestPlanningSceneState(const std::string& service_name = DEFAULT_PLANNING_SCENE_SERVICE);
340 
342  void stopSceneMonitor();
343 
352  void startWorldGeometryMonitor(const std::string& collision_objects_topic = DEFAULT_COLLISION_OBJECT_TOPIC,
353  const std::string& planning_scene_world_topic = DEFAULT_PLANNING_SCENE_WORLD_TOPIC,
354  const bool load_octomap_monitor = true);
355 
358 
360  void addUpdateCallback(const boost::function<void(SceneUpdateType)>& fn);
361 
363  void clearUpdateCallbacks();
364 
366  void getMonitoredTopics(std::vector<std::string>& topics) const;
367 
370  {
371  return last_update_time_;
372  }
373 
374  void publishDebugInformation(bool flag);
375 
377  void triggerSceneUpdateEvent(SceneUpdateType update_type);
378 
384  bool waitForCurrentRobotState(const ros::Time& t, double wait_time = 1.);
385 
387  void lockSceneRead();
388 
390  void unlockSceneRead();
391 
394  void lockSceneWrite();
395 
398  void unlockSceneWrite();
399 
400  void clearOctomap();
401 
402  // Called to update the planning scene with a new message.
403  bool newPlanningSceneMessage(const moveit_msgs::PlanningScene& scene);
404 
405 protected:
409  void initialize(const planning_scene::PlanningScenePtr& scene);
410 
412  void configureCollisionMatrix(const planning_scene::PlanningScenePtr& scene);
413 
416 
418  void collisionObjectCallback(const moveit_msgs::CollisionObjectConstPtr& obj);
419 
421  void newPlanningSceneWorldCallback(const moveit_msgs::PlanningSceneWorldConstPtr& world);
422 
424  void octomapUpdateCallback();
425 
427  void attachObjectCallback(const moveit_msgs::AttachedCollisionObjectConstPtr& obj);
428 
430  void currentStateAttachedBodyUpdateCallback(robot_state::AttachedBody* attached_body, bool just_attached);
431 
433  void currentWorldObjectUpdateCallback(const collision_detection::World::ObjectConstPtr& object,
435 
438 
441  void excludeWorldObjectFromOctree(const collision_detection::World::ObjectConstPtr& obj);
442  void includeWorldObjectInOctree(const collision_detection::World::ObjectConstPtr& obj);
443 
446  void excludeAttachedBodyFromOctree(const robot_state::AttachedBody* attached_body);
447  void includeAttachedBodyInOctree(const robot_state::AttachedBody* attached_body);
448 
449  bool getShapeTransformCache(const std::string& target_frame, const ros::Time& target_time,
451 
453  std::string monitor_name_;
454 
455  planning_scene::PlanningScenePtr scene_;
456  planning_scene::PlanningSceneConstPtr scene_const_;
457  planning_scene::PlanningScenePtr parent_scene_;
458  boost::shared_mutex scene_update_mutex_;
461 
465  std::shared_ptr<ros::AsyncSpinner> spinner_;
466 
467  std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
468 
469  std::string robot_description_;
470 
480  std::map<std::string, double> default_robot_link_padd_;
482  std::map<std::string, double> default_robot_link_scale_;
483 
484  // variables for planning scene publishing
486  std::unique_ptr<boost::thread> publish_planning_scene_;
491 
492  // subscribe to various sources of data
495 
498 
499  // include a octomap monitor
500  std::unique_ptr<occupancy_map_monitor::OccupancyMapMonitor> octomap_monitor_;
501 
502  // include a current state monitor
503  CurrentStateMonitorPtr current_state_monitor_;
504 
505  typedef std::map<const robot_model::LinkModel*,
506  std::vector<std::pair<occupancy_map_monitor::ShapeHandle, std::size_t> > >
508  typedef std::map<const robot_state::AttachedBody*,
509  std::vector<std::pair<occupancy_map_monitor::ShapeHandle, std::size_t> > >
511  typedef std::map<std::string, std::vector<std::pair<occupancy_map_monitor::ShapeHandle, const Eigen::Isometry3d*> > >
513 
517  mutable boost::recursive_mutex shape_handles_lock_;
518 
520  boost::recursive_mutex update_lock_;
521  std::vector<boost::function<void(SceneUpdateType)> > update_callbacks_;
522 
524 private:
525  void getUpdatedFrameTransforms(std::vector<geometry_msgs::TransformStamped>& transforms);
526 
527  // publish planning scene update diffs (runs in its own thread)
528  void scenePublishingThread();
529 
530  // called by current_state_monitor_ when robot state (as monitored on joint state topic) changes
531  void onStateUpdate(const sensor_msgs::JointStateConstPtr& joint_state);
532 
533  // called by state_update_timer_ when a state update it pending
535 
536  // Callback for a new planning scene msg
537  void newPlanningSceneCallback(const moveit_msgs::PlanningSceneConstPtr& scene);
538 
539  // Lock for state_update_pending_ and dt_state_update_
540  boost::mutex state_pending_mutex_;
541 
543  // This field is protected by state_pending_mutex_
544  volatile bool state_update_pending_;
545 
547  // This field is protected by state_pending_mutex_
549 
551  // Setting this to a non-zero value resolves issues when the sensor data is
552  // arriving so fast that it is preceding the transform state.
554 
556  // Check if last_state_update_ is true and if so call updateSceneWithCurrentState()
557  // Not safe to access from callback functions.
559 
561  // Only access this from callback functions (and constructor)
563 
564  robot_model_loader::RobotModelLoaderPtr rm_loader_;
565  robot_model::RobotModelConstPtr robot_model_;
566 
568 
571 };
572 
595 {
596 public:
597  LockedPlanningSceneRO(const PlanningSceneMonitorPtr& planning_scene_monitor)
598  : planning_scene_monitor_(planning_scene_monitor)
599  {
600  initialize(true);
601  }
602 
603  const PlanningSceneMonitorPtr& getPlanningSceneMonitor()
604  {
605  return planning_scene_monitor_;
606  }
607 
608  operator bool() const
609  {
610  return planning_scene_monitor_ && planning_scene_monitor_->getPlanningScene();
611  }
612 
613  operator const planning_scene::PlanningSceneConstPtr&() const
614  {
615  return static_cast<const PlanningSceneMonitor*>(planning_scene_monitor_.get())->getPlanningScene();
616  }
617 
618  const planning_scene::PlanningSceneConstPtr& operator->() const
619  {
620  return static_cast<const PlanningSceneMonitor*>(planning_scene_monitor_.get())->getPlanningScene();
621  }
622 
623 protected:
624  LockedPlanningSceneRO(const PlanningSceneMonitorPtr& planning_scene_monitor, bool read_only)
625  : planning_scene_monitor_(planning_scene_monitor)
626  {
627  initialize(read_only);
628  }
629 
630  void initialize(bool read_only)
631  {
632  if (planning_scene_monitor_)
633  lock_.reset(new SingleUnlock(planning_scene_monitor_.get(), read_only));
634  }
635 
637 
638  // we use this struct so that lock/unlock are called only once
639  // even if the LockedPlanningScene instance is copied around
641  {
643  : planning_scene_monitor_(planning_scene_monitor), read_only_(read_only)
644  {
645  if (read_only)
646  planning_scene_monitor_->lockSceneRead();
647  else
648  planning_scene_monitor_->lockSceneWrite();
649  }
651  {
652  if (read_only_)
653  planning_scene_monitor_->unlockSceneRead();
654  else
655  planning_scene_monitor_->unlockSceneWrite();
656  }
659  };
660 
661  PlanningSceneMonitorPtr planning_scene_monitor_;
662  SingleUnlockPtr lock_;
663 };
664 
687 {
688 public:
689  LockedPlanningSceneRW(const PlanningSceneMonitorPtr& planning_scene_monitor)
690  : LockedPlanningSceneRO(planning_scene_monitor, false)
691  {
692  }
693 
694  operator const planning_scene::PlanningScenePtr&()
695  {
696  return planning_scene_monitor_->getPlanningScene();
697  }
698 
699  const planning_scene::PlanningScenePtr& operator->()
700  {
701  return planning_scene_monitor_->getPlanningScene();
702  }
703 };
704 }
705 
706 #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_
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...
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
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 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)
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 Fri Oct 11 2019 03:56:35