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();
301 void updateSceneWithCurrentState(
bool skip_update_if_locked =
false);
308 void setStateUpdateFrequency(
double hz);
313 if (dt_state_update_.count() > 0.0)
315 return 1.0 / dt_state_update_.count();
326 void startSceneMonitor(
const std::string& scene_topic = DEFAULT_PLANNING_SCENE_TOPIC);
335 bool requestPlanningSceneState(
const std::string& service_name = DEFAULT_PLANNING_SCENE_SERVICE);
344 void providePlanningSceneService(
const std::string& service_name = DEFAULT_PLANNING_SCENE_SERVICE);
347 void stopSceneMonitor();
357 void startWorldGeometryMonitor(
const std::string& collision_objects_topic = DEFAULT_COLLISION_OBJECT_TOPIC,
358 const std::string& planning_scene_world_topic = DEFAULT_PLANNING_SCENE_WORLD_TOPIC,
359 const bool load_octomap_monitor =
true);
362 void stopWorldGeometryMonitor();
365 void addUpdateCallback(
const std::function<
void(SceneUpdateType)>& fn);
368 void clearUpdateCallbacks();
371 void getMonitoredTopics(std::vector<std::string>& topics)
const;
376 return last_update_time_;
379 void publishDebugInformation(
bool flag);
382 void triggerSceneUpdateEvent(SceneUpdateType update_type);
389 bool waitForCurrentRobotState(
const rclcpp::Time& t,
double wait_time = 1.);
394 bool newPlanningSceneMessage(
const moveit_msgs::msg::PlanningScene& scene);
397 bool processCollisionObjectMsg(
const moveit_msgs::msg::CollisionObject::ConstSharedPtr& collision_object_msg,
398 const std::optional<moveit_msgs::msg::ObjectColor>& color_msg = std::nullopt);
401 bool processAttachedCollisionObjectMsg(
402 const moveit_msgs::msg::AttachedCollisionObject::ConstSharedPtr& attached_collision_object_msg);
408 void initialize(
const planning_scene::PlanningScenePtr& scene);
411 void lockSceneRead();
414 void unlockSceneRead();
418 void lockSceneWrite();
422 void unlockSceneWrite();
425 void configureCollisionMatrix(
const planning_scene::PlanningScenePtr& scene);
428 void configureDefaultPadding();
431 void newPlanningSceneWorldCallback(
const moveit_msgs::msg::PlanningSceneWorld::ConstSharedPtr& world);
434 void octomapUpdateCallback();
440 void currentWorldObjectUpdateCallback(
const collision_detection::World::ObjectConstPtr&
object,
443 void includeRobotLinksInOctree();
444 void excludeRobotLinksFromOctree();
446 void excludeWorldObjectsFromOctree();
447 void includeWorldObjectsInOctree();
448 void excludeWorldObjectFromOctree(
const collision_detection::World::ObjectConstPtr& obj);
449 void includeWorldObjectInOctree(
const collision_detection::World::ObjectConstPtr& obj);
451 void excludeAttachedBodiesFromOctree();
452 void includeAttachedBodiesInOctree();
456 bool getShapeTransformCache(
const std::string& target_frame,
const rclcpp::Time& target_time,
469 std::shared_ptr<rclcpp::Node>
node_;
522 std::vector<std::pair<occupancy_map_monitor::ShapeHandle, std::size_t> > >
525 std::vector<std::pair<occupancy_map_monitor::ShapeHandle, std::size_t> > >;
527 std::map<std::string, std::vector<std::pair<occupancy_map_monitor::ShapeHandle, const Eigen::Isometry3d*> > >;
540 void getUpdatedFrameTransforms(std::vector<geometry_msgs::msg::TransformStamped>& transforms);
543 void scenePublishingThread();
546 void onStateUpdate(
const sensor_msgs::msg::JointState::ConstSharedPtr& joint_state);
549 void stateUpdateTimerCallback();
552 void newPlanningSceneCallback(
const moveit_msgs::msg::PlanningScene::ConstSharedPtr& scene);
555 void getPlanningSceneServiceCallback(
const moveit_msgs::srv::GetPlanningScene::Request::SharedPtr& req,
556 const moveit_msgs::srv::GetPlanningScene::Response::SharedPtr& res);
558 void updatePublishSettings(
bool publish_geom_updates,
bool publish_state_updates,
bool publish_transform_updates,
559 bool publish_planning_scene,
double publish_planning_scene_hz);
562 std::atomic<bool> state_update_pending_;
565 std::mutex state_update_mutex_;
570 std::chrono::system_clock::time_point last_robot_state_update_wall_time_;
574 std::chrono::duration<double> dt_state_update_;
579 rclcpp::Duration shape_transform_cache_lookup_wait_time_;
584 rclcpp::TimerBase::SharedPtr state_update_timer_;
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_;