wip: continued joint trajectory controller implementation

angler_control/controllers/controller.py

@@ -21,7 +21,7 @@
 from abc import ABC, abstractmethod
 
 from moveit_msgs.msg import RobotState
-from rclpy.callback_groups import MutuallyExclusiveCallbackGroup
+from rclpy.callback_groups import ReentrantCallbackGroup
 from rclpy.node import Node
 from rclpy.qos import qos_profile_sensor_data
 from std_srvs.srv import SetBool
@@ -61,16 +61,17 @@ def __init__(self, controller_name: str) -> None:
         )
 
         # Create a new callback group for the control loop timer
-        self.timer_cb_group = MutuallyExclusiveCallbackGroup()
-        self.create_timer(self.dt, self._update)
+        # Don't start it automatically though.
+        self.timer_cb_group = ReentrantCallbackGroup()
+        self.control_loop_timer = self.create_timer(self.dt, self._update)
 
     def on_robot_state_update(self, state: RobotState) -> None:
         """Execute this function on robot state update.
 
         Args:
             state: The most recent robot state update.
         """
-        # Don't do anything with the state
+        # Don't do anything with the state by default
         ...
 
     def _robot_state_cb(self, state: RobotState) -> None:

angler_control/controllers/robot_trajectory_controller/base_multidof_joint_trajectory_controller.py

