From b2a0c8c23d2f71eb6f6501ffb421012ec670a5b0 Mon Sep 17 00:00:00 2001 From: Misha Savchenko Date: Wed, 30 Sep 2026 16:22:21 -0500 Subject: [PATCH 1/3] added a behavior for setting the world --- .../imetro_behavior/geometry_behaviors.py | 30 +++++++++++++++++++ 1 file changed, 30 insertions(+) diff --git a/src/imetro_behavior/imetro_behavior/geometry_behaviors.py b/src/imetro_behavior/imetro_behavior/geometry_behaviors.py index 49b08a6..42daae9 100644 --- a/src/imetro_behavior/imetro_behavior/geometry_behaviors.py +++ b/src/imetro_behavior/imetro_behavior/geometry_behaviors.py @@ -30,8 +30,10 @@ TransformStamped, TwistStamped, ) +from phoebe_interfaces.srv import SetWorld from py_trees.common import Access, Status from py_trees.ports import BehaviourWithPorts, PortInformation +from imetro_behavior.ros_behaviors.service_client import RosServiceClientBase from rclpy.node import Node from rclpy.time import Time from scipy.spatial.transform import RigidTransform @@ -752,3 +754,31 @@ def terminate(self, new_status: Status) -> None: """Cleanup the publisher""" if self.status == Status.RUNNING and new_status == Status.INVALID and self.publisher is not None: self.publisher.destroy() + +class SetWorldBehavior(RosServiceClientBase): + """ + Update transform for the world_publisher node. + """ + + def __init__(self, name: str, **kwargs: Any): + super().__init__(name, service_type=SetWorld, **kwargs) + + INPUT_PORTS = {"transform_stamped": PortInformation(data_type=TransformStamped)} + + OUTPUT_PORTS = {} + + def create_request(self) -> SetWorld.Request: + """Create a SetWorld service request.""" + request = SetWorld.Request() + request.transform = self.get_input("transform_stamped").transform + return request + + def process_response(self, response: SetWorld.Response) -> Status: + """Process the SetWorld service response.""" + if response.success: + self.logger.info("Successfully set the world frame!") + return Status.SUCCESS + else: + self.logger.error("Error: failed to set the world frame.") + return Status.FAILURE + From 24b70738061575d4dab24074a8e0d7b19c036975 Mon Sep 17 00:00:00 2001 From: Misha Savchenko Date: Wed, 30 Sep 2026 16:22:37 -0500 Subject: [PATCH 2/3] added test for set world behavior --- .../tests/test_geometry_behaviors.py | 27 +++++++++++++++++++ 1 file changed, 27 insertions(+) diff --git a/src/imetro_behavior/tests/test_geometry_behaviors.py b/src/imetro_behavior/tests/test_geometry_behaviors.py index a34c49b..6261393 100644 --- a/src/imetro_behavior/tests/test_geometry_behaviors.py +++ b/src/imetro_behavior/tests/test_geometry_behaviors.py @@ -20,6 +20,7 @@ import numpy as np import pytest from geometry_msgs.msg import PoseStamped, TransformStamped +from phoebe_interfaces.srv import SetWorld from py_trees.blackboard import Blackboard from py_trees.common import Status from rclpy.node import Node @@ -41,6 +42,7 @@ TransformStampedToPoseStamped, TwistAboutPose, YamlPoseToPoseStamped, + SetWorldBehavior, ) @@ -620,3 +622,28 @@ def test_decompose_pose_stamped() -> None: assert frame_id_msg == "odom" assert translation_msg == [1.0, 2.0, 3.0] assert orientation_msg == [0.0, 0.0, 0.0, 1.0] + + +def test_set_world(ros_node: Node) -> None: + behavior = SetWorldBehavior(name="set_world_behavior", service_name="/set_world") + behavior.setup(node=ros_node) + + behavior.setup_ports() + # + transform_stamped = TransformStamped() + transform_stamped.transform.translation.x = 0.0 + transform_stamped.transform.translation.y = 0.1 + transform_stamped.transform.translation.z = 0.2 + + transform_stamped.transform.rotation.x = 0.3 + transform_stamped.transform.rotation.y = 0.4 + transform_stamped.transform.rotation.z = 0.5 + transform_stamped.transform.rotation.w = 0.6 + + Blackboard.set(behavior._get_blackboard_key("transform_stamped"), transform_stamped) + + assert isinstance(behavior.create_request(), SetWorld.Request) + + response = SetWorld.Response() + response.success = True + assert behavior.process_response(response) == Status.SUCCESS From a713fc85f153f48087d551f35832b4aa62283b72 Mon Sep 17 00:00:00 2001 From: Misha Savchenko Date: Wed, 30 Sep 2026 16:29:08 -0500 Subject: [PATCH 3/3] but what if we cant find phoebe_interfaces pkg --- .../imetro_behavior/geometry_behaviors.py | 54 ++++++++++--------- .../tests/test_geometry_behaviors.py | 47 ++++++++-------- 2 files changed, 56 insertions(+), 45 deletions(-) diff --git a/src/imetro_behavior/imetro_behavior/geometry_behaviors.py b/src/imetro_behavior/imetro_behavior/geometry_behaviors.py index 42daae9..cc8f29f 100644 --- a/src/imetro_behavior/imetro_behavior/geometry_behaviors.py +++ b/src/imetro_behavior/imetro_behavior/geometry_behaviors.py @@ -30,10 +30,8 @@ TransformStamped, TwistStamped, ) -from phoebe_interfaces.srv import SetWorld from py_trees.common import Access, Status from py_trees.ports import BehaviourWithPorts, PortInformation -from imetro_behavior.ros_behaviors.service_client import RosServiceClientBase from rclpy.node import Node from rclpy.time import Time from scipy.spatial.transform import RigidTransform @@ -42,6 +40,7 @@ from tf2_ros import TransformBroadcaster from imetro_behavior.helpers import set_ros_node +from imetro_behavior.ros_behaviors.service_client import RosServiceClientBase class CreatePoseStamped(BehaviourWithPorts): @@ -755,30 +754,37 @@ def terminate(self, new_status: Status) -> None: if self.status == Status.RUNNING and new_status == Status.INVALID and self.publisher is not None: self.publisher.destroy() -class SetWorldBehavior(RosServiceClientBase): - """ - Update transform for the world_publisher node. - """ - def __init__(self, name: str, **kwargs: Any): - super().__init__(name, service_type=SetWorld, **kwargs) +try: + from phoebe_interfaces.srv import SetWorld - INPUT_PORTS = {"transform_stamped": PortInformation(data_type=TransformStamped)} + class SetWorldBehavior(RosServiceClientBase): + """ + Update transform for the world_publisher node. + """ - OUTPUT_PORTS = {} + def __init__(self, name: str, **kwargs: Any): + super().__init__(name, service_type=SetWorld, **kwargs) + + INPUT_PORTS = {"transform_stamped": PortInformation(data_type=TransformStamped)} + + OUTPUT_PORTS = {} + + def create_request(self) -> SetWorld.Request: + """Create a SetWorld service request.""" + request = SetWorld.Request() + request.transform = self.get_input("transform_stamped").transform + return request + + def process_response(self, response: SetWorld.Response) -> Status: + """Process the SetWorld service response.""" + if response.success: + self.logger.info("Successfully set the world frame!") + return Status.SUCCESS + else: + self.logger.error("Error: failed to set the world frame.") + return Status.FAILURE - def create_request(self) -> SetWorld.Request: - """Create a SetWorld service request.""" - request = SetWorld.Request() - request.transform = self.get_input("transform_stamped").transform - return request - - def process_response(self, response: SetWorld.Response) -> Status: - """Process the SetWorld service response.""" - if response.success: - self.logger.info("Successfully set the world frame!") - return Status.SUCCESS - else: - self.logger.error("Error: failed to set the world frame.") - return Status.FAILURE +except ModuleNotFoundError as e: + print(e) diff --git a/src/imetro_behavior/tests/test_geometry_behaviors.py b/src/imetro_behavior/tests/test_geometry_behaviors.py index 6261393..55030d1 100644 --- a/src/imetro_behavior/tests/test_geometry_behaviors.py +++ b/src/imetro_behavior/tests/test_geometry_behaviors.py @@ -20,7 +20,6 @@ import numpy as np import pytest from geometry_msgs.msg import PoseStamped, TransformStamped -from phoebe_interfaces.srv import SetWorld from py_trees.blackboard import Blackboard from py_trees.common import Status from rclpy.node import Node @@ -42,7 +41,6 @@ TransformStampedToPoseStamped, TwistAboutPose, YamlPoseToPoseStamped, - SetWorldBehavior, ) @@ -624,26 +622,33 @@ def test_decompose_pose_stamped() -> None: assert orientation_msg == [0.0, 0.0, 0.0, 1.0] -def test_set_world(ros_node: Node) -> None: - behavior = SetWorldBehavior(name="set_world_behavior", service_name="/set_world") - behavior.setup(node=ros_node) - - behavior.setup_ports() - # - transform_stamped = TransformStamped() - transform_stamped.transform.translation.x = 0.0 - transform_stamped.transform.translation.y = 0.1 - transform_stamped.transform.translation.z = 0.2 +try: + from phoebe_interfaces.srv import SetWorld + + from imetro_behavior.geometry_behaviors import SetWorldBehavior + + def test_set_world(ros_node: Node) -> None: + behavior = SetWorldBehavior(name="set_world_behavior", service_name="/set_world") + behavior.setup(node=ros_node) + + behavior.setup_ports() + transform_stamped = TransformStamped() + transform_stamped.transform.translation.x = 0.0 + transform_stamped.transform.translation.y = 0.1 + transform_stamped.transform.translation.z = 0.2 + + transform_stamped.transform.rotation.x = 0.3 + transform_stamped.transform.rotation.y = 0.4 + transform_stamped.transform.rotation.z = 0.5 + transform_stamped.transform.rotation.w = 0.6 - transform_stamped.transform.rotation.x = 0.3 - transform_stamped.transform.rotation.y = 0.4 - transform_stamped.transform.rotation.z = 0.5 - transform_stamped.transform.rotation.w = 0.6 + Blackboard.set(behavior._get_blackboard_key("transform_stamped"), transform_stamped) - Blackboard.set(behavior._get_blackboard_key("transform_stamped"), transform_stamped) + assert isinstance(behavior.create_request(), SetWorld.Request) - assert isinstance(behavior.create_request(), SetWorld.Request) + response = SetWorld.Response() + response.success = True + assert behavior.process_response(response) == Status.SUCCESS - response = SetWorld.Response() - response.success = True - assert behavior.process_response(response) == Status.SUCCESS +except ModuleNotFoundError as e: + print(e)