moveit2
The MoveIt Motion Planning Framework for ROS 2.
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 <rclcpp/rclcpp.hpp>
40 #include <tf2_ros/buffer.h>
41 #include <tf2_ros/transform_listener.h>
48 #include <moveit_msgs/srv/get_planning_scene.hpp>
49 #include <memory>
50 #include <thread>
51 #include <shared_mutex>
52 #include <mutex>
53 #include <thread>
54 
55 #include <moveit_planning_scene_monitor_export.h>
56 
57 namespace planning_scene_monitor
58 {
59 MOVEIT_CLASS_FORWARD(PlanningSceneMonitor); // Defines PlanningSceneMonitorPtr, ConstPtr, WeakPtr... etc
60 
64 class MOVEIT_PLANNING_SCENE_MONITOR_EXPORT PlanningSceneMonitor
65 {
66 public:
71 
76 
78  {
80  UPDATE_NONE = 0,
81 
83  UPDATE_STATE = 1,
84 
86  UPDATE_TRANSFORMS = 2,
87 
90  UPDATE_GEOMETRY = 4,
91 
93  UPDATE_SCENE = 8 + UPDATE_STATE + UPDATE_TRANSFORMS + UPDATE_GEOMETRY
94  };
95 
97  static const std::string DEFAULT_JOINT_STATES_TOPIC; // "/joint_states"
98 
100  static const std::string DEFAULT_ATTACHED_COLLISION_OBJECT_TOPIC; // "/attached_collision_object"
101 
103  static const std::string DEFAULT_COLLISION_OBJECT_TOPIC; // "/collision_object"
104 
107  static const std::string DEFAULT_PLANNING_SCENE_WORLD_TOPIC; // "/planning_scene_world"
108 
110  static const std::string DEFAULT_PLANNING_SCENE_TOPIC; // "/planning_scene"
111 
113  static const std::string DEFAULT_PLANNING_SCENE_SERVICE; // "/get_planning_scene"
114 
117  static const std::string MONITORED_PLANNING_SCENE_TOPIC; // "monitored_planning_scene"
118 
124  [[deprecated("Passing tf2_ros::Buffer to PlanningSceneMonitor's constructor is deprecated")]] PlanningSceneMonitor(
125  const rclcpp::Node::SharedPtr& node, const std::string& robot_description,
126  const std::shared_ptr<tf2_ros::Buffer>& /* unused */, const std::string& name = "")
127  : PlanningSceneMonitor(node, robot_description, name)
128  {
129  }
130  PlanningSceneMonitor(const rclcpp::Node::SharedPtr& node, const std::string& robot_description,
131  const std::string& name = "");
132 
138  [[deprecated("Passing tf2_ros::Buffer to PlanningSceneMonitor's constructor is deprecated")]] PlanningSceneMonitor(
139  const rclcpp::Node::SharedPtr& node, const robot_model_loader::RobotModelLoaderPtr& rml,
140  const std::shared_ptr<tf2_ros::Buffer>& /* unused */, const std::string& name = "")
141  : PlanningSceneMonitor(node, rml, name)
142  {
143  }
144  PlanningSceneMonitor(const rclcpp::Node::SharedPtr& node, const robot_model_loader::RobotModelLoaderPtr& rml,
145  const std::string& name = "");
146 
153  [[deprecated("Passing tf2_ros::Buffer to PlanningSceneMonitor's constructor is deprecated")]] PlanningSceneMonitor(
154  const rclcpp::Node::SharedPtr& node, const planning_scene::PlanningScenePtr& scene,
155  const std::string& robot_description, const std::shared_ptr<tf2_ros::Buffer>& /* unused */,
156  const std::string& name = "")
157  : PlanningSceneMonitor(node, scene, robot_description, name)
158  {
159  }
160  PlanningSceneMonitor(const rclcpp::Node::SharedPtr& node, const planning_scene::PlanningScenePtr& scene,
161  const std::string& robot_description, const std::string& name = "");
162 
169  [[deprecated("Passing tf2_ros::Buffer to PlanningSceneMonitor's constructor is deprecated")]] PlanningSceneMonitor(
170  const rclcpp::Node::SharedPtr& node, const planning_scene::PlanningScenePtr& scene,
171  const robot_model_loader::RobotModelLoaderPtr& rml, const std::shared_ptr<tf2_ros::Buffer>& /* unused */,
172  const std::string& name = "")
173  : PlanningSceneMonitor(node, scene, rml, name)
174  {
175  }
176  PlanningSceneMonitor(const rclcpp::Node::SharedPtr& node, const planning_scene::PlanningScenePtr& scene,
177  const robot_model_loader::RobotModelLoaderPtr& rml, const std::string& name = "");
178 
180 
182  const std::string& getName() const
183  {
184  return monitor_name_;
185  }
186 
188  const robot_model_loader::RobotModelLoaderPtr& getRobotModelLoader() const
189  {
190  return rm_loader_;
191  }
192 
193  const moveit::core::RobotModelConstPtr& getRobotModel() const
194  {
195  return robot_model_;
196  }
197 
212  const planning_scene::PlanningScenePtr& getPlanningScene()
213  {
214  return scene_;
215  }
216 
220  const planning_scene::PlanningSceneConstPtr& getPlanningScene() const
221  {
222  return scene_const_;
223  }
224 
229  bool updatesScene(const planning_scene::PlanningSceneConstPtr& scene) const;
230 
235  bool updatesScene(const planning_scene::PlanningScenePtr& scene) const;
236 
239  const std::string& getRobotDescription() const
240  {
241  return robot_description_;
242  }
243 
245  double getDefaultRobotPadding() const
246  {
247  return default_robot_padd_;
248  }
249 
251  double getDefaultRobotScale() const
252  {
253  return default_robot_scale_;
254  }
255 
257  double getDefaultObjectPadding() const
258  {
259  return default_object_padd_;
260  }
261 
264  {
265  return default_attached_padd_;
266  }
267 
269  const std::shared_ptr<tf2_ros::Buffer>& getTFClient() const
270  {
271  return tf_buffer_;
272  }
273 
279  void monitorDiffs(bool flag);
280 
284  void startPublishingPlanningScene(SceneUpdateType event,
285  const std::string& planning_scene_topic = MONITORED_PLANNING_SCENE_TOPIC);
286 
288  void stopPublishingPlanningScene();
289 
291  void setPlanningScenePublishingFrequency(double hz);
292 
295  {
296  return publish_planning_scene_frequency_;
297  }
298 
301  const CurrentStateMonitorPtr& getStateMonitor() const
302  {
303  return current_state_monitor_;
304  }
305 
306  CurrentStateMonitorPtr& getStateMonitorNonConst()
307  {
308  return current_state_monitor_;
309  }
310 
316  void updateFrameTransforms();
317 
321  void startStateMonitor(const std::string& joint_states_topic = DEFAULT_JOINT_STATES_TOPIC,
322  const std::string& attached_objects_topic = DEFAULT_ATTACHED_COLLISION_OBJECT_TOPIC);
323 
325  void stopStateMonitor();
326 
331  void updateSceneWithCurrentState(bool skip_update_if_locked = false);
332 
338  void setStateUpdateFrequency(double hz);
339 
341  double getStateUpdateFrequency() const
342  {
343  if (dt_state_update_.count() > 0.0)
344  return 1.0 / dt_state_update_.count();
345  else
346  return 0.0;
347  }
348 
352  void startSceneMonitor(const std::string& scene_topic = DEFAULT_PLANNING_SCENE_TOPIC);
353 
361  bool requestPlanningSceneState(const std::string& service_name = DEFAULT_PLANNING_SCENE_SERVICE);
362 
370  void providePlanningSceneService(const std::string& service_name = DEFAULT_PLANNING_SCENE_SERVICE);
371 
373  void stopSceneMonitor();
374 
383  void startWorldGeometryMonitor(const std::string& collision_objects_topic = DEFAULT_COLLISION_OBJECT_TOPIC,
384  const std::string& planning_scene_world_topic = DEFAULT_PLANNING_SCENE_WORLD_TOPIC,
385  const bool load_octomap_monitor = true);
386 
388  void stopWorldGeometryMonitor();
389 
391  void addUpdateCallback(const std::function<void(SceneUpdateType)>& fn);
392 
394  void clearUpdateCallbacks();
395 
397  void getMonitoredTopics(std::vector<std::string>& topics) const;
398 
400  const rclcpp::Time& getLastUpdateTime() const
401  {
402  return last_update_time_;
403  }
404 
405  void publishDebugInformation(bool flag);
406 
408  void triggerSceneUpdateEvent(SceneUpdateType update_type);
409 
415  bool waitForCurrentRobotState(const rclcpp::Time& t, double wait_time = 1.);
416 
417  void clearOctomap();
418 
419  // Called to update the planning scene with a new message.
420  bool newPlanningSceneMessage(const moveit_msgs::msg::PlanningScene& scene);
421 
422 protected:
426  void initialize(const planning_scene::PlanningScenePtr& scene);
427 
429  void lockSceneRead();
430 
432  void unlockSceneRead();
433 
436  void lockSceneWrite();
437 
440  void unlockSceneWrite();
441 
443  void configureCollisionMatrix(const planning_scene::PlanningScenePtr& scene);
444 
446  void configureDefaultPadding();
447 
449  void collisionObjectCallback(const moveit_msgs::msg::CollisionObject::ConstSharedPtr& obj);
450 
452  void newPlanningSceneWorldCallback(const moveit_msgs::msg::PlanningSceneWorld::ConstSharedPtr& world);
453 
455  void octomapUpdateCallback();
456 
458  void attachObjectCallback(const moveit_msgs::msg::AttachedCollisionObject::ConstSharedPtr& obj);
459 
461  void currentStateAttachedBodyUpdateCallback(moveit::core::AttachedBody* attached_body, bool just_attached);
462 
464  void currentWorldObjectUpdateCallback(const collision_detection::World::ObjectConstPtr& object,
466 
467  void includeRobotLinksInOctree();
468  void excludeRobotLinksFromOctree();
469 
470  void excludeWorldObjectsFromOctree();
471  void includeWorldObjectsInOctree();
472  void excludeWorldObjectFromOctree(const collision_detection::World::ObjectConstPtr& obj);
473  void includeWorldObjectInOctree(const collision_detection::World::ObjectConstPtr& obj);
474 
475  void excludeAttachedBodiesFromOctree();
476  void includeAttachedBodiesInOctree();
477  void excludeAttachedBodyFromOctree(const moveit::core::AttachedBody* attached_body);
478  void includeAttachedBodyInOctree(const moveit::core::AttachedBody* attached_body);
479 
480  bool getShapeTransformCache(const std::string& target_frame, const rclcpp::Time& target_time,
482 
484  std::string monitor_name_;
485 
486  planning_scene::PlanningScenePtr scene_;
487  planning_scene::PlanningSceneConstPtr scene_const_;
488  planning_scene::PlanningScenePtr parent_scene_;
489  std::shared_mutex scene_update_mutex_;
490  rclcpp::Time last_update_time_;
491  rclcpp::Time last_robot_motion_time_;
492 
493  std::shared_ptr<rclcpp::Node> node_;
494 
495  // TODO: (anasarrak) callbacks on ROS2?
496  // https://answers.ros.org/question/300874/how-do-you-use-callbackgroups-as-a-replacement-for-callbackqueues-in-ros2/
497  // ros::CallbackQueue queue_;
498  std::shared_ptr<rclcpp::Node> pnode_;
499  std::shared_ptr<rclcpp::executors::SingleThreadedExecutor> private_executor_;
501 
502  std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
503  std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
504 
505  std::string robot_description_;
506 
516  std::map<std::string, double> default_robot_link_padd_;
518  std::map<std::string, double> default_robot_link_scale_;
519 
520  // variables for planning scene publishing
521  rclcpp::Publisher<moveit_msgs::msg::PlanningScene>::SharedPtr planning_scene_publisher_;
522  std::unique_ptr<std::thread> publish_planning_scene_;
525  std::atomic<SceneUpdateType> new_scene_update_;
526  std::condition_variable_any new_scene_update_condition_;
527 
528  // subscribe to various sources of data
529  rclcpp::Subscription<moveit_msgs::msg::PlanningScene>::SharedPtr planning_scene_subscriber_;
530  rclcpp::Subscription<moveit_msgs::msg::PlanningSceneWorld>::SharedPtr planning_scene_world_subscriber_;
531 
532  rclcpp::Subscription<moveit_msgs::msg::AttachedCollisionObject>::SharedPtr attached_collision_object_subscriber_;
533  rclcpp::Subscription<moveit_msgs::msg::CollisionObject>::SharedPtr collision_object_subscriber_;
534 
535  // provide an optional service to get the full planning scene state
536  // this is used by MoveGroup and related application nodes
537  rclcpp::Service<moveit_msgs::srv::GetPlanningScene>::SharedPtr get_scene_service_;
538 
539  // include a octomap monitor
540  std::unique_ptr<occupancy_map_monitor::OccupancyMapMonitor> octomap_monitor_;
541 
542  // include a current state monitor
543  CurrentStateMonitorPtr current_state_monitor_;
544 
545  typedef std::map<const moveit::core::LinkModel*,
546  std::vector<std::pair<occupancy_map_monitor::ShapeHandle, std::size_t> > >
549  std::vector<std::pair<occupancy_map_monitor::ShapeHandle, std::size_t> > >;
551  std::map<std::string, std::vector<std::pair<occupancy_map_monitor::ShapeHandle, const Eigen::Isometry3d*> > >;
552 
556  mutable std::recursive_mutex shape_handles_lock_;
557 
559  std::recursive_mutex update_lock_;
560  std::vector<std::function<void(SceneUpdateType)> > update_callbacks_;
562 
563 private:
564  void getUpdatedFrameTransforms(std::vector<geometry_msgs::msg::TransformStamped>& transforms);
565 
566  // publish planning scene update diffs (runs in its own thread)
567  void scenePublishingThread();
568 
569  // called by current_state_monitor_ when robot state (as monitored on joint state topic) changes
570  void onStateUpdate(const sensor_msgs::msg::JointState::ConstSharedPtr& joint_state);
571 
572  // called by state_update_timer_ when a state update it pending
573  void stateUpdateTimerCallback();
574 
575  // Callback for a new planning scene msg
576  void newPlanningSceneCallback(const moveit_msgs::msg::PlanningScene::ConstSharedPtr& scene);
577 
578  // Callback for requesting the full planning scene via service
579  void getPlanningSceneServiceCallback(const moveit_msgs::srv::GetPlanningScene::Request::SharedPtr& req,
580  const moveit_msgs::srv::GetPlanningScene::Response::SharedPtr& res);
581 
582  void updatePublishSettings(bool publish_geom_updates, bool publish_state_updates, bool publish_transform_updates,
583  bool publish_planning_scene, double publish_planning_scene_hz);
584 
586  std::atomic<bool> state_update_pending_;
587 
588  // Lock for writing last_robot_state_update_wall_time_ and dt_state_update_
589  std::mutex state_update_mutex_;
590 
592  // Only access this from callback functions (and constructor)
593  // This field is protected by state_update_mutex_
594  std::chrono::system_clock::time_point last_robot_state_update_wall_time_;
595 
597  // This field is protected by state_update_mutex_
598  std::chrono::duration<double> dt_state_update_; // 1hz
599 
601  // Setting this to a non-zero value resolves issues when the sensor data is
602  // arriving so fast that it is preceding the transform state.
603  rclcpp::Duration shape_transform_cache_lookup_wait_time_;
604 
606  // If state_update_pending_ is true, call updateSceneWithCurrentState()
607  // Not safe to access from callback functions.
608  rclcpp::TimerBase::SharedPtr state_update_timer_;
609 
610  robot_model_loader::RobotModelLoaderPtr rm_loader_;
611  moveit::core::RobotModelConstPtr robot_model_;
612 
614 
615  rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr callback_handler_;
616 
617  bool use_sim_time_;
618 
619  friend class LockedPlanningSceneRO;
620  friend class LockedPlanningSceneRW;
621 };
622 
645 {
646 public:
647  LockedPlanningSceneRO(const PlanningSceneMonitorPtr& planning_scene_monitor)
649  {
650  initialize(true);
651  }
652 
653  const PlanningSceneMonitorPtr& getPlanningSceneMonitor()
654  {
656  }
657 
658  operator bool() const
659  {
660  return planning_scene_monitor_ && planning_scene_monitor_->getPlanningScene();
661  }
662 
663  operator const planning_scene::PlanningSceneConstPtr&() const
664  {
665  return static_cast<const PlanningSceneMonitor*>(planning_scene_monitor_.get())->getPlanningScene();
666  }
667 
668  const planning_scene::PlanningSceneConstPtr& operator->() const
669  {
670  return static_cast<const PlanningSceneMonitor*>(planning_scene_monitor_.get())->getPlanningScene();
671  }
672 
673 protected:
674  LockedPlanningSceneRO(const PlanningSceneMonitorPtr& planning_scene_monitor, bool read_only)
676  {
677  initialize(read_only);
678  }
679 
680  void initialize(bool read_only)
681  {
683  lock_ = std::make_shared<SingleUnlock>(planning_scene_monitor_.get(), read_only);
684  }
685 
687 
688  // we use this struct so that lock/unlock are called only once
689  // even if the LockedPlanningScene instance is copied around
691  {
694  {
695  if (read_only)
697  else
699  }
701  {
702  if (read_only_)
704  else
706  }
709  };
710 
711  PlanningSceneMonitorPtr planning_scene_monitor_;
712  SingleUnlockPtr lock_;
713 };
714 
737 {
738 public:
739  LockedPlanningSceneRW(const PlanningSceneMonitorPtr& planning_scene_monitor)
741  {
742  }
743 
744  operator const planning_scene::PlanningScenePtr&()
745  {
746  return planning_scene_monitor_->getPlanningScene();
747  }
748 
749  const planning_scene::PlanningScenePtr& operator->()
750  {
751  return planning_scene_monitor_->getPlanningScene();
752  }
753 };
754 } // namespace planning_scene_monitor
Represents an action that occurred on an object in the world. Several bits may be set indicating seve...
Definition: world.h:265
Object defining bodies that can be attached to robot links.
Definition: attached_body.h:58
A link from the robot. Contains the constant transform applied to the link and its geometry.
Definition: link_model.h:72
This is a convenience class for obtaining access to an instance of a locked PlanningScene.
const planning_scene::PlanningSceneConstPtr & operator->() const
LockedPlanningSceneRO(const PlanningSceneMonitorPtr &planning_scene_monitor, bool read_only)
LockedPlanningSceneRO(const PlanningSceneMonitorPtr &planning_scene_monitor)
const PlanningSceneMonitorPtr & getPlanningSceneMonitor()
This is a convenience class for obtaining access to an instance of a locked PlanningScene.
const planning_scene::PlanningScenePtr & operator->()
LockedPlanningSceneRW(const PlanningSceneMonitorPtr &planning_scene_monitor)
PlanningSceneMonitor Subscribes to the topic planning_scene.
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.
const planning_scene::PlanningScenePtr & getPlanningScene()
Avoid this function! Returns an unsafe pointer to the current planning scene.
void lockSceneRead()
Lock the scene for reading (multiple threads can lock for reading at the same time)
void lockSceneWrite()
Lock the scene for writing (only one thread can lock for writing and no other thread can lock for rea...
void unlockSceneRead()
Unlock the scene from reading (multiple threads can lock for reading at the same time)
static const std::string DEFAULT_PLANNING_SCENE_SERVICE
The name of the service used by default for requesting full planning scene state.
double default_attached_padd_
default attached padding
std::unique_ptr< occupancy_map_monitor::OccupancyMapMonitor > octomap_monitor_
PlanningSceneMonitor(const rclcpp::Node::SharedPtr &node, const std::string &robot_description, const std::shared_ptr< tf2_ros::Buffer > &, const std::string &name="")
Constructor.
const moveit::core::RobotModelConstPtr & getRobotModel() const
const std::string & getRobotDescription() const
Get the stored robot description.
std::map< std::string, double > default_robot_link_padd_
default robot link padding
static const std::string DEFAULT_ATTACHED_COLLISION_OBJECT_TOPIC
The name of the topic used by default for attached collision objects.
PlanningSceneMonitor & operator=(const PlanningSceneMonitor &)=delete
PlanningSceneMonitor cannot be copy-assigned.
static const std::string DEFAULT_COLLISION_OBJECT_TOPIC
The name of the topic used by default for receiving collision objects in the world.
PlanningSceneMonitor(const PlanningSceneMonitor &)=delete
PlanningSceneMonitor cannot be copy-constructed.
rclcpp::Subscription< moveit_msgs::msg::AttachedCollisionObject >::SharedPtr attached_collision_object_subscriber_
std::map< std::string, std::vector< std::pair< occupancy_map_monitor::ShapeHandle, const Eigen::Isometry3d * > > > CollisionBodyShapeHandles
const std::string & getName() const
Get the name of this monitor.
rclcpp::Service< moveit_msgs::srv::GetPlanningScene >::SharedPtr get_scene_service_
std::map< const moveit::core::LinkModel *, std::vector< std::pair< occupancy_map_monitor::ShapeHandle, std::size_t > > > LinkShapeHandles
double getDefaultAttachedObjectPadding() const
Get the default attached padding.
planning_scene::PlanningSceneConstPtr scene_const_
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.
rclcpp::Subscription< moveit_msgs::msg::CollisionObject >::SharedPtr collision_object_subscriber_
rclcpp::Subscription< moveit_msgs::msg::PlanningScene >::SharedPtr planning_scene_subscriber_
PlanningSceneMonitor(const rclcpp::Node::SharedPtr &node, const robot_model_loader::RobotModelLoaderPtr &rml, const std::shared_ptr< tf2_ros::Buffer > &, const std::string &name="")
Constructor.
double getDefaultRobotPadding() const
Get the default robot padding.
std::shared_mutex scene_update_mutex_
if diffs are monitored, this is the pointer to the parent scene
std::shared_ptr< tf2_ros::TransformListener > tf_listener_
double getStateUpdateFrequency() const
Get the maximum frequency (Hz) at which the current state of the planning scene is updated.
std::shared_ptr< rclcpp::executors::SingleThreadedExecutor > private_executor_
const CurrentStateMonitorPtr & getStateMonitor() const
Get the stored instance of the stored current state monitor.
const planning_scene::PlanningSceneConstPtr & getPlanningScene() const
Avoid this function! Returns an unsafe pointer to the current planning scene.
std::map< const moveit::core::AttachedBody *, std::vector< std::pair< occupancy_map_monitor::ShapeHandle, std::size_t > > > AttachedBodyShapeHandles
double getDefaultObjectPadding() const
Get the default object padding.
std::map< std::string, double > default_robot_link_scale_
default robot link scale
rclcpp::Time last_robot_motion_time_
Last time the state was updated.
double getDefaultRobotScale() const
Get the default robot scaling.
const rclcpp::Time & getLastUpdateTime() const
Return the time when the last update was made to the planning scene (by any monitor)
std::string monitor_name_
The name of this scene monitor.
PlanningSceneMonitor(const rclcpp::Node::SharedPtr &node, const planning_scene::PlanningScenePtr &scene, const std::string &robot_description, const std::shared_ptr< tf2_ros::Buffer > &, const std::string &name="")
Constructor.
std::recursive_mutex update_lock_
lock access to update_callbacks_
rclcpp::Time last_update_time_
mutex for stored scene
PlanningSceneMonitor(const rclcpp::Node::SharedPtr &node, const planning_scene::PlanningScenePtr &scene, const robot_model_loader::RobotModelLoaderPtr &rml, const std::shared_ptr< tf2_ros::Buffer > &, const std::string &name="")
Constructor.
std::unique_ptr< std::thread > publish_planning_scene_
std::shared_ptr< rclcpp::Node > node_
Last time the robot has moved.
rclcpp::Publisher< moveit_msgs::msg::PlanningScene >::SharedPtr planning_scene_publisher_
static const std::string DEFAULT_JOINT_STATES_TOPIC
The name of the topic used by default for receiving joint states.
std::vector< std::function< void(SceneUpdateType)> > update_callbacks_
double getPlanningScenePublishingFrequency() const
Get the maximum frequency at which planning scenes are published (Hz)
planning_scene::PlanningScenePtr parent_scene_
std::shared_ptr< tf2_ros::Buffer > tf_buffer_
void unlockSceneWrite()
Lock the scene from writing (only one thread can lock for writing and no other thread can lock for re...
rclcpp::Subscription< moveit_msgs::msg::PlanningSceneWorld >::SharedPtr planning_scene_world_subscriber_
const robot_model_loader::RobotModelLoaderPtr & getRobotModelLoader() const
Get the user kinematic model loader.
std::map< ShapeHandle, Eigen::Isometry3d, std::less< ShapeHandle >, Eigen::aligned_allocator< std::pair< const ShapeHandle, Eigen::Isometry3d > > > ShapeTransformCache
scene
Definition: pick.py:52
MOVEIT_CLASS_FORWARD(PlanningSceneMonitor)
name
Definition: setup.py:7
SingleUnlock(PlanningSceneMonitor *planning_scene_monitor, bool read_only)