@@ -69,8 +69,8 @@ def _set_point_before_trajectory_msg(
         self.time_before_trajectory = current_time
         self.state_before_trajectory = current_point
 
+    @staticmethod
     def merge_trajectory_with_current_state(
-        self,
         current_time: Time,
         state: MultiDOFJointTrajectoryPoint,
         trajectory: MultiDOFJointTrajectory,
@@ -88,7 +88,7 @@ def update(self, trajectory: MultiDOFJointTrajectory):
         )
         self.sampled_already = False
 
-    def value(
+    def sample(
         self,
         t: Time,
         start_point: MultiDOFJointTrajectoryPoint,
@@ -105,7 +105,13 @@ def value(
         if t < self.time_before_trajectory:
             return None
 
-        # TODO(evan): finish this: https://github.com/ros-controls/ros2_controllers/blob/83e415c0f00512398df3c3a508f738fc76a7ecda/joint_trajectory_controller/src/trajectory.cpp#L86
+        output_state = MultiDOFJointTrajectoryPoint()
+        first_point_in_msg = self.trajectory.points[0]
+        first_point_timestamp = self.start_time + first_point_in_msg.time_from_start
+
+        if t < first_point_timestamp:
+            if True:
+                ...
 
     def interpolate(self):
         ...

angler_control/controllers/robot_trajectory_controller/multidof_joint_trajectory_controller.py

@@ -0,0 +1,128 @@
+# Copyright 2023, Evan Palmer
+#
+# Permission is hereby granted, free of charge, to any person obtaining a copy
+# of this software and associated documentation files (the "Software"), to deal
+# in the Software without restriction, including without limitation the rights
+# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+# copies of the Software, and to permit persons to whom the Software is
+# furnished to do so, subject to the following conditions:
+#
+# The above copyright notice and this permission notice shall be included in
+# all copies or substantial portions of the Software.
+#
+# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
+# THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+# THE SOFTWARE.
+
+from abc import abstractmethod
+
+import rclpy
+from control_msgs.action import FollowJointTrajectory
+from controllers.controller import BaseController
+from rclpy.action import ActionServer, GoalResponse
+from rclpy.timer import Timer
+
+
+class MultiDOFJointTrajectoryController(BaseController):
+    """Base interface for a MultiDOFJointTrajectory controller."""
+
+    def __init__(self, controller_name: str) -> None:
+        """Create a new multi-DOF joint trajectory controller.
+
+        Args:
+            controller_name: The unique controller name.
+        """
+        super().__init__(controller_name)
+
+        self.goal_handle: FollowJointTrajectory.Goal | None = None
+        self.goal_result: FollowJointTrajectory.Result | None = None
+        self._preempted = False
+        self._running = False
+        self._timer: Timer | None = None
+
+        self.execute_trajectory_client = ActionServer(
+            self,
+            FollowJointTrajectory,
+            f"/angler/{controller_name}/execute_trajectory",
+            goal_callback=self.handle_action_request_cb,
+            execute_callback=self.execute_trajectory_cb,
+        )
+
+    def handle_action_request_cb(
+        self, goal: FollowJointTrajectory.Goal
+    ) -> GoalResponse:
+        if not self.armed:
+            return GoalResponse.REJECT
+
+        if self.goal_handle is not None:
+            self._preempted = True
+            return GoalResponse.ACCEPT
+
+        return GoalResponse.ACCEPT
+
+    @abstractmethod
+    def on_update(self) -> None:
+        if self.goal_handle is None:
+            return
+
+    async def execute_trajectory_cb(
+        self, goal: FollowJointTrajectory.Goal
+    ) -> FollowJointTrajectory.Result:
+        # Wait for any previous goals to be cleaned up
+        timer = self.create_rate(self.dt)
+        while self._running and rclpy.ok():
+            self.get_logger().info(
+                "Waiting for previous joint trajectory goal to exit!"
+            )
+            timer.sleep()
+
+        # Clean things up after the previous action
+        self._preempted = False
+        self._running = True
+        self.goal_handle = goal
+        self.goal_result = None
+
+        # Spin until we get a response from the controller or the goal is preempted/
+        # canceled
+        while (
+            rclpy.ok()
+            and not self._preempted
+            and self._running
+            and self.goal_result is None
+            and not self.goal_handle.is_cancel_requested  # type: ignore
+        ):
+            timer.sleep()
+
+        self._running = False
+
+        if self._preempted:
+            return self.preempt_current_goal()
+        elif self.goal_handle.is_cancel_requested:  # type: ignore
+            return self.cancel_current_goal()
+        elif self.goal_result is not None:
+            return self.goal_result
+
+        # If we get here, something bad happened
+        raise RuntimeError(
+            f"An error occurred while attempting to execute the goal {goal}"
+        )
+
+    def preempt_current_goal(self) -> FollowJointTrajectory.Result:
+        result = FollowJointTrajectory.Result()
+        result.error_code = FollowJointTrajectory.Result.INVALID_GOAL
+        result.error_string = (
+            "The current action was cancelled by a new incoming action"
+        )
+        self.goal_handle.canceled()  # type: ignore
+        return result
+
+    def cancel_current_goal(self) -> FollowJointTrajectory.Result:
+        result = FollowJointTrajectory.Result()
+        result.error_code = FollowJointTrajectory.Result.INVALID_GOAL
+        result.error_string = "The current action was cancelled by a client!"
+        self.goal_handle.canceled()  # type: ignore
+        return result

angler_control/controllers/robot_trajectory_controller/trajectory.py

@@ -0,0 +1,37 @@
+# Copyright 2023, Evan Palmer
+#
+# Permission is hereby granted, free of charge, to any person obtaining a copy
+# of this software and associated documentation files (the "Software"), to deal
+# in the Software without restriction, including without limitation the rights
+# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+# copies of the Software, and to permit persons to whom the Software is
+# furnished to do so, subject to the following conditions:
+#
+# The above copyright notice and this permission notice shall be included in
+# all copies or substantial portions of the Software.
+#
+# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
+# THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+# THE SOFTWARE.
+
+from rclpy.time import Time
+from trajectory_msgs.msg import MultiDOFJointTrajectory, MultiDOFJointTrajectoryPoint
+
+
+class Trajectory:
+    def __init__(self, trajectory: MultiDOFJointTrajectory | None = None) -> None:
+        pass
+
+    def interpolate(
+        self,
+        start_point: MultiDOFJointTrajectoryPoint,
+        end_point: MultiDOFJointTrajectoryPoint,
+    ):
+        ...
+
+    def sample(self, t: Time):
+        ...