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 <tf/tf.h>
43 #include <tf/message_filter.h>
51 #include <boost/noncopyable.hpp>
52 #include <boost/thread/shared_mutex.hpp>
53 #include <boost/thread/recursive_mutex.hpp>
54 #include <memory>
55 
56 namespace planning_scene_monitor
57 {
58 MOVEIT_CLASS_FORWARD(PlanningSceneMonitor);
59 
63 class PlanningSceneMonitor : private boost::noncopyable
64 {
65 public:
67  {
70 
73 
76 
80 
83  };
84 
86  static const std::string DEFAULT_JOINT_STATES_TOPIC; // "/joint_states"
87 
89  static const std::string DEFAULT_ATTACHED_COLLISION_OBJECT_TOPIC; // "/attached_collision_object"
90 
92  static const std::string DEFAULT_COLLISION_OBJECT_TOPIC; // "/collision_object"
93 
96  static const std::string DEFAULT_PLANNING_SCENE_WORLD_TOPIC; // "/planning_scene_world"
97 
99  static const std::string DEFAULT_PLANNING_SCENE_TOPIC; // "/planning_scene"
100 
102  static const std::string DEFAULT_PLANNING_SCENE_SERVICE; // "/get_planning_scene"
103 
106  static const std::string MONITORED_PLANNING_SCENE_TOPIC; // "monitored_planning_scene"
107 
113  PlanningSceneMonitor(const std::string& robot_description,
115  const std::string& name = "");
116 
122  PlanningSceneMonitor(const robot_model_loader::RobotModelLoaderPtr& rml,
124  const std::string& name = "");
125 
132  PlanningSceneMonitor(const planning_scene::PlanningScenePtr& scene, const std::string& robot_description,
134  const std::string& name = "");
135 
142  PlanningSceneMonitor(const planning_scene::PlanningScenePtr& scene,
143  const robot_model_loader::RobotModelLoaderPtr& rml,
145  const std::string& name = "");
146 
156  PlanningSceneMonitor(const planning_scene::PlanningScenePtr& scene,
157  const robot_model_loader::RobotModelLoaderPtr& rml, const ros::NodeHandle& nh,
159  const std::string& name = "");
160 
162 
164  const std::string& getName() const
165  {
166  return monitor_name_;
167  }
168 
170  const robot_model_loader::RobotModelLoaderPtr& getRobotModelLoader() const
171  {
172  return rm_loader_;
173  }
174 
175  const robot_model::RobotModelConstPtr& getRobotModel() const
176  {
177  return robot_model_;
178  }
179 
194  const planning_scene::PlanningScenePtr& getPlanningScene()
195  {
196  return scene_;
197  }
198 
202  const planning_scene::PlanningSceneConstPtr& getPlanningScene() const
203  {
204  return scene_const_;
205  }
206 
211  bool updatesScene(const planning_scene::PlanningSceneConstPtr& scene) const;
212 
217  bool updatesScene(const planning_scene::PlanningScenePtr& scene) const;
218 
221  const std::string& getRobotDescription() const
222  {
223  return robot_description_;
224  }
225 
227  double getDefaultRobotPadding() const
228  {
229  return default_robot_padd_;
230  }
231 
233  double getDefaultRobotScale() const
234  {
235  return default_robot_scale_;
236  }
237 
239  double getDefaultObjectPadding() const
240  {
241  return default_object_padd_;
242  }
243 
246  {
247  return default_attached_padd_;
248  }
249 
252  {
253  return tf_;
254  }
255 
261  void monitorDiffs(bool flag);
262 
267  const std::string& planning_scene_topic = MONITORED_PLANNING_SCENE_TOPIC);
268 
271 
273  void setPlanningScenePublishingFrequency(double hz);
274 
277  {
279  }
280 
283  const CurrentStateMonitorPtr& getStateMonitor() const
284  {
285  return current_state_monitor_;
286  }
287 
288  CurrentStateMonitorPtr& getStateMonitorNonConst()
289  {
290  return current_state_monitor_;
291  }
292 
298  void updateFrameTransforms();
299 
303  void startStateMonitor(const std::string& joint_states_topic = DEFAULT_JOINT_STATES_TOPIC,
304  const std::string& attached_objects_topic = DEFAULT_ATTACHED_COLLISION_OBJECT_TOPIC);
305 
307  void stopStateMonitor();
308 
313 
319  void setStateUpdateFrequency(double hz);
320 
322  double getStateUpdateFrequency() const
323  {
324  if (!dt_state_update_.isZero())
325  return 1.0 / dt_state_update_.toSec();
326  else
327  return 0.0;
328  }
329 
333  void startSceneMonitor(const std::string& scene_topic = DEFAULT_PLANNING_SCENE_TOPIC);
334 
341  bool requestPlanningSceneState(const std::string& service_name = DEFAULT_PLANNING_SCENE_SERVICE);
342 
344  void stopSceneMonitor();
345 
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 collisionObjectFailTFCallback(const moveit_msgs::CollisionObjectConstPtr& obj,
423 
425  void newPlanningSceneWorldCallback(const moveit_msgs::PlanningSceneWorldConstPtr& world);
426 
428  void octomapUpdateCallback();
429 
431  void attachObjectCallback(const moveit_msgs::AttachedCollisionObjectConstPtr& obj);
432 
434  void currentStateAttachedBodyUpdateCallback(robot_state::AttachedBody* attached_body, bool just_attached);
435 
437  void currentWorldObjectUpdateCallback(const collision_detection::World::ObjectConstPtr& object,
439 
442 
445  void excludeWorldObjectFromOctree(const collision_detection::World::ObjectConstPtr& obj);
446  void includeWorldObjectInOctree(const collision_detection::World::ObjectConstPtr& obj);
447 
450  void excludeAttachedBodyFromOctree(const robot_state::AttachedBody* attached_body);
451  void includeAttachedBodyInOctree(const robot_state::AttachedBody* attached_body);
452 
453  bool getShapeTransformCache(const std::string& target_frame, const ros::Time& target_time,
455 
457  std::string monitor_name_;
458 
459  planning_scene::PlanningScenePtr scene_;
460  planning_scene::PlanningSceneConstPtr scene_const_;
461  planning_scene::PlanningScenePtr parent_scene_;
462  boost::shared_mutex scene_update_mutex_;
465 
469  std::shared_ptr<ros::AsyncSpinner> spinner_;
471  std::string robot_description_;
472 
482  std::map<std::string, double> default_robot_link_padd_;
484  std::map<std::string, double> default_robot_link_scale_;
485 
486  // variables for planning scene publishing
488  std::unique_ptr<boost::thread> publish_planning_scene_;
493 
494  // subscribe to various sources of data
497 
499 
500  std::unique_ptr<message_filters::Subscriber<moveit_msgs::CollisionObject> > collision_object_subscriber_;
501  std::unique_ptr<tf::MessageFilter<moveit_msgs::CollisionObject> > collision_object_filter_;
502 
503  // include a octomap monitor
504  std::unique_ptr<occupancy_map_monitor::OccupancyMapMonitor> octomap_monitor_;
505 
506  // include a current state monitor
507  CurrentStateMonitorPtr current_state_monitor_;
508 
509  typedef std::map<const robot_model::LinkModel*,
510  std::vector<std::pair<occupancy_map_monitor::ShapeHandle, std::size_t> > >
512  typedef std::map<const robot_state::AttachedBody*,
513  std::vector<std::pair<occupancy_map_monitor::ShapeHandle, std::size_t> > >
515  typedef std::map<std::string, std::vector<std::pair<occupancy_map_monitor::ShapeHandle, const Eigen::Affine3d*> > >
517 
521  mutable boost::recursive_mutex shape_handles_lock_;
522 
524  boost::recursive_mutex update_lock_;
525  std::vector<boost::function<void(SceneUpdateType)> > update_callbacks_;
526 
528 private:
529  void getUpdatedFrameTransforms(std::vector<geometry_msgs::TransformStamped>& transforms);
530 
531  // publish planning scene update diffs (runs in its own thread)
532  void scenePublishingThread();
533 
534  // called by current_state_monitor_ when robot state (as monitored on joint state topic) changes
535  void onStateUpdate(const sensor_msgs::JointStateConstPtr& joint_state);
536 
537  // called by state_update_timer_ when a state update it pending
539 
540  // Callback for a new planning scene msg
541  void newPlanningSceneCallback(const moveit_msgs::PlanningSceneConstPtr& scene);
542 
543  // Lock for state_update_pending_ and dt_state_update_
544  boost::mutex state_pending_mutex_;
545 
547  // This field is protected by state_pending_mutex_
548  volatile bool state_update_pending_;
549 
551  // This field is protected by state_pending_mutex_
553 
555  // Setting this to a non-zero value resolves issues when the sensor data is
556  // arriving so fast that it is preceding the transform state.
558 
560  // Check if last_state_update_ is true and if so call updateSceneWithCurrentState()
561  // Not safe to access from callback functions.
563 
565  // Only access this from callback functions (and constructor)
567 
568  robot_model_loader::RobotModelLoaderPtr rm_loader_;
569  robot_model::RobotModelConstPtr robot_model_;
570 
572 
575 };
576 
599 {
600 public:
601  LockedPlanningSceneRO(const PlanningSceneMonitorPtr& planning_scene_monitor)
602  : planning_scene_monitor_(planning_scene_monitor)
603  {
604  initialize(true);
605  }
606 
607  const PlanningSceneMonitorPtr& getPlanningSceneMonitor()
608  {
609  return planning_scene_monitor_;
610  }
611 
612  operator bool() const
613  {
614  return planning_scene_monitor_ && planning_scene_monitor_->getPlanningScene();
615  }
616 
617  operator const planning_scene::PlanningSceneConstPtr&() const
618  {
619  return static_cast<const PlanningSceneMonitor*>(planning_scene_monitor_.get())->getPlanningScene();
620  }
621 
622  const planning_scene::PlanningSceneConstPtr& operator->() const
623  {
624  return static_cast<const PlanningSceneMonitor*>(planning_scene_monitor_.get())->getPlanningScene();
625  }
626 
627 protected:
628  LockedPlanningSceneRO(const PlanningSceneMonitorPtr& planning_scene_monitor, bool read_only)
629  : planning_scene_monitor_(planning_scene_monitor)
630  {
631  initialize(read_only);
632  }
633 
634  void initialize(bool read_only)
635  {
636  if (planning_scene_monitor_)
637  lock_.reset(new SingleUnlock(planning_scene_monitor_.get(), read_only));
638  }
639 
641 
642  // we use this struct so that lock/unlock are called only once
643  // even if the LockedPlanningScene instance is copied around
645  {
647  : planning_scene_monitor_(planning_scene_monitor), read_only_(read_only)
648  {
649  if (read_only)
650  planning_scene_monitor_->lockSceneRead();
651  else
652  planning_scene_monitor_->lockSceneWrite();
653  }
655  {
656  if (read_only_)
657  planning_scene_monitor_->unlockSceneRead();
658  else
659  planning_scene_monitor_->unlockSceneWrite();
660  }
663  };
664 
665  PlanningSceneMonitorPtr planning_scene_monitor_;
666  SingleUnlockPtr lock_;
667 };
668 
691 {
692 public:
693  LockedPlanningSceneRW(const PlanningSceneMonitorPtr& planning_scene_monitor)
694  : LockedPlanningSceneRO(planning_scene_monitor, false)
695  {
696  }
697 
698  operator const planning_scene::PlanningScenePtr&()
699  {
700  return planning_scene_monitor_->getPlanningScene();
701  }
702 
703  const planning_scene::PlanningScenePtr& operator->()
704  {
705  return planning_scene_monitor_->getPlanningScene();
706  }
707 };
708 }
709 
710 #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.
std::shared_ptr< ros::AsyncSpinner > spinner_
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 Wed Jul 10 2019 04:03:32