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 #pragma once
38 
39 #include <ros/ros.h>
40 #include <ros/callback_queue.h>
41 #include <tf2_ros/buffer.h>
48 #include <moveit_msgs/GetPlanningScene.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); // Defines PlanningSceneMonitorPtr, ConstPtr, WeakPtr... etc
57 
61 class PlanningSceneMonitor : private boost::noncopyable
62 {
63 public:
64  enum SceneUpdateType
65  {
67  UPDATE_NONE = 0,
68 
70  UPDATE_STATE = 1,
71 
74 
77  UPDATE_GEOMETRY = 4,
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 moveit::core::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 
243  double getDefaultAttachedObjectPadding() const
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 
340  bool requestPlanningSceneState(const std::string& service_name = DEFAULT_PLANNING_SCENE_SERVICE);
341 
349  void providePlanningSceneService(const std::string& service_name = DEFAULT_PLANNING_SCENE_SERVICE);
350 
353 
362  void startWorldGeometryMonitor(const std::string& collision_objects_topic = DEFAULT_COLLISION_OBJECT_TOPIC,
363  const std::string& planning_scene_world_topic = DEFAULT_PLANNING_SCENE_WORLD_TOPIC,
364  const bool load_octomap_monitor = true);
365 
368 
370  void addUpdateCallback(const boost::function<void(SceneUpdateType)>& fn);
371 
373  void clearUpdateCallbacks();
374 
376  void getMonitoredTopics(std::vector<std::string>& topics) const;
377 
379  const ros::Time& getLastUpdateTime() const
380  {
381  return last_update_time_;
382  }
383 
384  void publishDebugInformation(bool flag);
385 
387  void triggerSceneUpdateEvent(SceneUpdateType update_type);
388 
394  bool waitForCurrentRobotState(const ros::Time& t, double wait_time = 1.);
395 
397  void lockSceneRead();
398 
400  void unlockSceneRead();
401 
404  void lockSceneWrite();
405 
408  void unlockSceneWrite();
409 
410  void clearOctomap();
411 
412  // Called to update the planning scene with a new message.
413  bool newPlanningSceneMessage(const moveit_msgs::PlanningScene& scene);
414 
415 protected:
419  void initialize(const planning_scene::PlanningScenePtr& scene);
420 
422  void configureCollisionMatrix(const planning_scene::PlanningScenePtr& scene);
423 
426 
428  void collisionObjectCallback(const moveit_msgs::CollisionObjectConstPtr& obj);
429 
431  void newPlanningSceneWorldCallback(const moveit_msgs::PlanningSceneWorldConstPtr& world);
432 
434  void octomapUpdateCallback();
435 
437  void attachObjectCallback(const moveit_msgs::AttachedCollisionObjectConstPtr& obj);
438 
440  void currentStateAttachedBodyUpdateCallback(moveit::core::AttachedBody* attached_body, bool just_attached);
441 
443  void currentWorldObjectUpdateCallback(const collision_detection::World::ObjectConstPtr& object,
445 
448 
451  void excludeWorldObjectFromOctree(const collision_detection::World::ObjectConstPtr& obj);
452  void includeWorldObjectInOctree(const collision_detection::World::ObjectConstPtr& obj);
453 
457  void includeAttachedBodyInOctree(const moveit::core::AttachedBody* attached_body);
458 
459  bool getShapeTransformCache(const std::string& target_frame, const ros::Time& target_time,
461 
463  std::string monitor_name_;
464 
465  planning_scene::PlanningScenePtr scene_;
466  planning_scene::PlanningSceneConstPtr scene_const_;
467  planning_scene::PlanningScenePtr parent_scene_;
468  boost::shared_mutex scene_update_mutex_;
471 
475  std::shared_ptr<ros::AsyncSpinner> spinner_;
476 
477  std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
478 
479  std::string robot_description_;
480 
482  double default_robot_padd_;
484  double default_robot_scale_;
486  double default_object_padd_;
488  double default_attached_padd_;
490  std::map<std::string, double> default_robot_link_padd_;
492  std::map<std::string, double> default_robot_link_scale_;
493 
494  // variables for planning scene publishing
496  std::unique_ptr<boost::thread> publish_planning_scene_;
500  boost::condition_variable_any new_scene_update_condition_;
501 
502  // subscribe to various sources of data
505 
508 
509  // provide an optional service to get the full planning scene state
510  // this is used by MoveGroup and related application nodes
512 
513  // include a octomap monitor
514  std::unique_ptr<occupancy_map_monitor::OccupancyMapMonitor> octomap_monitor_;
515 
516  // include a current state monitor
517  CurrentStateMonitorPtr current_state_monitor_;
518 
519  typedef std::map<const moveit::core::LinkModel*,
520  std::vector<std::pair<occupancy_map_monitor::ShapeHandle, std::size_t> > >
523  std::vector<std::pair<occupancy_map_monitor::ShapeHandle, std::size_t> > >;
525  std::map<std::string, std::vector<std::pair<occupancy_map_monitor::ShapeHandle, const Eigen::Isometry3d*> > >;
526 
530  mutable boost::recursive_mutex shape_handles_lock_;
531 
533  boost::recursive_mutex update_lock_;
534  std::vector<boost::function<void(SceneUpdateType)> > update_callbacks_;
535 
537 private:
538  void getUpdatedFrameTransforms(std::vector<geometry_msgs::TransformStamped>& transforms);
539 
540  // publish planning scene update diffs (runs in its own thread)
541  void scenePublishingThread();
542 
543  // called by current_state_monitor_ when robot state (as monitored on joint state topic) changes
544  void onStateUpdate(const sensor_msgs::JointStateConstPtr& joint_state);
545 
546  // called by state_update_timer_ when a state update it pending
548 
549  // Callback for a new planning scene msg
550  void newPlanningSceneCallback(const moveit_msgs::PlanningSceneConstPtr& scene);
551 
552  // Callback for requesting the full planning scene via service
553  bool getPlanningSceneServiceCallback(moveit_msgs::GetPlanningScene::Request& req,
554  moveit_msgs::GetPlanningScene::Response& res);
555 
556  // Lock for state_update_pending_ and dt_state_update_
557  boost::mutex state_pending_mutex_;
558 
560  // This field is protected by state_pending_mutex_
561  volatile bool state_update_pending_;
562 
564  // This field is protected by state_pending_mutex_
566 
568  // Setting this to a non-zero value resolves issues when the sensor data is
569  // arriving so fast that it is preceding the transform state.
571 
573  // Check if last_state_update_ is true and if so call updateSceneWithCurrentState()
574  // Not safe to access from callback functions.
576 
578  // Only access this from callback functions (and constructor)
580 
581  robot_model_loader::RobotModelLoaderPtr rm_loader_;
582  moveit::core::RobotModelConstPtr robot_model_;
583 
585 
588 };
589 
612 {
613 public:
614  LockedPlanningSceneRO(const PlanningSceneMonitorPtr& planning_scene_monitor)
616  {
617  initialize(true);
618  }
619 
620  const PlanningSceneMonitorPtr& getPlanningSceneMonitor()
621  {
623  }
624 
625  operator bool() const
626  {
627  return planning_scene_monitor_ && planning_scene_monitor_->getPlanningScene();
628  }
629 
630  operator const planning_scene::PlanningSceneConstPtr &() const
631  {
632  return static_cast<const PlanningSceneMonitor*>(planning_scene_monitor_.get())->getPlanningScene();
633  }
634 
635  const planning_scene::PlanningSceneConstPtr& operator->() const
636  {
637  return static_cast<const PlanningSceneMonitor*>(planning_scene_monitor_.get())->getPlanningScene();
638  }
639 
640 protected:
641  LockedPlanningSceneRO(const PlanningSceneMonitorPtr& planning_scene_monitor, bool read_only)
643  {
644  initialize(read_only);
645  }
646 
647  void initialize(bool read_only)
648  {
650  lock_ = std::make_shared<SingleUnlock>(planning_scene_monitor_.get(), read_only);
651  }
652 
654 
655  // we use this struct so that lock/unlock are called only once
656  // even if the LockedPlanningScene instance is copied around
658  {
661  {
662  if (read_only)
664  else
666  }
668  {
669  if (read_only_)
671  else
673  }
675  bool read_only_;
676  };
677 
678  PlanningSceneMonitorPtr planning_scene_monitor_;
679  SingleUnlockPtr lock_;
680 };
681 
704 {
705 public:
706  LockedPlanningSceneRW(const PlanningSceneMonitorPtr& planning_scene_monitor)
708  {
709  }
710 
711  operator const planning_scene::PlanningScenePtr &()
712  {
713  return planning_scene_monitor_->getPlanningScene();
714  }
715 
716  const planning_scene::PlanningScenePtr& operator->()
717  {
718  return planning_scene_monitor_->getPlanningScene();
719  }
720 };
721 } // namespace planning_scene_monitor
planning_scene_monitor::PlanningSceneMonitor::excludeWorldObjectFromOctree
void excludeWorldObjectFromOctree(const collision_detection::World::ObjectConstPtr &obj)
Definition: planning_scene_monitor.cpp:863
planning_scene_monitor::PlanningSceneMonitor::planning_scene_world_subscriber_
ros::Subscriber planning_scene_world_subscriber_
Definition: planning_scene_monitor.h:536
planning_scene_monitor::LockedPlanningSceneRO::SingleUnlock
Definition: planning_scene_monitor.h:689
moveit::core::LinkModel
planning_scene_monitor::PlanningSceneMonitor::collision_body_shape_handles_
CollisionBodyShapeHandles collision_body_shape_handles_
Definition: planning_scene_monitor.h:561
planning_scene_monitor::PlanningSceneMonitor::publishDebugInformation
void publishDebugInformation(bool flag)
Definition: planning_scene_monitor.cpp:1399
planning_scene_monitor
Definition: current_state_monitor.h:47
planning_scene_monitor::PlanningSceneMonitor::onStateUpdate
void onStateUpdate(const sensor_msgs::JointStateConstPtr &joint_state)
Definition: planning_scene_monitor.cpp:1201
planning_scene_monitor::PlanningSceneMonitor::currentWorldObjectUpdateCallback
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.
Definition: planning_scene_monitor.cpp:915
planning_scene_monitor::PlanningSceneMonitor::getDefaultRobotScale
double getDefaultRobotScale() const
Get the default robot scaling.
Definition: planning_scene_monitor.h:263
planning_scene_monitor::PlanningSceneMonitor::default_attached_padd_
double default_attached_padd_
default attached padding
Definition: planning_scene_monitor.h:520
planning_scene_monitor::PlanningSceneMonitor::default_robot_link_padd_
std::map< std::string, double > default_robot_link_padd_
default robot link padding
Definition: planning_scene_monitor.h:522
planning_scene_monitor::PlanningSceneMonitor::configureDefaultPadding
void configureDefaultPadding()
Configure the default padding.
Definition: planning_scene_monitor.cpp:1448
planning_scene_monitor::PlanningSceneMonitor::attached_body_shape_handles_
AttachedBodyShapeHandles attached_body_shape_handles_
Definition: planning_scene_monitor.h:560
ros::Publisher
planning_scene_monitor::PlanningSceneMonitor::DEFAULT_PLANNING_SCENE_SERVICE
static const std::string DEFAULT_PLANNING_SCENE_SERVICE
The name of the service used by default for requesting full planning scene state.
Definition: planning_scene_monitor.h:132
occupancy_map_monitor::ShapeTransformCache
std::map< ShapeHandle, Eigen::Isometry3d, std::less< ShapeHandle >, Eigen::aligned_allocator< std::pair< const ShapeHandle, Eigen::Isometry3d > > > ShapeTransformCache
planning_scene_monitor::PlanningSceneMonitor::triggerSceneUpdateEvent
void triggerSceneUpdateEvent(SceneUpdateType update_type)
This function is called every time there is a change to the planning scene.
Definition: planning_scene_monitor.cpp:493
planning_scene_monitor::PlanningSceneMonitor::~PlanningSceneMonitor
~PlanningSceneMonitor()
Definition: planning_scene_monitor.cpp:192
planning_scene_monitor::PlanningSceneMonitor::getShapeTransformCache
bool getShapeTransformCache(const std::string &target_frame, const ros::Time &target_time, occupancy_map_monitor::ShapeTransformCache &cache) const
Definition: planning_scene_monitor.cpp:1042
planning_scene_monitor::PlanningSceneMonitor::stopStateMonitor
void stopStateMonitor()
Stop the state monitor.
Definition: planning_scene_monitor.cpp:1186
planning_scene_monitor::PlanningSceneMonitor::updatesScene
bool updatesScene(const planning_scene::PlanningSceneConstPtr &scene) const
Return true if the scene scene can be updated directly or indirectly by this monitor....
Definition: planning_scene_monitor.cpp:488
planning_scene_monitor::PlanningSceneMonitor::newPlanningSceneMessage
bool newPlanningSceneMessage(const moveit_msgs::PlanningScene &scene)
Definition: planning_scene_monitor.cpp:587
planning_scene_monitor::PlanningSceneMonitor::getStateMonitor
const CurrentStateMonitorPtr & getStateMonitor() const
Get the stored instance of the stored current state monitor.
Definition: planning_scene_monitor.h:313
planning_scene_monitor::PlanningSceneMonitor::octomapUpdateCallback
void octomapUpdateCallback()
Callback for octomap updates.
Definition: planning_scene_monitor.cpp:1257
planning_scene_monitor::PlanningSceneMonitor::addUpdateCallback
void addUpdateCallback(const boost::function< void(SceneUpdateType)> &fn)
Add a function to be called when an update to the scene is received.
Definition: planning_scene_monitor.cpp:1332
planning_scene_monitor::PlanningSceneMonitor::spinner_
std::shared_ptr< ros::AsyncSpinner > spinner_
Definition: planning_scene_monitor.h:507
planning_scene_monitor::LockedPlanningSceneRO::lock_
SingleUnlockPtr lock_
Definition: planning_scene_monitor.h:711
planning_scene_monitor::PlanningSceneMonitor::getStateMonitorNonConst
CurrentStateMonitorPtr & getStateMonitorNonConst()
Definition: planning_scene_monitor.h:318
ros.h
planning_scene_monitor::PlanningSceneMonitor::collision_loader_
collision_detection::CollisionPluginLoader collision_loader_
Definition: planning_scene_monitor.h:616
planning_scene_monitor::PlanningSceneMonitor::scenePublishingThread
void scenePublishingThread()
Definition: planning_scene_monitor.cpp:366
planning_scene_monitor::PlanningSceneMonitor::default_robot_padd_
double default_robot_padd_
default robot padding
Definition: planning_scene_monitor.h:514
planning_scene_monitor::PlanningSceneMonitor::attached_collision_object_subscriber_
ros::Subscriber attached_collision_object_subscriber_
Definition: planning_scene_monitor.h:538
planning_scene_monitor::PlanningSceneMonitor::setStateUpdateFrequency
void setStateUpdateFrequency(double hz)
Update the scene using the monitored state at a specified frequency, in Hz. This function has an effe...
Definition: planning_scene_monitor.cpp:1281
planning_scene_monitor::PlanningSceneMonitor::collisionObjectCallback
void collisionObjectCallback(const moveit_msgs::CollisionObjectConstPtr &obj)
Callback for a new collision object msg.
Definition: planning_scene_monitor.cpp:691
planning_scene_monitor::PlanningSceneMonitor::scene_const_
planning_scene::PlanningSceneConstPtr scene_const_
Definition: planning_scene_monitor.h:498
planning_scene_monitor::LockedPlanningSceneRO::getPlanningSceneMonitor
const PlanningSceneMonitorPtr & getPlanningSceneMonitor()
Definition: planning_scene_monitor.h:652
ros::WallTimer
planning_scene_monitor::PlanningSceneMonitor::update_lock_
boost::recursive_mutex update_lock_
lock access to update_callbacks_
Definition: planning_scene_monitor.h:565
planning_scene_monitor::PlanningSceneMonitor::collision_object_subscriber_
ros::Subscriber collision_object_subscriber_
Definition: planning_scene_monitor.h:539
planning_scene_monitor::PlanningSceneMonitor::getPlanningScene
const planning_scene::PlanningScenePtr & getPlanningScene()
Avoid this function! Returns an unsafe pointer to the current planning scene.
Definition: planning_scene_monitor.h:224
current_state_monitor.h
buffer.h
collision_detection::World::Action
planning_scene_monitor::LockedPlanningSceneRW::LockedPlanningSceneRW
LockedPlanningSceneRW(const PlanningSceneMonitorPtr &planning_scene_monitor)
Definition: planning_scene_monitor.h:738
planning_scene_monitor::PlanningSceneMonitor::updateFrameTransforms
void updateFrameTransforms()
Update the transforms for the frames that are not part of the kinematic model using tf....
Definition: planning_scene_monitor.cpp:1381
planning_scene_monitor::PlanningSceneMonitor::LinkShapeHandles
std::map< const moveit::core::LinkModel *, std::vector< std::pair< occupancy_map_monitor::ShapeHandle, std::size_t > > > LinkShapeHandles
Definition: planning_scene_monitor.h:553
planning_scene_monitor::PlanningSceneMonitor::getName
const std::string & getName() const
Get the name of this monitor.
Definition: planning_scene_monitor.h:194
planning_scene_monitor::PlanningSceneMonitor::excludeWorldObjectsFromOctree
void excludeWorldObjectsFromOctree()
Definition: planning_scene_monitor.cpp:815
planning_scene_monitor::PlanningSceneMonitor::last_robot_motion_time_
ros::Time last_robot_motion_time_
Last time the state was updated.
Definition: planning_scene_monitor.h:502
planning_scene_monitor::PlanningSceneMonitor::DynamicReconfigureImpl
Definition: planning_scene_monitor.cpp:91
planning_scene_monitor::PlanningSceneMonitor::rm_loader_
robot_model_loader::RobotModelLoaderPtr rm_loader_
Definition: planning_scene_monitor.h:613
planning_scene_monitor::PlanningSceneMonitor::new_scene_update_
SceneUpdateType new_scene_update_
Definition: planning_scene_monitor.h:531
planning_scene_monitor::PlanningSceneMonitor::tf_buffer_
std::shared_ptr< tf2_ros::Buffer > tf_buffer_
Definition: planning_scene_monitor.h:509
ros::ServiceServer
planning_scene_monitor::PlanningSceneMonitor::state_update_timer_
ros::WallTimer state_update_timer_
timer for state updates.
Definition: planning_scene_monitor.h:607
planning_scene_monitor::PlanningSceneMonitor::SceneUpdateType
SceneUpdateType
Definition: planning_scene_monitor.h:96
moveit::core::AttachedBody
planning_scene_monitor::PlanningSceneMonitor::default_robot_scale_
double default_robot_scale_
default robot scaling
Definition: planning_scene_monitor.h:516
planning_scene_monitor::LockedPlanningSceneRW
This is a convenience class for obtaining access to an instance of a locked PlanningScene.
Definition: planning_scene_monitor.h:735
planning_scene_monitor::PlanningSceneMonitor::state_pending_mutex_
boost::mutex state_pending_mutex_
Definition: planning_scene_monitor.h:589
planning_scene_monitor::PlanningSceneMonitor::initialize
void initialize(const planning_scene::PlanningScenePtr &scene)
Initialize the planning scene monitor.
Definition: planning_scene_monitor.cpp:214
planning_scene_monitor::PlanningSceneMonitor::publish_update_types_
SceneUpdateType publish_update_types_
Definition: planning_scene_monitor.h:530
planning_scene_monitor::PlanningSceneMonitor::octomap_monitor_
std::unique_ptr< occupancy_map_monitor::OccupancyMapMonitor > octomap_monitor_
Definition: planning_scene_monitor.h:546
planning_scene_monitor::PlanningSceneMonitor::updateSceneWithCurrentState
void updateSceneWithCurrentState()
Update the scene using the monitored state. This function is automatically called when an update to t...
Definition: planning_scene_monitor.cpp:1306
planning_scene_monitor::PlanningSceneMonitor::getRobotModel
const moveit::core::RobotModelConstPtr & getRobotModel() const
Definition: planning_scene_monitor.h:205
planning_scene_monitor::PlanningSceneMonitor::getDefaultAttachedObjectPadding
double getDefaultAttachedObjectPadding() const
Get the default attached padding.
Definition: planning_scene_monitor.h:275
planning_scene_monitor::PlanningSceneMonitor::last_update_time_
ros::Time last_update_time_
mutex for stored scene
Definition: planning_scene_monitor.h:501
planning_scene_monitor::PlanningSceneMonitor::CollisionBodyShapeHandles
std::map< std::string, std::vector< std::pair< occupancy_map_monitor::ShapeHandle, const Eigen::Isometry3d * > > > CollisionBodyShapeHandles
Definition: planning_scene_monitor.h:557
planning_scene_monitor::PlanningSceneMonitor::new_scene_update_condition_
boost::condition_variable_any new_scene_update_condition_
Definition: planning_scene_monitor.h:532
planning_scene_monitor::PlanningSceneMonitor::lockSceneRead
void lockSceneRead()
Lock the scene for reading (multiple threads can lock for reading at the same time)
Definition: planning_scene_monitor.cpp:991
ros::CallbackQueue
planning_scene_monitor::PlanningSceneMonitor::unlockSceneRead
void unlockSceneRead()
Unlock the scene from reading (multiple threads can lock for reading at the same time)
Definition: planning_scene_monitor.cpp:998
planning_scene_monitor::PlanningSceneMonitor::DEFAULT_PLANNING_SCENE_WORLD_TOPIC
static const std::string DEFAULT_PLANNING_SCENE_WORLD_TOPIC
Definition: planning_scene_monitor.h:126
planning_scene_monitor::LockedPlanningSceneRO::SingleUnlock::SingleUnlock
SingleUnlock(PlanningSceneMonitor *planning_scene_monitor, bool read_only)
Definition: planning_scene_monitor.h:691
planning_scene_monitor::LockedPlanningSceneRO::operator->
const planning_scene::PlanningSceneConstPtr & operator->() const
Definition: planning_scene_monitor.h:667
planning_scene_monitor::PlanningSceneMonitor::getTFClient
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.
Definition: planning_scene_monitor.h:281
planning_scene_monitor::PlanningSceneMonitor::root_nh_
ros::NodeHandle root_nh_
Definition: planning_scene_monitor.h:505
planning_scene_monitor::PlanningSceneMonitor::current_state_monitor_
CurrentStateMonitorPtr current_state_monitor_
Definition: planning_scene_monitor.h:549
planning_scene_monitor::PlanningSceneMonitor::stopPublishingPlanningScene
void stopPublishingPlanningScene()
Stop publishing the maintained planning scene.
Definition: planning_scene_monitor.cpp:338
planning_scene_monitor::PlanningSceneMonitor::excludeAttachedBodyFromOctree
void excludeAttachedBodyFromOctree(const moveit::core::AttachedBody *attached_body)
Definition: planning_scene_monitor.cpp:824
planning_scene_monitor::PlanningSceneMonitor::UPDATE_STATE
@ UPDATE_STATE
The state in the monitored scene was updated.
Definition: planning_scene_monitor.h:102
planning_scene_monitor::LockedPlanningSceneRO::SingleUnlock::~SingleUnlock
~SingleUnlock()
Definition: planning_scene_monitor.h:699
planning_scene_monitor::PlanningSceneMonitor::reconfigure_impl_
DynamicReconfigureImpl * reconfigure_impl_
Definition: planning_scene_monitor.h:618
planning_scene_monitor::PlanningSceneMonitor::queue_
ros::CallbackQueue queue_
Definition: planning_scene_monitor.h:506
planning_scene_monitor::PlanningSceneMonitor::stopSceneMonitor
void stopSceneMonitor()
Stop the scene monitor.
Definition: planning_scene_monitor.cpp:1033
planning_scene_monitor::PlanningSceneMonitor::stopWorldGeometryMonitor
void stopWorldGeometryMonitor()
Stop the world geometry monitor.
Definition: planning_scene_monitor.cpp:1140
planning_scene_monitor::PlanningSceneMonitor::DEFAULT_COLLISION_OBJECT_TOPIC
static const std::string DEFAULT_COLLISION_OBJECT_TOPIC
The name of the topic used by default for receiving collision objects in the world.
Definition: planning_scene_monitor.h:122
planning_scene_monitor::PlanningSceneMonitor::startStateMonitor
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.
Definition: planning_scene_monitor.cpp:1156
planning_scene_monitor::LockedPlanningSceneRO::SingleUnlock::planning_scene_monitor_
PlanningSceneMonitor * planning_scene_monitor_
Definition: planning_scene_monitor.h:706
planning_scene_monitor::PlanningSceneMonitor::excludeRobotLinksFromOctree
void excludeRobotLinksFromOctree()
Definition: planning_scene_monitor.cpp:720
planning_scene_monitor::PlanningSceneMonitor::getPlanningSceneServiceCallback
bool getPlanningSceneServiceCallback(moveit_msgs::GetPlanningScene::Request &req, moveit_msgs::GetPlanningScene::Response &res)
Definition: planning_scene_monitor.cpp:544
planning_scene_monitor::PlanningSceneMonitor::publish_planning_scene_
std::unique_ptr< boost::thread > publish_planning_scene_
Definition: planning_scene_monitor.h:528
planning_scene_monitor::PlanningSceneMonitor::last_robot_state_update_wall_time_
ros::WallTime last_robot_state_update_wall_time_
Last time the state was updated from current_state_monitor_.
Definition: planning_scene_monitor.h:611
planning_scene_monitor::PlanningSceneMonitor::get_scene_service_
ros::ServiceServer get_scene_service_
Definition: planning_scene_monitor.h:543
planning_scene_monitor::PlanningSceneMonitor::unlockSceneWrite
void unlockSceneWrite()
Lock the scene from writing (only one thread can lock for writing and no other thread can lock for re...
Definition: planning_scene_monitor.cpp:1012
planning_scene_monitor::PlanningSceneMonitor::robot_description_
std::string robot_description_
Definition: planning_scene_monitor.h:511
planning_scene_monitor::PlanningSceneMonitor::default_object_padd_
double default_object_padd_
default object padding
Definition: planning_scene_monitor.h:518
planning_scene_monitor::PlanningSceneMonitor::robot_model_
moveit::core::RobotModelConstPtr robot_model_
Definition: planning_scene_monitor.h:614
planning_scene_monitor::PlanningSceneMonitor::includeAttachedBodyInOctree
void includeAttachedBodyInOctree(const moveit::core::AttachedBody *attached_body)
Definition: planning_scene_monitor.cpp:846
planning_scene_monitor::PlanningSceneMonitor::stateUpdateTimerCallback
void stateUpdateTimerCallback(const ros::WallTimerEvent &event)
Definition: planning_scene_monitor.cpp:1226
ros::WallTime
planning_scene_monitor::PlanningSceneMonitor::setPlanningScenePublishingFrequency
void setPlanningScenePublishingFrequency(double hz)
Set the maximum frequency at which planning scenes are being published.
Definition: planning_scene_monitor.cpp:1345
planning_scene_monitor::PlanningSceneMonitor::default_robot_link_scale_
std::map< std::string, double > default_robot_link_scale_
default robot link scale
Definition: planning_scene_monitor.h:524
planning_scene_monitor::PlanningSceneMonitor::monitor_name_
std::string monitor_name_
The name of this scene monitor.
Definition: planning_scene_monitor.h:495
planning_scene_monitor::PlanningSceneMonitor::publish_planning_scene_frequency_
double publish_planning_scene_frequency_
Definition: planning_scene_monitor.h:529
planning_scene_monitor::PlanningSceneMonitor
PlanningSceneMonitor Subscribes to the topic planning_scene.
Definition: planning_scene_monitor.h:93
planning_scene_monitor::PlanningSceneMonitor::shape_handles_lock_
boost::recursive_mutex shape_handles_lock_
Definition: planning_scene_monitor.h:562
planning_scene_monitor::PlanningSceneMonitor::getRobotDescription
const std::string & getRobotDescription() const
Get the stored robot description.
Definition: planning_scene_monitor.h:251
planning_scene_monitor::PlanningSceneMonitor::startWorldGeometryMonitor
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:
Definition: planning_scene_monitor.cpp:1097
callback_queue.h
planning_scene_monitor::PlanningSceneMonitor::AttachedBodyShapeHandles
std::map< const moveit::core::AttachedBody *, std::vector< std::pair< occupancy_map_monitor::ShapeHandle, std::size_t > > > AttachedBodyShapeHandles
Definition: planning_scene_monitor.h:555
planning_scene_monitor::PlanningSceneMonitor::getLastUpdateTime
const ros::Time & getLastUpdateTime() const
Return the time when the last update was made to the planning scene (by any monitor)
Definition: planning_scene_monitor.h:411
planning_scene_monitor::LockedPlanningSceneRO::planning_scene_monitor_
PlanningSceneMonitorPtr planning_scene_monitor_
Definition: planning_scene_monitor.h:710
occupancy_map_monitor.h
planning_scene_monitor::PlanningSceneMonitor::UPDATE_NONE
@ UPDATE_NONE
No update.
Definition: planning_scene_monitor.h:99
planning_scene_monitor::MOVEIT_CLASS_FORWARD
MOVEIT_CLASS_FORWARD(CurrentStateMonitor)
planning_scene_monitor::PlanningSceneMonitor::scene_
planning_scene::PlanningScenePtr scene_
Definition: planning_scene_monitor.h:497
planning_scene_monitor::PlanningSceneMonitor::newPlanningSceneWorldCallback
void newPlanningSceneWorldCallback(const moveit_msgs::PlanningSceneWorldConstPtr &world)
Callback for a new planning scene world.
Definition: planning_scene_monitor.cpp:667
planning_scene_monitor::LockedPlanningSceneRO::LockedPlanningSceneRO
LockedPlanningSceneRO(const PlanningSceneMonitorPtr &planning_scene_monitor)
Definition: planning_scene_monitor.h:646
planning_scene_monitor::PlanningSceneMonitor::nh_
ros::NodeHandle nh_
Last time the robot has moved.
Definition: planning_scene_monitor.h:504
planning_scene_monitor::PlanningSceneMonitor::DEFAULT_JOINT_STATES_TOPIC
static const std::string DEFAULT_JOINT_STATES_TOPIC
The name of the topic used by default for receiving joint states.
Definition: planning_scene_monitor.h:116
planning_scene_monitor::PlanningSceneMonitor::currentStateAttachedBodyUpdateCallback
void currentStateAttachedBodyUpdateCallback(moveit::core::AttachedBody *attached_body, bool just_attached)
Callback for a change for an attached object of the current state of the planning scene.
Definition: planning_scene_monitor.cpp:903
ros::WallTimerEvent
planning_scene.h
collision_detection::CollisionPluginLoader
This is used to load the collision plugin.
Definition: collision_plugin_loader.h:78
planning_scene_monitor::PlanningSceneMonitor::DEFAULT_PLANNING_SCENE_TOPIC
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.
Definition: planning_scene_monitor.h:129
planning_scene_monitor::LockedPlanningSceneRO::MOVEIT_STRUCT_FORWARD
MOVEIT_STRUCT_FORWARD(SingleUnlock)
planning_scene_monitor::PlanningSceneMonitor::MONITORED_PLANNING_SCENE_TOPIC
static const std::string MONITORED_PLANNING_SCENE_TOPIC
Definition: planning_scene_monitor.h:136
ros::Time
planning_scene_monitor::PlanningSceneMonitor::includeRobotLinksInOctree
void includeRobotLinksInOctree()
Definition: planning_scene_monitor.cpp:756
planning_scene_monitor::PlanningSceneMonitor::getStateUpdateFrequency
double getStateUpdateFrequency() const
Get the maximum frequency (Hz) at which the current state of the planning scene is updated.
Definition: planning_scene_monitor.h:352
planning_scene_monitor::PlanningSceneMonitor::waitForCurrentRobotState
bool waitForCurrentRobotState(const ros::Time &t, double wait_time=1.)
Wait for robot state to become more recent than time t.
Definition: planning_scene_monitor.cpp:934
planning_scene_monitor::PlanningSceneMonitor::state_update_pending_
volatile bool state_update_pending_
True when we need to update the RobotState from current_state_monitor_.
Definition: planning_scene_monitor.h:593
planning_scene_monitor::PlanningSceneMonitor::attachObjectCallback
void attachObjectCallback(const moveit_msgs::AttachedCollisionObjectConstPtr &obj)
Callback for a new attached object msg.
Definition: planning_scene_monitor.cpp:706
class_forward.h
planning_scene_monitor::PlanningSceneMonitor::startSceneMonitor
void startSceneMonitor(const std::string &scene_topic=DEFAULT_PLANNING_SCENE_TOPIC)
Start the scene monitor (ROS topic-based)
Definition: planning_scene_monitor.cpp:1019
planning_scene_monitor::LockedPlanningSceneRO
This is a convenience class for obtaining access to an instance of a locked PlanningScene.
Definition: planning_scene_monitor.h:643
planning_scene_monitor::PlanningSceneMonitor::newPlanningSceneCallback
void newPlanningSceneCallback(const moveit_msgs::PlanningSceneConstPtr &scene)
Definition: planning_scene_monitor.cpp:559
planning_scene_monitor::PlanningSceneMonitor::planning_scene_subscriber_
ros::Subscriber planning_scene_subscriber_
Definition: planning_scene_monitor.h:535
planning_scene_monitor::PlanningSceneMonitor::includeWorldObjectsInOctree
void includeWorldObjectsInOctree()
Definition: planning_scene_monitor.cpp:799
planning_scene_monitor::PlanningSceneMonitor::excludeAttachedBodiesFromOctree
void excludeAttachedBodiesFromOctree()
Definition: planning_scene_monitor.cpp:787
planning_scene_monitor::PlanningSceneMonitor::configureCollisionMatrix
void configureCollisionMatrix(const planning_scene::PlanningScenePtr &scene)
Configure the collision matrix for a particular scene.
Definition: planning_scene_monitor.cpp:1405
planning_scene_monitor::PlanningSceneMonitor::dt_state_update_
ros::WallDuration dt_state_update_
the amount of time to wait in between updates to the robot state
Definition: planning_scene_monitor.h:597
planning_scene_monitor::PlanningSceneMonitor::UPDATE_GEOMETRY
@ UPDATE_GEOMETRY
The geometry of the scene was updated. This includes receiving new octomaps, collision objects,...
Definition: planning_scene_monitor.h:109
planning_scene_monitor::PlanningSceneMonitor::providePlanningSceneService
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 ...
Definition: planning_scene_monitor.cpp:537
planning_scene_monitor::PlanningSceneMonitor::getMonitoredTopics
void getMonitoredTopics(std::vector< std::string > &topics) const
Get the topic names that the monitor is listening to.
Definition: planning_scene_monitor.cpp:453
planning_scene_monitor::PlanningSceneMonitor::getUpdatedFrameTransforms
void getUpdatedFrameTransforms(std::vector< geometry_msgs::TransformStamped > &transforms)
Definition: planning_scene_monitor.cpp:1352
planning_scene_monitor::PlanningSceneMonitor::getPlanningScenePublishingFrequency
double getPlanningScenePublishingFrequency() const
Get the maximum frequency at which planning scenes are published (Hz)
Definition: planning_scene_monitor.h:306
planning_scene_monitor::PlanningSceneMonitor::clearUpdateCallbacks
void clearUpdateCallbacks()
Clear the functions to be called when an update to the scene is received.
Definition: planning_scene_monitor.cpp:1339
planning_scene_monitor::PlanningSceneMonitor::clearOctomap
void clearOctomap()
Definition: planning_scene_monitor.cpp:564
planning_scene_monitor::PlanningSceneMonitor::UPDATE_SCENE
@ UPDATE_SCENE
The entire scene was updated.
Definition: planning_scene_monitor.h:112
DurationBase< WallDuration >::toSec
double toSec() const
planning_scene_monitor::LockedPlanningSceneRO::initialize
void initialize(bool read_only)
Definition: planning_scene_monitor.h:679
planning_scene_monitor::PlanningSceneMonitor::parent_scene_
planning_scene::PlanningScenePtr parent_scene_
Definition: planning_scene_monitor.h:499
ros::WallDuration
planning_scene_monitor::PlanningSceneMonitor::requestPlanningSceneState
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...
Definition: planning_scene_monitor.cpp:504
planning_scene_monitor::LockedPlanningSceneRO::SingleUnlock::read_only_
bool read_only_
Definition: planning_scene_monitor.h:707
planning_scene_monitor::PlanningSceneMonitor::getDefaultObjectPadding
double getDefaultObjectPadding() const
Get the default object padding.
Definition: planning_scene_monitor.h:269
planning_scene_monitor::PlanningSceneMonitor::startPublishingPlanningScene
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...
Definition: planning_scene_monitor.cpp:352
planning_scene_monitor::PlanningSceneMonitor::monitorDiffs
void monitorDiffs(bool flag)
By default, the maintained planning scene does not reason about diffs. When the flag passed in is tru...
Definition: planning_scene_monitor.cpp:291
planning_scene_monitor::PlanningSceneMonitor::update_callbacks_
std::vector< boost::function< void(SceneUpdateType)> > update_callbacks_
Definition: planning_scene_monitor.h:566
planning_scene_monitor::PlanningSceneMonitor::UPDATE_TRANSFORMS
@ UPDATE_TRANSFORMS
The maintained set of fixed transforms in the monitored scene was updated.
Definition: planning_scene_monitor.h:105
ros::Duration
collision_plugin_loader.h
planning_scene_monitor::PlanningSceneMonitor::getDefaultRobotPadding
double getDefaultRobotPadding() const
Get the default robot padding.
Definition: planning_scene_monitor.h:257
planning_scene_monitor::PlanningSceneMonitor::getRobotModelLoader
const robot_model_loader::RobotModelLoaderPtr & getRobotModelLoader() const
Get the user kinematic model loader.
Definition: planning_scene_monitor.h:200
planning_scene_monitor::PlanningSceneMonitor::planning_scene_publisher_
ros::Publisher planning_scene_publisher_
Definition: planning_scene_monitor.h:527
planning_scene_monitor::PlanningSceneMonitor::link_shape_handles_
LinkShapeHandles link_shape_handles_
Definition: planning_scene_monitor.h:559
planning_scene_monitor::PlanningSceneMonitor::includeWorldObjectInOctree
void includeWorldObjectInOctree(const collision_detection::World::ObjectConstPtr &obj)
Definition: planning_scene_monitor.cpp:886
planning_scene_monitor::PlanningSceneMonitor::scene_update_mutex_
boost::shared_mutex scene_update_mutex_
if diffs are monitored, this is the pointer to the parent scene
Definition: planning_scene_monitor.h:500
planning_scene_monitor::PlanningSceneMonitor::includeAttachedBodiesInOctree
void includeAttachedBodiesInOctree()
Definition: planning_scene_monitor.cpp:771
DurationBase< WallDuration >::isZero
bool isZero() const
ros::NodeHandle
ros::Subscriber
planning_scene_monitor::LockedPlanningSceneRW::operator->
const planning_scene::PlanningScenePtr & operator->()
Definition: planning_scene_monitor.h:748
planning_scene_monitor::PlanningSceneMonitor::PlanningSceneMonitor
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.
Definition: planning_scene_monitor.cpp:149
planning_scene_monitor::PlanningSceneMonitor::shape_transform_cache_lookup_wait_time_
ros::Duration shape_transform_cache_lookup_wait_time_
the amount of time to wait when looking up transforms
Definition: planning_scene_monitor.h:602
planning_scene_monitor::PlanningSceneMonitor::DEFAULT_ATTACHED_COLLISION_OBJECT_TOPIC
static const std::string DEFAULT_ATTACHED_COLLISION_OBJECT_TOPIC
The name of the topic used by default for attached collision objects.
Definition: planning_scene_monitor.h:119
robot_model_loader.h
planning_scene_monitor::PlanningSceneMonitor::lockSceneWrite
void lockSceneWrite()
Lock the scene for writing (only one thread can lock for writing and no other thread can lock for rea...
Definition: planning_scene_monitor.cpp:1005


planning
Author(s): Ioan Sucan , Sachin Chitta
autogenerated on Wed Jul 21 2021 02:24:08