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>
51 #include <shared_mutex>
55 #include <moveit_planning_scene_monitor_export.h>
86 UPDATE_TRANSFORMS = 2,
93 UPDATE_SCENE = 8 + UPDATE_STATE + UPDATE_TRANSFORMS + UPDATE_GEOMETRY
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>& ,
const std::string&
name =
"")
131 const std::string&
name =
"");
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>& ,
const std::string&
name =
"")
144 PlanningSceneMonitor(
const rclcpp::Node::SharedPtr& node,
const robot_model_loader::RobotModelLoaderPtr& rml,
145 const std::string&
name =
"");
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>& ,
156 const std::string&
name =
"")
161 const std::string& robot_description,
const std::string&
name =
"");
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>& ,
172 const std::string&
name =
"")
177 const robot_model_loader::RobotModelLoaderPtr& rml,
const std::string&
name =
"");
184 return monitor_name_;
229 bool updatesScene(
const planning_scene::PlanningSceneConstPtr&
scene)
const;
235 bool updatesScene(
const planning_scene::PlanningScenePtr&
scene)
const;
241 return robot_description_;
247 return default_robot_padd_;
253 return default_robot_scale_;
259 return default_object_padd_;
265 return default_attached_padd_;
279 void monitorDiffs(
bool flag);
284 void startPublishingPlanningScene(SceneUpdateType event,
285 const std::string& planning_scene_topic = MONITORED_PLANNING_SCENE_TOPIC);
288 void stopPublishingPlanningScene();
291 void setPlanningScenePublishingFrequency(
double hz);
296 return publish_planning_scene_frequency_;
303 return current_state_monitor_;
308 return current_state_monitor_;
316 void updateFrameTransforms();
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);
325 void stopStateMonitor();
331 void updateSceneWithCurrentState(
bool skip_update_if_locked =
false);
338 void setStateUpdateFrequency(
double hz);
343 if (dt_state_update_.count() > 0.0)
344 return 1.0 / dt_state_update_.count();
352 void startSceneMonitor(
const std::string& scene_topic = DEFAULT_PLANNING_SCENE_TOPIC);
361 bool requestPlanningSceneState(
const std::string& service_name = DEFAULT_PLANNING_SCENE_SERVICE);
370 void providePlanningSceneService(
const std::string& service_name = DEFAULT_PLANNING_SCENE_SERVICE);
373 void stopSceneMonitor();
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);
388 void stopWorldGeometryMonitor();
391 void addUpdateCallback(
const std::function<
void(SceneUpdateType)>& fn);
394 void clearUpdateCallbacks();
397 void getMonitoredTopics(std::vector<std::string>& topics)
const;
402 return last_update_time_;
405 void publishDebugInformation(
bool flag);
408 void triggerSceneUpdateEvent(SceneUpdateType update_type);
415 bool waitForCurrentRobotState(
const rclcpp::Time& t,
double wait_time = 1.);
420 bool newPlanningSceneMessage(
const moveit_msgs::msg::PlanningScene&
scene);
426 void initialize(
const planning_scene::PlanningScenePtr&
scene);
429 void lockSceneRead();
432 void unlockSceneRead();
436 void lockSceneWrite();
440 void unlockSceneWrite();
443 void configureCollisionMatrix(
const planning_scene::PlanningScenePtr&
scene);
446 void configureDefaultPadding();
449 void collisionObjectCallback(
const moveit_msgs::msg::CollisionObject::ConstSharedPtr& obj);
452 void newPlanningSceneWorldCallback(
const moveit_msgs::msg::PlanningSceneWorld::ConstSharedPtr& world);
455 void octomapUpdateCallback();
458 void attachObjectCallback(
const moveit_msgs::msg::AttachedCollisionObject::ConstSharedPtr& obj);
464 void currentWorldObjectUpdateCallback(
const collision_detection::World::ObjectConstPtr&
object,
467 void includeRobotLinksInOctree();
468 void excludeRobotLinksFromOctree();
470 void excludeWorldObjectsFromOctree();
471 void includeWorldObjectsInOctree();
472 void excludeWorldObjectFromOctree(
const collision_detection::World::ObjectConstPtr& obj);
473 void includeWorldObjectInOctree(
const collision_detection::World::ObjectConstPtr& obj);
475 void excludeAttachedBodiesFromOctree();
476 void includeAttachedBodiesInOctree();
480 bool getShapeTransformCache(
const std::string& target_frame,
const rclcpp::Time& target_time,
493 std::shared_ptr<rclcpp::Node>
node_;
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*> > >;
564 void getUpdatedFrameTransforms(std::vector<geometry_msgs::msg::TransformStamped>& transforms);
567 void scenePublishingThread();
570 void onStateUpdate(
const sensor_msgs::msg::JointState::ConstSharedPtr& joint_state);
573 void stateUpdateTimerCallback();
576 void newPlanningSceneCallback(
const moveit_msgs::msg::PlanningScene::ConstSharedPtr&
scene);
579 void getPlanningSceneServiceCallback(
const moveit_msgs::srv::GetPlanningScene::Request::SharedPtr& req,
580 const moveit_msgs::srv::GetPlanningScene::Response::SharedPtr& res);
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);
586 std::atomic<bool> state_update_pending_;
589 std::mutex state_update_mutex_;
594 std::chrono::system_clock::time_point last_robot_state_update_wall_time_;
598 std::chrono::duration<double> dt_state_update_;
603 rclcpp::Duration shape_transform_cache_lookup_wait_time_;
608 rclcpp::TimerBase::SharedPtr state_update_timer_;
610 robot_model_loader::RobotModelLoaderPtr rm_loader_;
611 moveit::core::RobotModelConstPtr robot_model_;
615 rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr callback_handler_;
658 operator bool()
const
663 operator const planning_scene::PlanningSceneConstPtr&()
const
668 const planning_scene::PlanningSceneConstPtr&
operator->()
const
744 operator const planning_scene::PlanningScenePtr&()
Represents an action that occurred on an object in the world. Several bits may be set indicating seve...
Object defining bodies that can be attached to robot links.
A link from the robot. Contains the constant transform applied to the link and its geometry.
This is a convenience class for obtaining access to an instance of a locked PlanningScene.
const planning_scene::PlanningSceneConstPtr & operator->() const
PlanningSceneMonitorPtr planning_scene_monitor_
void initialize(bool read_only)
LockedPlanningSceneRO(const PlanningSceneMonitorPtr &planning_scene_monitor, bool read_only)
LockedPlanningSceneRO(const PlanningSceneMonitorPtr &planning_scene_monitor)
const PlanningSceneMonitorPtr & getPlanningSceneMonitor()
MOVEIT_STRUCT_FORWARD(SingleUnlock)
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.
planning_scene::PlanningScenePtr scene_
double default_attached_padd_
default attached padding
double default_robot_scale_
default robot scaling
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
AttachedBodyShapeHandles attached_body_shape_handles_
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.
LinkShapeHandles link_shape_handles_
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.
SceneUpdateType publish_update_types_
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
double default_object_padd_
default object padding
std::shared_ptr< rclcpp::Node > pnode_
const std::string & getName() const
Get the name of this monitor.
rclcpp::Service< moveit_msgs::srv::GetPlanningScene >::SharedPtr get_scene_service_
static const std::string DEFAULT_PLANNING_SCENE_WORLD_TOPIC
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.
CurrentStateMonitorPtr & getStateMonitorNonConst()
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
double publish_planning_scene_frequency_
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::atomic< SceneUpdateType > new_scene_update_
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::string robot_description_
std::map< const moveit::core::AttachedBody *, std::vector< std::pair< occupancy_map_monitor::ShapeHandle, std::size_t > > > AttachedBodyShapeHandles
std::recursive_mutex shape_handles_lock_
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.
std::thread private_executor_thread_
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)
double default_robot_padd_
default robot padding
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.
CurrentStateMonitorPtr current_state_monitor_
std::recursive_mutex update_lock_
lock access to update_callbacks_
CollisionBodyShapeHandles collision_body_shape_handles_
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.
static const std::string MONITORED_PLANNING_SCENE_TOPIC
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::condition_variable_any new_scene_update_condition_
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
MOVEIT_CLASS_FORWARD(PlanningSceneMonitor)
PlanningSceneMonitor * planning_scene_monitor_
SingleUnlock(PlanningSceneMonitor *planning_scene_monitor, bool read_only)