diff --git a/src/imetro_behavior/imetro_behavior/moveit_behaviors.py b/src/imetro_behavior/imetro_behavior/moveit_behaviors.py index a885933..b13ef3d 100644 --- a/src/imetro_behavior/imetro_behavior/moveit_behaviors.py +++ b/src/imetro_behavior/imetro_behavior/moveit_behaviors.py @@ -1034,3 +1034,41 @@ def parse_xml(self, robot_description) -> list[CollisionObject]: collision_objects.append(collision_object) return collision_objects + + +class AddCollisionBoxToPlanningScene(BehaviourWithPorts): + """ + Append to the MoveIt's planning scene a collision object of specified size, at specific pose. + """ + + INPUT_PORTS = { + "planning_scene": PortInformation(data_type=PlanningScene, required=True), + "collision_object_id": PortInformation(data_type=str, required=True, default_value=""), + "size": PortInformation(data_type=list[float], required=False, default_value=[1.0, 1.0, 1.0]), + "pose_stamped": PortInformation(data_type=PoseStamped, required=False, default_value=PoseStamped()), + } + + OUTPUT_PORTS = {"modified_planning_scene": PortInformation(data_type=PlanningScene)} + + def update(self) -> Status: + """Create the message and set it as an output port.""" + + pose_stamped = self.get_input("pose_stamped") + + collision_object = CollisionObject() + collision_object.id = self.get_input("collision_object_id") + collision_object.header.frame_id = pose_stamped.header.frame_id + + primitive = SolidPrimitive() + primitive.type = SolidPrimitive.BOX + primitive.dimensions = self.get_input("size") + + collision_object.primitives.append(primitive) + collision_object.primitive_poses.append(pose_stamped.pose) + + planning_scene = self.get_input("planning_scene") + planning_scene.world.collision_objects.append(collision_object) + planning_scene.is_diff = True + + self._set_output("modified_planning_scene", planning_scene) + return Status.SUCCESS diff --git a/src/imetro_behavior/tests/test_moveit_behaviors.py b/src/imetro_behavior/tests/test_moveit_behaviors.py index 73efb79..ee0d005 100644 --- a/src/imetro_behavior/tests/test_moveit_behaviors.py +++ b/src/imetro_behavior/tests/test_moveit_behaviors.py @@ -39,6 +39,7 @@ from tf2_ros import Buffer from imetro_behavior.moveit_behaviors import ( + AddCollisionBoxToPlanningScene, ExecuteTrajectoryBehavior, ModifyCollisions, PlanArcPath, @@ -507,3 +508,66 @@ def test_planning_scene_from_robot_description_behavior( assert sphere_primitive.type == SolidPrimitive.SPHERE assert len(sphere_primitive.dimensions) == 1 assert sphere_primitive.dimensions[0] == pytest.approx(0.6, abs=1e-9) + + +@pytest.fixture() +def add_collision_box_to_planning_scene_behavior( + ros_node: Node, +) -> AddCollisionBoxToPlanningScene: + behavior = AddCollisionBoxToPlanningScene(name="add_collision_box_to_planning_scene_test") + behavior.setup_ports() + + planning_scene = PlanningScene() + collision_object_id = "random_object_name" + size = [0.1, 0.2, 0.3] + pose_stamped = PoseStamped() + pose_stamped.header.frame_id = "test_reference_frame" + + pose_stamped.pose.position.x = 0.4 + pose_stamped.pose.position.y = 0.5 + pose_stamped.pose.position.z = 0.6 + + pose_stamped.pose.orientation.x = 0.7 + pose_stamped.pose.orientation.y = 0.8 + pose_stamped.pose.orientation.z = 0.9 + pose_stamped.pose.orientation.w = 1.1 + + set_input(behavior, "planning_scene", planning_scene) + set_input(behavior, "collision_object_id", collision_object_id) + set_input(behavior, "size", size) + set_input(behavior, "pose_stamped", pose_stamped) + return behavior + + +def test_add_collision_box_to_planning_scene_behavior( + add_collision_box_to_planning_scene_behavior: AddCollisionBoxToPlanningScene, +) -> None: + + assert add_collision_box_to_planning_scene_behavior.update() == Status.SUCCESS + + modified_planning_scene = add_collision_box_to_planning_scene_behavior.get_last_output("modified_planning_scene") + + assert isinstance(modified_planning_scene, PlanningScene) + assert len(modified_planning_scene.world.collision_objects) == 1 + + box_collision_object = modified_planning_scene.world.collision_objects[0] + assert len(box_collision_object.primitives) == 1 + box_primitive = box_collision_object.primitives[0] + assert box_primitive.type == SolidPrimitive.BOX + assert len(box_primitive.dimensions) == 3 + assert box_primitive.dimensions[0] == pytest.approx(0.1, abs=1e-9) + assert box_primitive.dimensions[1] == pytest.approx(0.2, abs=1e-9) + assert box_primitive.dimensions[2] == pytest.approx(0.3, abs=1e-9) + + box_pose = box_collision_object.primitive_poses[0] + assert box_pose.position.x == pytest.approx(0.4, abs=1e-9) + assert box_pose.position.y == pytest.approx(0.5, abs=1e-9) + assert box_pose.position.z == pytest.approx(0.6, abs=1e-9) + + assert box_pose.orientation.x == pytest.approx(0.7, abs=1e-9) + assert box_pose.orientation.y == pytest.approx(0.8, abs=1e-9) + assert box_pose.orientation.z == pytest.approx(0.9, abs=1e-9) + assert box_pose.orientation.w == pytest.approx(1.1, abs=1e-9) + + assert box_collision_object.header.frame_id == "test_reference_frame" + assert box_collision_object.id == "random_object_name"