From e4ecf569428943179d2eea7e49dcc1bd15ba5892 Mon Sep 17 00:00:00 2001 From: Matthew Barulic Date: Tue, 7 Jul 2026 01:47:04 +0900 Subject: [PATCH] Updates controls test scripts from comp --- .../scripts/line_test.py | 74 ++++++++++ .../scripts/pivot_test.py | 135 ++++++++++++++++++ .../scripts/waypoints_test.py | 48 ++++--- 3 files changed, 240 insertions(+), 17 deletions(-) create mode 100755 motion/ateam_controls_testing/scripts/line_test.py create mode 100755 motion/ateam_controls_testing/scripts/pivot_test.py diff --git a/motion/ateam_controls_testing/scripts/line_test.py b/motion/ateam_controls_testing/scripts/line_test.py new file mode 100755 index 00000000..e8902b10 --- /dev/null +++ b/motion/ateam_controls_testing/scripts/line_test.py @@ -0,0 +1,74 @@ +#! /usr/bin/env python3 + +# Copyright 2026 A Team +# +# 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. + +import argparse + +from ateam_msgs.msg import RobotMotionCommand + +import rclpy +from rclpy.node import Node +from rclpy.qos import qos_profile_system_default + + +command_msg = RobotMotionCommand() +command_msg.body_control_mode = RobotMotionCommand.BCM_POINT_LINE +command_msg.kick_request = RobotMotionCommand.KR_DISABLE +command_msg.line_global_theta = 0.0 +command_msg.line_start_x = -2.0 +command_msg.line_start_y = 0.0 +command_msg.line_target_x = 0.0 +command_msg.line_target_y = 0.0 +command_msg.line_dir_x = 0.0 +command_msg.line_dir_y = 1.0 +command_msg.line_velocity = 0.4 +command_msg.line_max_vel_colinear = 10.0 +command_msg.line_max_vel_perp = 2.0 +command_msg.line_max_vel_angular = 5.0 +command_msg.line_max_accel_colinear = 2.0 +command_msg.line_max_accel_perp = 2.0 +command_msg.line_colinear_start_thresh = 0.1 + + +if __name__ == '__main__': + argparser = argparse.ArgumentParser( + description='Test node for sending line maneuvers to the robot' + ) + argparser.add_argument('robot_id', type=int) + args = argparser.parse_args() + + rclpy.init() + node = Node('line_test') + + command_pub = node.create_publisher( + RobotMotionCommand, + f'/robot_motion_commands/robot{args.robot_id}', + qos_profile_system_default, + ) + + rate = node.create_rate(100) + while rclpy.ok(): + rclpy.spin_once(node) + command_pub.publish(command_msg) + rate.sleep() + + node.destroy_node() + rclpy.shutdown() diff --git a/motion/ateam_controls_testing/scripts/pivot_test.py b/motion/ateam_controls_testing/scripts/pivot_test.py new file mode 100755 index 00000000..cea3ed98 --- /dev/null +++ b/motion/ateam_controls_testing/scripts/pivot_test.py @@ -0,0 +1,135 @@ +#! /usr/bin/env python3 + +# Copyright 2026 A Team +# +# 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. + +import argparse +from dataclasses import dataclass +import math +import time + +from ateam_msgs.msg import RobotMotionCommand, VisionStateRobot +import rclpy +from rclpy.node import Node +from rclpy.qos import qos_profile_system_default +from transforms3d.euler import quat2euler +import warnings +warnings.filterwarnings("ignore", category=SyntaxWarning, message="invalid escape sequence") +from angles import shortest_angular_distance +warnings.resetwarnings() + +angular_threshold = 0.1 + + +@dataclass +class Target(): + theta: float + orbit_radius: float + inset_angle: float + hold_time: float + + +targets = [ + Target(0.0, 0.1, math.pi / 2, 1.0), + Target(math.pi/2, 0.1, math.pi / 2, 1.0), + Target(math.pi, 0.1, math.pi / 2, 1.0), + Target(-math.pi/2, 0.1, math.pi / 2, 1.0), +] + +current_index = 0 +waypoint_hold_start_time = None +vision_robot_state_msg = None + + +def vision_callback(msg: VisionStateRobot): + """Store latest message.""" + global vision_robot_state_msg + vision_robot_state_msg = msg + + +def publish_command(index: int): + target = targets[index] + command_msg = RobotMotionCommand() + command_msg.body_control_mode = RobotMotionCommand.BCM_HEADING_PIVOT + command_msg.kick_request = RobotMotionCommand.KR_DISABLE + command_msg.pivot_global_theta = target.theta + command_msg.pivot_orbit_radius = target.orbit_radius + command_msg.pivot_inset_angle = target.inset_angle + command_msg.pivot_max_angular_acc = 8.0 + command_msg.pivot_max_angular_vel = 4.0 + command_pub.publish(command_msg) + + +def is_at_target(index: int): + target = targets[index] + # get yaw from quat + q = vision_robot_state_msg.pose.orientation + _, _, theta = quat2euler([q.w, q.x, q.y, q.z]) + return ( + vision_robot_state_msg.visible + and abs(shortest_angular_distance(theta, target.theta)) < angular_threshold + ) + + +if __name__ == '__main__': + argparser = argparse.ArgumentParser( + description='Test node for sending pivots to the robot' + ) + argparser.add_argument('robot_id', type=int) + argparser.add_argument('--color', '-c', type=str, default='blue') + args = argparser.parse_args() + + rclpy.init() + node = Node('pivot_test') + + command_pub = node.create_publisher( + RobotMotionCommand, + f'/robot_motion_commands/robot{args.robot_id}', + qos_profile_system_default, + ) + vision_sub = node.create_subscription( + VisionStateRobot, + f'/{args.color}_team/robot{args.robot_id}', + vision_callback, + qos_profile_system_default, + ) + + while rclpy.ok(): + rclpy.spin_once(node) + publish_command(0) + if vision_robot_state_msg is None: + continue + if is_at_target(0): + node.get_logger().info('Reached waypoint 0. Starting cycle.') + break + + while rclpy.ok(): + rclpy.spin_once(node) + if is_at_target(current_index): + if waypoint_hold_start_time is None: + waypoint_hold_start_time = time.time() + elif (time.time() - waypoint_hold_start_time) > targets[current_index].hold_time: + current_index = (current_index + 1) % len(targets) + waypoint_hold_start_time = None + node.get_logger().info(f'Moving to waypoint {current_index}') + publish_command(current_index) + + node.destroy_node() + rclpy.shutdown() diff --git a/motion/ateam_controls_testing/scripts/waypoints_test.py b/motion/ateam_controls_testing/scripts/waypoints_test.py index db6f0e18..3534a7f6 100755 --- a/motion/ateam_controls_testing/scripts/waypoints_test.py +++ b/motion/ateam_controls_testing/scripts/waypoints_test.py @@ -21,6 +21,12 @@ # THE SOFTWARE. import argparse +import warnings +warnings.filterwarnings("ignore", category=SyntaxWarning, message="invalid escape sequence") +from angles import shortest_angular_distance +warnings.resetwarnings() +from dataclasses import dataclass +import math import time from ateam_msgs.msg import RobotMotionCommand, VisionStateRobot @@ -29,15 +35,23 @@ from rclpy.qos import qos_profile_system_default from transforms3d.euler import quat2euler -linear_threshold = 1e-2 +linear_threshold = 2.0e-2 angular_threshold = 0.0349 -# x, y, theta, hold time + +@dataclass +class Waypoint(): + x: float + y: float + theta: float + hold_time: float + + waypoints = [ - (-0.5, -0.5, 0.0, 1.0), - (-0.5, 0.5, 0.0, 1.0), - (0.5, 0.5, 0.0, 1.0), - (0.5, -0.5, 0.0, 1.0), + Waypoint(-4.3, -2.5, math.pi/2, 4.0), + Waypoint(-0.5, -2.5, math.pi/2, 4.0), + # Waypoint(-3.2, 0.5, 0.0, 1.0), + # Waypoint(-3.2, -0.5, 0.0, 1.0), ] current_index = 0 @@ -55,14 +69,14 @@ def publish_waypoint_command(index: int): waypoint = waypoints[index] command_msg = RobotMotionCommand() command_msg.body_control_mode = RobotMotionCommand.BCM_GLOBAL_POSITION - command_msg.pose.x = waypoint[0] - command_msg.pose.y = waypoint[1] - command_msg.pose.theta = waypoint[2] + command_msg.pose.x = waypoint.x + command_msg.pose.y = waypoint.y + command_msg.pose.theta = waypoint.theta command_msg.kick_request = RobotMotionCommand.KR_DISABLE - command_msg.limit_acc_linear = 3.0 - command_msg.limit_vel_linear = 3.0 - command_msg.limit_acc_angular = 8.0 - command_msg.limit_vel_angular = 4.0 + command_msg.limit_acc_linear = 1.5 + command_msg.limit_vel_linear = 2.0 + command_msg.limit_acc_angular = 2.0 + command_msg.limit_vel_angular = 2.0 command_pub.publish(command_msg) @@ -73,9 +87,9 @@ def is_at_waypoint(index: int): _, _, theta = quat2euler([q.w, q.x, q.y, q.z]) return ( vision_robot_state_msg.visible - and abs(vision_robot_state_msg.pose.position.x - waypoint[0]) < linear_threshold - and abs(vision_robot_state_msg.pose.position.y - waypoint[1]) < linear_threshold - and abs(theta - waypoint[2]) < angular_threshold + and abs(vision_robot_state_msg.pose.position.x - waypoint.x) < linear_threshold + and abs(vision_robot_state_msg.pose.position.y - waypoint.y) < linear_threshold + and abs(shortest_angular_distance(theta, waypoint.theta)) < angular_threshold ) @@ -116,7 +130,7 @@ def is_at_waypoint(index: int): if is_at_waypoint(current_index): if waypoint_hold_start_time is None: waypoint_hold_start_time = time.time() - elif (time.time() - waypoint_hold_start_time) > waypoints[current_index][3]: + elif (time.time() - waypoint_hold_start_time) > waypoints[current_index].hold_time: current_index = (current_index + 1) % len(waypoints) waypoint_hold_start_time = None node.get_logger().info(f'Moving to waypoint {current_index}')