diff --git a/src/imetro_behavior/imetro_behavior/decorators.py b/src/imetro_behavior/imetro_behavior/decorators.py index 58081d1..a1a349c 100644 --- a/src/imetro_behavior/imetro_behavior/decorators.py +++ b/src/imetro_behavior/imetro_behavior/decorators.py @@ -16,7 +16,7 @@ # WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the # License for the specific language governing permissions and limitations # under the License. - +import time from collections.abc import Iterator from py_trees.behaviour import Behaviour @@ -91,3 +91,64 @@ def tick(self) -> Iterator[Behaviour]: def update(self) -> Status: """Reflect the child's status, since it is only ticked when the variable is true.""" return self.decorated.status + + +class TimeoutPort(PortsMixin, Decorator): + """ + Reimplementation of the Timeout behavior from the py_trees library + [Link](https://py-trees.readthedocs.io/en/devel/modules.html#py_trees.decorators.Timeout) for reference. + + NOTE: This behavior is supposed to be temporary. + + Executes a child/subtree with a timeout. + + A decorator that applies a timeout pattern to an existing behaviour. + If the timeout is reached, the encapsulated behaviour's + :meth:`~py_trees.behaviour.Behaviour.stop` method is called with + status :data:`~py_trees.common.Status.FAILURE` otherwise it will + simply directly tick and return with the same status + as that of its encapsulated behaviour. + """ + + finish_time: float = None + + @classmethod + def input_ports(cls) -> dict: + """Return the input port declarations.""" + return {"duration": PortInformation(data_type=float, required=True)} + + @classmethod + def output_ports(cls) -> dict: + """Return the output port declarations.""" + return {} + + def initialise(self) -> None: + """Reset the feedback message and finish time on behaviour entry.""" + self.finish_time = time.monotonic() + self.get_input("duration") + self.feedback_message = "" + + def update(self) -> Status: + """ + Fail on timeout, or block / reflect the child's result accordingly. + + Terminate the child and return + :data:`~py_trees.common.Status.FAILURE` + if the timeout is exceeded. + + Returns: + the behaviour's new status :class:`~py_trees.common.Status` + """ + + current_time = time.monotonic() + + if self.decorated.status == Status.RUNNING and current_time > self.finish_time: + self.feedback_message = "timed out" + self.logger.debug(f"{self.__class__.__name__}.update() {self.feedback_message}") + # invalidate the decorated (i.e. cancel it), could also put this logic in a terminate() method + self.decorated.stop(Status.INVALID) + return Status.FAILURE + if self.decorated.status == Status.RUNNING: + self.feedback_message = f"time still ticking ... [remaining: {self.finish_time - current_time}s]" + else: + self.feedback_message = "child finished before timeout triggered" + return self.decorated.status diff --git a/src/imetro_behavior/imetro_behavior/geometry_behaviors.py b/src/imetro_behavior/imetro_behavior/geometry_behaviors.py index 1235621..a49a0b5 100644 --- a/src/imetro_behavior/imetro_behavior/geometry_behaviors.py +++ b/src/imetro_behavior/imetro_behavior/geometry_behaviors.py @@ -17,6 +17,8 @@ # License for the specific language governing permissions and limitations # under the License. +from typing import Any + import copy import yaml @@ -29,7 +31,7 @@ from py_trees.common import Access, Status from py_trees.ports import BehaviourWithPorts, PortInformation import tf2_geometry_msgs -from geometry_msgs.msg import PoseStamped, Pose, Point, Quaternion, TransformStamped +from geometry_msgs.msg import PoseStamped, Pose, Point, Quaternion, TransformStamped, TwistStamped from std_msgs.msg import Header from tf2_ros import TransformBroadcaster @@ -712,3 +714,72 @@ def update(self) -> Status: transform_stamped = self.get_input("transform_stamped") self.tf_broadcaster.sendTransform(transform_stamped) return Status.SUCCESS + + +class PublishTwist(BehaviourWithPorts): + """ + Takes in `linear` and `angular` velocities and converts them geometry_msgs/msg/Twist. + Creates std_msgs/msg/Header from `frame_id` input and current ROS 2 timestamp. + Combines both into geometry_msgs/msg/TwistStamped and publishes onto the `topic_name`. + """ + + def __init__( + self, + name: str, + *, + topic_name: str, + **kwargs: Any, + ) -> None: + super().__init__(name, **kwargs) + self.topic_name = topic_name + self.publisher = None + self.twist_stamped = None + + @classmethod + def input_ports(cls) -> dict: + """Return the input port declarations.""" + return { + "linear_velocity": PortInformation(data_type=list[float], required=True), + "angular_velocity": PortInformation(data_type=list[float], required=True), + "frame_id": PortInformation(data_type=str, required=False), + } + + @classmethod + def output_ports(cls) -> dict: + """Return the output port declarations.""" + return {} + + def initialise(self) -> None: + """Create TwistStamped message publisher, assemble twist message.""" + self.publisher = self.node.create_publisher(TwistStamped, self.topic_name, 1) + self.twist_stamped = TwistStamped() + + linear = self.get_input("linear_velocity", [0.0, 0.0, 0.0]) + angular = self.get_input("angular_velocity", [0.0, 0.0, 0.0]) + + self.twist_stamped.twist.linear.x = linear[0] + self.twist_stamped.twist.linear.y = linear[1] + self.twist_stamped.twist.linear.z = linear[2] + + self.twist_stamped.twist.angular.x = angular[0] + self.twist_stamped.twist.angular.y = angular[1] + self.twist_stamped.twist.angular.z = angular[2] + + self.twist_stamped.header.stamp = self.node.get_clock().now().to_msg() + self.twist_stamped.header.frame_id = self.get_input("frame_id", "map") + + def setup(self, **kwargs): + """Get the ROS node from the blackboard.""" + self.node = kwargs.get("node") + if not isinstance(self.node, Node): + raise KeyError(f"A valid ROS node is required to setup the '{self.qualified_name}' node.") + + def update(self) -> Status: + """Publish twist stamped message.""" + self.publisher.publish(self.twist_stamped) + return Status.SUCCESS + + def terminate(self, new_status: Status) -> Status: + """Cleanup the publisher""" + if self.status == Status.RUNNING and new_status == Status.INVALID: + self.publisher.destroy() diff --git a/src/imetro_behavior/tests/test_decorators.py b/src/imetro_behavior/tests/test_decorators.py index edcfcf4..4c18f2c 100644 --- a/src/imetro_behavior/tests/test_decorators.py +++ b/src/imetro_behavior/tests/test_decorators.py @@ -16,12 +16,13 @@ # WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the # License for the specific language governing permissions and limitations # under the License. +import time import py_trees from py_trees.blackboard import Blackboard from py_trees.common import Status -from imetro_behavior.decorators import SuccessIfVariableIsFalse, SuccessIfVariableIsTrue +from imetro_behavior.decorators import SuccessIfVariableIsFalse, SuccessIfVariableIsTrue, TimeoutPort def test_success_if_variable_is_true_skips_child() -> None: @@ -88,3 +89,85 @@ def test_success_if_variable_is_false_ticks_child() -> None: decorator.tick_once() assert decorator.status == Status.FAILURE assert child.status == Status.FAILURE + + +def test_timeout_reinitialization() -> None: + running = py_trees.behaviours.Running(name="Running") + + timeout = TimeoutPort(name="Timeout", child=running) + timeout.setup_ports() + Blackboard.set(timeout._get_blackboard_key("duration"), 0.2) + + # Test that it times out and re-initialises properly + for i in range(0, 2): + timeout.tick_once() + + assert timeout.status == Status.RUNNING + assert running.status == Status.RUNNING + + time.sleep(0.3) + + timeout.tick_once() + + assert timeout.status == Status.FAILURE + assert running.status == Status.INVALID + + +def test_timeout_passthrough_success() -> None: + # test that it passes on success + count = py_trees.behaviours.StatusQueue( + name="Queue", + queue=[Status.RUNNING], + eventually=Status.SUCCESS, + ) + + timeout = TimeoutPort(name="Timeout", child=count) + timeout.setup_ports() + Blackboard.set(timeout._get_blackboard_key("duration"), 0.2) + + timeout.tick_once() + assert count.status == Status.RUNNING + assert timeout.status == Status.RUNNING + + timeout.tick_once() + assert count.status == Status.SUCCESS + assert timeout.status == Status.SUCCESS + + +def test_timeout_passthrough_failure() -> None: + # test that it passes on failure + failure = py_trees.behaviours.Failure(name="Failure") + + timeout = TimeoutPort(name="Timeout", child=failure) + timeout.setup_ports() + Blackboard.set(timeout._get_blackboard_key("duration"), 0.2) + + timeout.tick_once() + + assert failure.status == Status.FAILURE + assert timeout.status == Status.FAILURE + + +def test_timeout_status_passthrough_order() -> None: + # test that it succeeds if child succeeds on last tick + count = py_trees.behaviours.StatusQueue( + name="Queue", + queue=[Status.RUNNING], + eventually=Status.SUCCESS, + ) + timeout = TimeoutPort(name="Timeout", child=count) + timeout.setup_ports() + + Blackboard.set(timeout._get_blackboard_key("duration"), 0.1) + + timeout.tick_once() + + assert count.status == Status.RUNNING + assert timeout.status == Status.RUNNING + + time.sleep(0.2) # go past the duration + + timeout.tick_once() + + assert count.status == Status.SUCCESS + assert timeout.status == Status.SUCCESS