From 74e59dd4cbd7a0942e1b69448fb3e53c177b1e63 Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Thu, 10 Aug 2023 01:07:56 -0700 Subject: [PATCH] wip: continued joint trajectory controller implementation --- angler_control/controllers/controller.py | 9 +- ...se_multidof_joint_trajectory_controller.py | 12 +- .../multidof_joint_trajectory_controller.py | 128 ++++++++++++++++++ .../robot_trajectory_controller/trajectory.py | 37 +++++ 4 files changed, 179 insertions(+), 7 deletions(-) create mode 100644 angler_control/controllers/robot_trajectory_controller/multidof_joint_trajectory_controller.py create mode 100644 angler_control/controllers/robot_trajectory_controller/trajectory.py diff --git a/angler_control/controllers/controller.py b/angler_control/controllers/controller.py index af3aba3..9933565 100644 --- a/angler_control/controllers/controller.py +++ b/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,8 +61,9 @@ 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. @@ -70,7 +71,7 @@ def on_robot_state_update(self, state: RobotState) -> None: 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: diff --git a/angler_control/controllers/robot_trajectory_controller/base_multidof_joint_trajectory_controller.py b/angler_control/controllers/robot_trajectory_controller/base_multidof_joint_trajectory_controller.py index a3c887f..e92011c 100644 --- a/angler_control/controllers/robot_trajectory_controller/base_multidof_joint_trajectory_controller.py +++ b/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): ... diff --git a/angler_control/controllers/robot_trajectory_controller/multidof_joint_trajectory_controller.py b/angler_control/controllers/robot_trajectory_controller/multidof_joint_trajectory_controller.py new file mode 100644 index 0000000..1769134 --- /dev/null +++ b/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 diff --git a/angler_control/controllers/robot_trajectory_controller/trajectory.py b/angler_control/controllers/robot_trajectory_controller/trajectory.py new file mode 100644 index 0000000..664f5d5 --- /dev/null +++ b/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): + ...