diff --git a/rcdt_utilities/nodes/moveit_controller_node.py b/rcdt_utilities/nodes/moveit_controller_node.py index 8535414..9e9fe59 100755 --- a/rcdt_utilities/nodes/moveit_controller_node.py +++ b/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) diff --git a/rcdt_utilities_msgs/CMakeLists.txt b/rcdt_utilities_msgs/CMakeLists.txt index 6814f1a..7fe0eb3 100644 --- a/rcdt_utilities_msgs/CMakeLists.txt +++ b/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) diff --git a/rcdt_utilities_msgs/srv/AddObject.srv b/rcdt_utilities_msgs/srv/AddObject.srv new file mode 100644 index 0000000..849b887 --- /dev/null +++ b/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 \ No newline at end of file