Feat/add-moveit-collision-object-services

rcdt_utilities/nodes/moveit_controller_node.py

@@ -11,14 +11,18 @@
 from rclpy.action import ActionServer
 from rclpy.action.server import ServerGoalHandle
 
-from moveit.planning import MoveItPy, PlanningComponent
+from moveit.planning import MoveItPy, PlanningComponent, PlanningSceneMonitor
 from moveit.core.robot_state import RobotState
 from moveit.core.robot_model import RobotModel, JointModelGroup
 from moveit.core.planning_interface import MotionPlanResponse
 from moveit.core.controller_manager import ExecutionStatus
-from rcdt_utilities_msgs.action import Moveit
+from moveit.core.planning_scene import PlanningScene
 
 from geometry_msgs.msg import PoseStamped
+from moveit_msgs.msg import CollisionObject
+from rcdt_utilities_msgs.action import Moveit
+from rcdt_utilities_msgs.srv import AddObject
+from std_srvs.srv import Trigger
 
 
 class MoveitControllerNode(Node):
@@ -32,8 +36,11 @@ def __init__(self, group: str) -> None:
         if not self.init_links(group):
             return
         self.planner: PlanningComponent = self.robot.get_planning_component(group)
+        self.monitor: PlanningSceneMonitor = self.robot.get_planning_scene_monitor()
 
         self.server = ActionServer(self, Moveit, name, self.callback)
+        self.create_service(AddObject, "~/add_object", self.add_object)
+        self.create_service(Trigger, "~/clear_objects", self.clear_objects)
 
     def init_links(self, group: str) -> bool:
         robot_model: RobotModel = self.robot.get_robot_model()
@@ -97,6 +104,34 @@ def move_to_pose(self, pose: PoseStamped) -> bool:
         self.planner.set_goal_state(pose_stamped_msg=pose, pose_link=self.ee_link)
         return self.plan_and_execute()
 
+    def add_object(
+        self, request: AddObject.Request, response: AddObject.Response
+    ) -> None:
+        with self.monitor.read_write() as scene:
+            scene: PlanningScene
+
+            collision_object = request.object
+            collision_object.operation = CollisionObject.ADD
+            collision_object.header.frame_id = "fr3_link0"
+
+            scene.apply_collision_object(collision_object)
+            current_state: RobotState = scene.current_state
+            current_state.update()
+            response.success = True
+            return response
+
+    def clear_objects(
+        self, _request: Trigger.Request, response: Trigger.Response
+    ) -> None:
+        with self.monitor.read_write() as scene:
+            scene: PlanningScene
+
+            scene.remove_all_collision_objects()
+            current_state: RobotState = scene.current_state
+            current_state.update()
+            response.success = True
+            return response
+
 
 def main(args: str = None) -> None:
     rclpy.init(args=args)

rcdt_utilities_msgs/CMakeLists.txt

@@ -11,11 +11,13 @@ find_package(ament_cmake_python REQUIRED)
 find_package(rosidl_default_generators REQUIRED)
 find_package(std_msgs REQUIRED)
 find_package(geometry_msgs REQUIRED)
+find_package(moveit_msgs REQUIRED)
 
 rosidl_generate_interfaces(${PROJECT_NAME}
   "srv/AddMarker.srv"
+  "srv/AddObject.srv"
   "action/Moveit.action"
-  DEPENDENCIES std_msgs geometry_msgs
+  DEPENDENCIES std_msgs geometry_msgs moveit_msgs
 )
 
 if(BUILD_TESTING)

rcdt_utilities_msgs/srv/AddObject.srv

@@ -0,0 +1,7 @@
+# SPDX-FileCopyrightText: Alliander N. V.
+#
+# SPDX-License-Identifier: Apache-2.0
+
+moveit_msgs/CollisionObject object
+---
+bool success