From 447dc17ab3013b3cc1ded2d35f409dddd3502436 Mon Sep 17 00:00:00 2001 From: Phoebe Bridgeback Date: Tue, 29 Sep 2026 17:47:12 -0500 Subject: [PATCH 1/6] added a CreatePose behavior --- .../imetro_behavior/geometry_behaviors.py | 25 +++++++++++++++++++ 1 file changed, 25 insertions(+) diff --git a/src/imetro_behavior/imetro_behavior/geometry_behaviors.py b/src/imetro_behavior/imetro_behavior/geometry_behaviors.py index 49b08a6..5c9aa8a 100644 --- a/src/imetro_behavior/imetro_behavior/geometry_behaviors.py +++ b/src/imetro_behavior/imetro_behavior/geometry_behaviors.py @@ -70,6 +70,31 @@ def update(self) -> Status: self._set_output("msg", msg) return Status.SUCCESS +class CreatePose(BehaviourWithPorts): + """Create a Pose ROS message.""" + + INPUT_PORTS = { + "position_xyz": PortInformation(data_type=list[float], required=True), + "orientation_xyzw": PortInformation(data_type=list[float], required=True), + } + + OUTPUT_PORTS = {"msg": PortInformation(data_type=Pose, required=True)} + + def update(self) -> Status: + """Create the message and set it as an output port.""" + msg = Pose() + position_xyz = self.get_input("position_xyz") + orientation_xyzw = self.get_input("orientation_xyzw") + msg.position.x = position_xyz[0] + msg.position.y = position_xyz[1] + msg.position.z = position_xyz[2] + msg.orientation.x = orientation_xyzw[0] + msg.orientation.y = orientation_xyzw[1] + msg.orientation.z = orientation_xyzw[2] + msg.orientation.w = orientation_xyzw[3] + self._set_output("msg", msg) + return Status.SUCCESS + class TransformPose(BehaviourWithPorts): """Transforms a PoseStamped ROS message to a specified frame.""" From d11baf53a03c36e121822be666a9465b57c5e6d7 Mon Sep 17 00:00:00 2001 From: Phoebe Bridgeback Date: Tue, 29 Sep 2026 17:47:33 -0500 Subject: [PATCH 2/6] added behavior for adding collision to the planning scene --- .../imetro_behavior/moveit_behaviors.py | 35 +++++++++++++++++++ 1 file changed, 35 insertions(+) diff --git a/src/imetro_behavior/imetro_behavior/moveit_behaviors.py b/src/imetro_behavior/imetro_behavior/moveit_behaviors.py index a885933..f920c24 100644 --- a/src/imetro_behavior/imetro_behavior/moveit_behaviors.py +++ b/src/imetro_behavior/imetro_behavior/moveit_behaviors.py @@ -1034,3 +1034,38 @@ 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=""), + "reference_frame": PortInformation(data_type=str, required=True), + "size": PortInformation(data_type=list[float], required=False, default_value=[1.0, 1.0, 1.0]), + "pose": PortInformation(data_type=Pose, required=False, default_value=Pose())} + + OUTPUT_PORTS = {"modified_planning_scene": PortInformation(data_type=PlanningScene)} + + def update(self) -> Status: + """Create the message and set it as an output port.""" + + collision_object = CollisionObject() + collision_object.id = self.get_input("collision_object_id") + collision_object.header.frame_id = self.get_input("reference_frame") + + primitive = SolidPrimitive() + primitive.type = SolidPrimitive.BOX + primitive.dimensions = self.get_input("size") + + collision_object.primitives.append(primitive) + collision_object.primitive_poses.append(self.get_input("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 \ No newline at end of file From 12bcdd615ebd4549679b1f39fbe922e52473b44e Mon Sep 17 00:00:00 2001 From: Misha Savchenko Date: Tue, 29 Sep 2026 17:48:29 -0500 Subject: [PATCH 3/6] pre-commit --- .../imetro_behavior/geometry_behaviors.py | 1 + .../imetro_behavior/moveit_behaviors.py | 20 ++++++++++--------- 2 files changed, 12 insertions(+), 9 deletions(-) diff --git a/src/imetro_behavior/imetro_behavior/geometry_behaviors.py b/src/imetro_behavior/imetro_behavior/geometry_behaviors.py index 5c9aa8a..afe3c25 100644 --- a/src/imetro_behavior/imetro_behavior/geometry_behaviors.py +++ b/src/imetro_behavior/imetro_behavior/geometry_behaviors.py @@ -70,6 +70,7 @@ def update(self) -> Status: self._set_output("msg", msg) return Status.SUCCESS + class CreatePose(BehaviourWithPorts): """Create a Pose ROS message.""" diff --git a/src/imetro_behavior/imetro_behavior/moveit_behaviors.py b/src/imetro_behavior/imetro_behavior/moveit_behaviors.py index f920c24..28c1bd5 100644 --- a/src/imetro_behavior/imetro_behavior/moveit_behaviors.py +++ b/src/imetro_behavior/imetro_behavior/moveit_behaviors.py @@ -1041,21 +1041,23 @@ 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=""), - "reference_frame": PortInformation(data_type=str, required=True), - "size": PortInformation(data_type=list[float], required=False, default_value=[1.0, 1.0, 1.0]), - "pose": PortInformation(data_type=Pose, required=False, default_value=Pose())} + INPUT_PORTS = { + "planning_scene": PortInformation(data_type=PlanningScene, required=True), + "collision_object_id": PortInformation(data_type=str, required=True, default_value=""), + "reference_frame": PortInformation(data_type=str, required=True), + "size": PortInformation(data_type=list[float], required=False, default_value=[1.0, 1.0, 1.0]), + "pose": PortInformation(data_type=Pose, required=False, default_value=Pose()), + } OUTPUT_PORTS = {"modified_planning_scene": PortInformation(data_type=PlanningScene)} def update(self) -> Status: """Create the message and set it as an output port.""" - + collision_object = CollisionObject() collision_object.id = self.get_input("collision_object_id") - collision_object.header.frame_id = self.get_input("reference_frame") - + collision_object.header.frame_id = self.get_input("reference_frame") + primitive = SolidPrimitive() primitive.type = SolidPrimitive.BOX primitive.dimensions = self.get_input("size") @@ -1068,4 +1070,4 @@ def update(self) -> Status: planning_scene.is_diff = True self._set_output("modified_planning_scene", planning_scene) - return Status.SUCCESS \ No newline at end of file + return Status.SUCCESS From e8b59665cce9d075124109d824e1adccc8af352b Mon Sep 17 00:00:00 2001 From: Phoebe Bridgeback Date: Wed, 30 Sep 2026 08:04:43 -0500 Subject: [PATCH 4/6] removed create pose behavior --- .../imetro_behavior/geometry_behaviors.py | 26 ------------------- 1 file changed, 26 deletions(-) diff --git a/src/imetro_behavior/imetro_behavior/geometry_behaviors.py b/src/imetro_behavior/imetro_behavior/geometry_behaviors.py index afe3c25..49b08a6 100644 --- a/src/imetro_behavior/imetro_behavior/geometry_behaviors.py +++ b/src/imetro_behavior/imetro_behavior/geometry_behaviors.py @@ -71,32 +71,6 @@ def update(self) -> Status: return Status.SUCCESS -class CreatePose(BehaviourWithPorts): - """Create a Pose ROS message.""" - - INPUT_PORTS = { - "position_xyz": PortInformation(data_type=list[float], required=True), - "orientation_xyzw": PortInformation(data_type=list[float], required=True), - } - - OUTPUT_PORTS = {"msg": PortInformation(data_type=Pose, required=True)} - - def update(self) -> Status: - """Create the message and set it as an output port.""" - msg = Pose() - position_xyz = self.get_input("position_xyz") - orientation_xyzw = self.get_input("orientation_xyzw") - msg.position.x = position_xyz[0] - msg.position.y = position_xyz[1] - msg.position.z = position_xyz[2] - msg.orientation.x = orientation_xyzw[0] - msg.orientation.y = orientation_xyzw[1] - msg.orientation.z = orientation_xyzw[2] - msg.orientation.w = orientation_xyzw[3] - self._set_output("msg", msg) - return Status.SUCCESS - - class TransformPose(BehaviourWithPorts): """Transforms a PoseStamped ROS message to a specified frame.""" From 1f7d85acf0b2e6b506eadbfd02d2678e4eb4c12d Mon Sep 17 00:00:00 2001 From: Phoebe Bridgeback Date: Wed, 30 Sep 2026 08:05:01 -0500 Subject: [PATCH 5/6] changed pose input to posestamped --- src/imetro_behavior/imetro_behavior/moveit_behaviors.py | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/src/imetro_behavior/imetro_behavior/moveit_behaviors.py b/src/imetro_behavior/imetro_behavior/moveit_behaviors.py index 28c1bd5..b13ef3d 100644 --- a/src/imetro_behavior/imetro_behavior/moveit_behaviors.py +++ b/src/imetro_behavior/imetro_behavior/moveit_behaviors.py @@ -1044,9 +1044,8 @@ class AddCollisionBoxToPlanningScene(BehaviourWithPorts): INPUT_PORTS = { "planning_scene": PortInformation(data_type=PlanningScene, required=True), "collision_object_id": PortInformation(data_type=str, required=True, default_value=""), - "reference_frame": PortInformation(data_type=str, required=True), "size": PortInformation(data_type=list[float], required=False, default_value=[1.0, 1.0, 1.0]), - "pose": PortInformation(data_type=Pose, required=False, default_value=Pose()), + "pose_stamped": PortInformation(data_type=PoseStamped, required=False, default_value=PoseStamped()), } OUTPUT_PORTS = {"modified_planning_scene": PortInformation(data_type=PlanningScene)} @@ -1054,16 +1053,18 @@ class AddCollisionBoxToPlanningScene(BehaviourWithPorts): 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 = self.get_input("reference_frame") + 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(self.get_input("pose")) + collision_object.primitive_poses.append(pose_stamped.pose) planning_scene = self.get_input("planning_scene") planning_scene.world.collision_objects.append(collision_object) From 0c2f66f1562a253c9e6e7ea751b3847a813e4523 Mon Sep 17 00:00:00 2001 From: Misha Savchenko Date: Wed, 30 Sep 2026 08:05:19 -0500 Subject: [PATCH 6/6] test for AddCollisionBoxToPlanningScene behavior --- .../tests/test_moveit_behaviors.py | 64 +++++++++++++++++++ 1 file changed, 64 insertions(+) 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"