[PSM] Add proccess Collision Object to PSM and request planning scene…

… to moveit py to allow syncing of mutliple PSM (#2536)

* PlanningSceneMonitor and request planning scene to moveit py to allow syncing of multiple planning scene monitors

* pre-commit fixes

* Update moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp

First catch empty scene to not have a unneeded indents.

Co-authored-by: Sebastian Jahr <[email protected]>

* Removed unneeded callback functions

---------

Co-authored-by: Sebastian Jahr <[email protected]>

moveit_py/moveit/planning.pyi

@@ -60,6 +60,9 @@ class PlanningSceneMonitor:
     def stop_state_monitor(self, *args, **kwargs) -> Any: ...
     def update_frame_transforms(self, *args, **kwargs) -> Any: ...
     def wait_for_current_robot_state(self, *args, **kwargs) -> Any: ...
+    def request_planning_scene_state(self, *args, **kwargs) -> Any: ...
+    def process_collision_object(self, *args, **kwargs) -> Any: ...
+    def process_attached_collision_object(self, *args, **kwargs) -> Any: ...
     @property
     def name(self) -> Any: ...
 

moveit_py/src/moveit/moveit_ros/planning_scene_monitor/planning_scene_monitor.cpp

@@ -42,6 +42,22 @@ namespace bind_planning_scene_monitor
 {
 static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_py.bind_planning_scene_monitor");
 
+bool processCollisionObject(const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor,
+                            moveit_msgs::msg::CollisionObject& collision_object_msg)
+{
+  moveit_msgs::msg::CollisionObject::ConstSharedPtr const_ptr =
+      std::make_shared<const moveit_msgs::msg::CollisionObject>(collision_object_msg);
+  return planning_scene_monitor->processCollisionObjectMsg(const_ptr);
+}
+
+bool processAttachedCollisionObjectMsg(const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor,
+                                       moveit_msgs::msg::AttachedCollisionObject& attached_collision_object_msg)
+{
+  moveit_msgs::msg::AttachedCollisionObject::ConstSharedPtr const_ptr =
+      std::make_shared<const moveit_msgs::msg::AttachedCollisionObject>(attached_collision_object_msg);
+  return planning_scene_monitor->processAttachedCollisionObjectMsg(const_ptr);
+}
+
 LockedPlanningSceneContextManagerRO
 readOnly(const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor)
 {
@@ -119,6 +135,14 @@ void initPlanningSceneMonitor(py::module& m)
            R"(
 	       Stops the state monitor.
 	   )")
+      .def("request_planning_scene_state", &planning_scene_monitor::PlanningSceneMonitor::requestPlanningSceneState,
+           py::arg("service_name"),
+           R"(
+	       Request the planning scene.
+
+            Args:
+               service_name (str): The name of the service to call.
+	   )")
 
       .def("wait_for_current_robot_state", &planning_scene_monitor::PlanningSceneMonitor::waitForCurrentRobotState,
            R"(
@@ -129,6 +153,23 @@ void initPlanningSceneMonitor(py::module& m)
            R"(
            Clears the octomap.
            )")
+      .def("process_collision_object", &moveit_py::bind_planning_scene_monitor::processCollisionObject,
+           py::arg("collision_object_msg"),  // py::arg("color_msg") = nullptr,
+           R"(
+           Apply a collision object to the planning scene.
+
+	      Args:
+               collision_object_msg (moveit_msgs.msg.CollisionObject): The collision object to apply to the planning scene.
+           )")
+      .def("process_attached_collision_object",
+           &moveit_py::bind_planning_scene_monitor::processAttachedCollisionObjectMsg,
+           py::arg("attached_collision_object_msg"),
+           R"(
+           Apply an attached collision object msg to the planning scene.
+
+	      Args:
+               attached_collision_object_msg (moveit_msgs.msg.AttachedCollisionObject): The attached collision object to apply to the planning scene.
+           )")
 
       .def("read_only", &moveit_py::bind_planning_scene_monitor::readOnly,
            R"(

moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/planning_scene_monitor.h

@@ -392,6 +392,13 @@ class MOVEIT_PLANNING_SCENE_MONITOR_EXPORT PlanningSceneMonitor
   // Called to update the planning scene with a new message.
   bool newPlanningSceneMessage(const moveit_msgs::msg::PlanningScene& scene);
 
+  // Called to update a collision object in the planning scene.
+  bool processCollisionObjectMsg(const moveit_msgs::msg::CollisionObject::ConstSharedPtr& collision_object_msg);
+
+  // Called to update an attached collision object in the planning scene.
+  bool processAttachedCollisionObjectMsg(
+      const moveit_msgs::msg::AttachedCollisionObject::ConstSharedPtr& attached_collision_object_msg);
+
 protected:
   /** @brief Initialize the planning scene monitor
    *  @param scene The scene instance to fill with data (an instance is allocated if the one passed in is not allocated)
@@ -418,18 +425,12 @@ class MOVEIT_PLANNING_SCENE_MONITOR_EXPORT PlanningSceneMonitor
   /** @brief Configure the default padding*/
   void configureDefaultPadding();
 
-  /** @brief Callback for a new collision object msg*/
-  void collisionObjectCallback(const moveit_msgs::msg::CollisionObject::ConstSharedPtr& obj);
-
   /** @brief Callback for a new planning scene world*/
   void newPlanningSceneWorldCallback(const moveit_msgs::msg::PlanningSceneWorld::ConstSharedPtr& world);
 
   /** @brief Callback for octomap updates */
   void octomapUpdateCallback();
 
-  /** @brief Callback for a new attached object msg*/
-  void attachObjectCallback(const moveit_msgs::msg::AttachedCollisionObject::ConstSharedPtr& obj);
-
   /** @brief Callback for a change for an attached object of the current state of the planning scene */
   void currentStateAttachedBodyUpdateCallback(moveit::core::AttachedBody* attached_body, bool just_attached);
 

moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp

@@ -780,57 +780,65 @@ bool PlanningSceneMonitor::newPlanningSceneMessage(const moveit_msgs::msg::Plann
   return result;
 }
 
-void PlanningSceneMonitor::newPlanningSceneWorldCallback(
-    const moveit_msgs::msg::PlanningSceneWorld::ConstSharedPtr& world)
+bool PlanningSceneMonitor::processCollisionObjectMsg(const moveit_msgs::msg::CollisionObject::ConstSharedPtr& object)
 {
-  if (scene_)
+  if (!scene_)
+    return false;
+
+  updateFrameTransforms();
   {
-    updateFrameTransforms();
-    {
-      std::unique_lock<std::shared_mutex> ulock(scene_update_mutex_);
-      last_update_time_ = rclcpp::Clock().now();
-      scene_->getWorldNonConst()->clearObjects();
-      scene_->processPlanningSceneWorldMsg(*world);
-      if (octomap_monitor_)
-      {
-        if (world->octomap.octomap.data.empty())
-        {
-          octomap_monitor_->getOcTreePtr()->lockWrite();
-          octomap_monitor_->getOcTreePtr()->clear();
-          octomap_monitor_->getOcTreePtr()->unlockWrite();
-        }
-      }
-    }
-    triggerSceneUpdateEvent(UPDATE_SCENE);
+    std::unique_lock<std::shared_mutex> ulock(scene_update_mutex_);
+    last_update_time_ = rclcpp::Clock().now();
+    if (!scene_->processCollisionObjectMsg(*object))
+      return false;
   }
+  triggerSceneUpdateEvent(UPDATE_GEOMETRY);
+  RCLCPP_INFO(logger_, "Published update collision object");
+  return true;
 }
 
-void PlanningSceneMonitor::collisionObjectCallback(const moveit_msgs::msg::CollisionObject::ConstSharedPtr& obj)
+bool PlanningSceneMonitor::processAttachedCollisionObjectMsg(
+    const moveit_msgs::msg::AttachedCollisionObject::ConstSharedPtr& object)
 {
   if (!scene_)
-    return;
+  {
+    return false;
+  }
 
   updateFrameTransforms();
   {
     std::unique_lock<std::shared_mutex> ulock(scene_update_mutex_);
     last_update_time_ = rclcpp::Clock().now();
-    if (!scene_->processCollisionObjectMsg(*obj))
-      return;
+    if (!scene_->processAttachedCollisionObjectMsg(*object))
+      return false;
   }
   triggerSceneUpdateEvent(UPDATE_GEOMETRY);
+  RCLCPP_INFO(logger_, "Published update attached");
+  return true;
 }
 
-void PlanningSceneMonitor::attachObjectCallback(const moveit_msgs::msg::AttachedCollisionObject::ConstSharedPtr& obj)
+void PlanningSceneMonitor::newPlanningSceneWorldCallback(
+    const moveit_msgs::msg::PlanningSceneWorld::ConstSharedPtr& world)
 {
   if (scene_)
   {
     updateFrameTransforms();
     {
       std::unique_lock<std::shared_mutex> ulock(scene_update_mutex_);
       last_update_time_ = rclcpp::Clock().now();
-      scene_->processAttachedCollisionObjectMsg(*obj);
+      scene_->getWorldNonConst()->clearObjects();
+      scene_->processPlanningSceneWorldMsg(*world);
+      if (octomap_monitor_)
+      {
+        if (world->octomap.octomap.data.empty())
+        {
+          octomap_monitor_->getOcTreePtr()->lockWrite();
+          octomap_monitor_->getOcTreePtr()->clear();
+          octomap_monitor_->getOcTreePtr()->unlockWrite();
+        }
+      }
     }
-    triggerSceneUpdateEvent(UPDATE_GEOMETRY);
+    triggerSceneUpdateEvent(UPDATE_SCENE);
   }
 }
 
@@ -1256,7 +1264,7 @@ void PlanningSceneMonitor::startWorldGeometryMonitor(const std::string& collisio
   {
     collision_object_subscriber_ = pnode_->create_subscription<moveit_msgs::msg::CollisionObject>(
         collision_objects_topic, rclcpp::SystemDefaultsQoS(),
-        [this](const moveit_msgs::msg::CollisionObject::ConstSharedPtr& obj) { return collisionObjectCallback(obj); });
+        [this](const moveit_msgs::msg::CollisionObject::ConstSharedPtr& obj) { processCollisionObjectMsg(obj); });
     RCLCPP_INFO(logger_, "Listening to '%s'", collision_objects_topic.c_str());
   }
 
@@ -1339,7 +1347,7 @@ void PlanningSceneMonitor::startStateMonitor(const std::string& joint_states_top
       attached_collision_object_subscriber_ = pnode_->create_subscription<moveit_msgs::msg::AttachedCollisionObject>(
           attached_objects_topic, rclcpp::SystemDefaultsQoS(),
           [this](const moveit_msgs::msg::AttachedCollisionObject::ConstSharedPtr& obj) {
-            return attachObjectCallback(obj);
+            processAttachedCollisionObjectMsg(obj);
           });
       RCLCPP_INFO(logger_, "Listening to '%s' for attached collision objects",
                   attached_collision_object_subscriber_->get_topic_name());