From 2df3363f331ce8a823cbb1b806cc7e185481397c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?J=C3=A9r=C3=B4me=20Guzzi?= Date: Tue, 27 Jun 2023 18:18:29 +0200 Subject: [PATCH] Updated servos Added service to get the angle using the SDK api. Corrected the action target. Added index checking. --- robomaster_msgs/CMakeLists.txt | 1 + robomaster_msgs/srv/GetServoAngle.srv | 5 ++ .../robomaster_ros/modules/servo.py | 53 ++++++++++++++----- 3 files changed, 45 insertions(+), 14 deletions(-) create mode 100644 robomaster_msgs/srv/GetServoAngle.srv diff --git a/robomaster_msgs/CMakeLists.txt b/robomaster_msgs/CMakeLists.txt index 85941ad..0e544bc 100644 --- a/robomaster_msgs/CMakeLists.txt +++ b/robomaster_msgs/CMakeLists.txt @@ -74,6 +74,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "srv/GetADC.srv" "srv/GetIO.srv" "srv/GetPulse.srv" + "srv/GetServoAngle.srv" DEPENDENCIES std_msgs ) diff --git a/robomaster_msgs/srv/GetServoAngle.srv b/robomaster_msgs/srv/GetServoAngle.srv new file mode 100644 index 0000000..c80e29f --- /dev/null +++ b/robomaster_msgs/srv/GetServoAngle.srv @@ -0,0 +1,5 @@ +# Command to read the servo angle. + +uint8 index # ID of the servo (between 0 and 2) +--- +float32 angle # absolute angle in rad diff --git a/robomaster_ros/robomaster_ros/modules/servo.py b/robomaster_ros/robomaster_ros/modules/servo.py index 60dcb27..20e0643 100644 --- a/robomaster_ros/robomaster_ros/modules/servo.py +++ b/robomaster_ros/robomaster_ros/modules/servo.py @@ -7,6 +7,7 @@ import robomaster.robot import sensor_msgs.msg import robomaster_msgs.msg +import robomaster_msgs.srv import robomaster_msgs.action from typing import Dict, List, Tuple, Any, Optional, cast @@ -15,7 +16,7 @@ from ..client import RoboMasterROS from .. import Module -from ..utils import rate, deg +from ..utils import rate, deg, rad from ..action import add_cb # DONE from the APP 1 unit = 0.17578 degress @@ -60,10 +61,14 @@ def angle(self) -> float: def speed(self) -> float: return self.direction * self.speed_value * math.pi / UNIT_180_DEG / 5 - # TODO(Jerome): review - def external(self, angle: float) -> float: - return (self.direction * (angle - self.reference_angle) + - self.reference_value * math.pi / UNIT_180_DEG) + # convert an angle in the URDF defined joint frame to an angle in the servo own frame. + def angle_to_servo(self, angle: float) -> float: + return self.direction * (angle - self.reference_angle) + + # convert an angle to the URDF defined joint frame from an angle in the servo own frame, + # i.e. the inverse of `angle_to_servo` + def angle_from_servo(self, zero_angle: float) -> float: + return self.direction * zero_angle + self.reference_angle # TODO(maybe): add get_angle @@ -107,6 +112,10 @@ def __init__(self, robot: robomaster.robot.Robot, node: 'RoboMasterROS') -> None node, robomaster_msgs.action.MoveServo, 'move_servo', self.execute_move_servo_callback, goal_callback=self.new_servo_goal_callback, cancel_callback=self.cancel_callback, callback_group=cbg) + get_angle_cbg = rclpy.callback_groups.MutuallyExclusiveCallbackGroup() + node.create_service( + robomaster_msgs.srv.GetServoAngle, 'get_servo_angle', + self.get_servo_angle_cb, callback_group=get_angle_cbg) def publish_sensor_raw_state(self, msg: ServoData) -> None: omsg = robomaster_msgs.msg.ServoRawState() @@ -132,11 +141,17 @@ def execute_move_servo_callback(self, goal_handle: Any ) -> robomaster_msgs.action.MoveServo.Result: # TODO(jerome): Complete with failures, ... request = goal_handle.request + if request.index not in self.servos: + self.logger.warning(f'Unknown servo with index {request.index}') + goal_handle.abort() + return robomaster_msgs.action.MoveServo.Result() servo = self.servos[request.index] feedback_msg = robomaster_msgs.action.MoveServo.Feedback() try: + # `angle` is the angle with respect to the servo zero in degrees + # `index` starts from 1 self.action = self.api.moveto( - index=servo.index + 1, angle=round(deg(servo.external(request.angle)))) + index=servo.index + 1, angle=round(deg(servo.angle_to_servo(request.angle)))) except RuntimeError as e: self.logger.warning(f'Cannot move servo: {e}') goal_handle.abort() @@ -147,7 +162,7 @@ def cb() -> None: feedback_msg.progress = cast(robomaster.action.Action, self.action)._percent * 0.01 goal_handle.publish_feedback(feedback_msg) add_cb(self.action, cb) - # TODO(Jerome): parametrize timeput + # TODO(Jerome): parametrize timeout self.action.wait_for_completed(timeout=7) if self.action.has_succeeded: goal_handle.succeed() @@ -187,10 +202,20 @@ def updated_servo(self, msg: ServoData) -> None: self.publish_sensor_raw_state(msg) def has_received_cmd(self, msg: robomaster_msgs.msg.ServoCommand) -> None: - if msg.enable: - # self.logger.info("\n\nSEND drive_speed\n\n") - speed = rpm(msg.angular_speed) - self.api.drive_speed(index=msg.index + 1, speed=speed) - self.logger.info(f"SENT drive_speed {speed}") - else: - self.api.pause() + if msg.index in self.servos: + if msg.enable: + speed = rpm(msg.angular_speed) * self.servos[msg.index].direction + self.api.drive_speed(index=msg.index + 1, speed=speed) + self.logger.debug(f"servo target speed set to {speed}") + else: + self.api.pause(index=msg.index + 1) + + def get_servo_angle_cb(self, request: robomaster_msgs.srv.GetServoAngle.Request, + response: robomaster_msgs.srv.GetServoAngle.Response + ) -> robomaster_msgs.srv.GetServoAngle.Response: + if request.index in self.servos: + beta: float = self.api.get_angle(index=request.index + 1) + self.logger.info(f"beta {beta}") + response.angle = self.servos[request.index].angle_from_servo( + rad(beta) - math.pi) + return response