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 const std::string&
name =
"");
130 PlanningSceneMonitor(
const rclcpp::Node::SharedPtr& node,
const robot_model_loader::RobotModelLoaderPtr& rml,
131 const std::string&
name =
"");
138 PlanningSceneMonitor(
const rclcpp::Node::SharedPtr& node,
const planning_scene::PlanningScenePtr& scene,
139 const std::string& robot_description,
const std::string&
name =
"");
146 PlanningSceneMonitor(
const rclcpp::Node::SharedPtr& node,
const planning_scene::PlanningScenePtr& scene,
147 const robot_model_loader::RobotModelLoaderPtr& rml,
const std::string&
name =
"");
154 return monitor_name_;
199 bool updatesScene(
const planning_scene::PlanningSceneConstPtr& scene)
const;
205 bool updatesScene(
const planning_scene::PlanningScenePtr& scene)
const;
211 return robot_description_;
217 return default_robot_padd_;
223 return default_robot_scale_;
229 return default_object_padd_;
235 return default_attached_padd_;
249 void monitorDiffs(
bool flag);
254 void startPublishingPlanningScene(SceneUpdateType event,
255 const std::string& planning_scene_topic = MONITORED_PLANNING_SCENE_TOPIC);
258 void stopPublishingPlanningScene();
261 void setPlanningScenePublishingFrequency(
double hz);
266 return publish_planning_scene_frequency_;
273 return current_state_monitor_;
278 return current_state_monitor_;
286 void updateFrameTransforms();
291 void startStateMonitor(
const std::string& joint_states_topic = DEFAULT_JOINT_STATES_TOPIC,
292 const std::string& attached_objects_topic = DEFAULT_ATTACHED_COLLISION_OBJECT_TOPIC);
295 void stopStateMonitor();
300 void updateSceneWithCurrentState();
307 void setStateUpdateFrequency(
double hz);
312 if (dt_state_update_.count() > 0.0)
314 return 1.0 / dt_state_update_.count();
325 void startSceneMonitor(
const std::string& scene_topic = DEFAULT_PLANNING_SCENE_TOPIC);
334 bool requestPlanningSceneState(
const std::string& service_name = DEFAULT_PLANNING_SCENE_SERVICE);
343 void providePlanningSceneService(
const std::string& service_name = DEFAULT_PLANNING_SCENE_SERVICE);
346 void stopSceneMonitor();
356 void startWorldGeometryMonitor(
const std::string& collision_objects_topic = DEFAULT_COLLISION_OBJECT_TOPIC,
357 const std::string& planning_scene_world_topic = DEFAULT_PLANNING_SCENE_WORLD_TOPIC,
358 const bool load_octomap_monitor =
true);
361 void stopWorldGeometryMonitor();
364 void addUpdateCallback(
const std::function<
void(SceneUpdateType)>& fn);
367 void clearUpdateCallbacks();
370 void getMonitoredTopics(std::vector<std::string>& topics)
const;
375 return last_update_time_;
378 void publishDebugInformation(
bool flag);
381 void triggerSceneUpdateEvent(SceneUpdateType update_type);
388 bool waitForCurrentRobotState(
const rclcpp::Time& t,
double wait_time = 1.);
393 bool newPlanningSceneMessage(
const moveit_msgs::msg::PlanningScene& scene);
396 bool processCollisionObjectMsg(
const moveit_msgs::msg::CollisionObject::ConstSharedPtr& collision_object_msg,
397 const std::optional<moveit_msgs::msg::ObjectColor>& color_msg = std::nullopt);
401 const moveit_msgs::msg::AttachedCollisionObject::ConstSharedPtr& attached_collision_object_msg);
407 void initialize(
const planning_scene::PlanningScenePtr& scene);
410 void lockSceneRead();
413 void unlockSceneRead();
417 void lockSceneWrite();
421 void unlockSceneWrite();
424 void configureCollisionMatrix(
const planning_scene::PlanningScenePtr& scene);
427 void configureDefaultPadding();
430 void newPlanningSceneWorldCallback(
const moveit_msgs::msg::PlanningSceneWorld::ConstSharedPtr& world);
433 void octomapUpdateCallback();
439 void currentWorldObjectUpdateCallback(
const collision_detection::World::ObjectConstPtr&
object,
442 void includeRobotLinksInOctree();
443 void excludeRobotLinksFromOctree();
445 void excludeWorldObjectsFromOctree();
446 void includeWorldObjectsInOctree();
447 void excludeWorldObjectFromOctree(
const collision_detection::World::ObjectConstPtr& obj);
448 void includeWorldObjectInOctree(
const collision_detection::World::ObjectConstPtr& obj);
450 void excludeAttachedBodiesFromOctree();
451 void includeAttachedBodiesInOctree();
455 bool getShapeTransformCache(
const std::string& target_frame,
const rclcpp::Time& target_time,
468 std::shared_ptr<rclcpp::Node>
node_;
521 std::vector<std::pair<occupancy_map_monitor::ShapeHandle, std::size_t> > >
524 std::vector<std::pair<occupancy_map_monitor::ShapeHandle, std::size_t> > >;
526 std::map<std::string, std::vector<std::pair<occupancy_map_monitor::ShapeHandle, const Eigen::Isometry3d*> > >;
539 void getUpdatedFrameTransforms(std::vector<geometry_msgs::msg::TransformStamped>& transforms);
542 void scenePublishingThread();
545 void onStateUpdate(
const sensor_msgs::msg::JointState::ConstSharedPtr& joint_state);
548 void stateUpdateTimerCallback();
551 void newPlanningSceneCallback(
const moveit_msgs::msg::PlanningScene::ConstSharedPtr& scene);
554 void getPlanningSceneServiceCallback(
const moveit_msgs::srv::GetPlanningScene::Request::SharedPtr& req,
555 const moveit_msgs::srv::GetPlanningScene::Response::SharedPtr& res);
557 void updatePublishSettings(
bool publish_geom_updates,
bool publish_state_updates,
bool publish_transform_updates,
558 bool publish_planning_scene,
double publish_planning_scene_hz);
561 std::mutex state_pending_mutex_;
565 volatile bool state_update_pending_;
569 std::chrono::duration<double> dt_state_update_;
574 rclcpp::Duration shape_transform_cache_lookup_wait_time_;
580 rclcpp::TimerBase::SharedPtr state_update_timer_;
584 std::chrono::system_clock::time_point last_robot_state_update_wall_time_;
586 robot_model_loader::RobotModelLoaderPtr rm_loader_;
587 moveit::core::RobotModelConstPtr robot_model_;
591 rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr callback_handler_;
598 rclcpp::Logger logger_;
636 operator bool()
const
641 operator const planning_scene::PlanningSceneConstPtr&()
const
646 const planning_scene::PlanningSceneConstPtr&
operator->()
const
730 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_
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_
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.
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
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.
bool processAttachedCollisionObjectMsg(const planning_scene_monitor::PlanningSceneMonitorPtr &planning_scene_monitor, moveit_msgs::msg::AttachedCollisionObject &attached_collision_object_msg)
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)