Publish twist

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

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()

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