From 53eb62f2010280eab6c773bc644788378de03323 Mon Sep 17 00:00:00 2001 From: Matthew Woodward Date: Tue, 16 Jun 2026 23:24:17 -0400 Subject: [PATCH 01/16] awaiting rust bindings --- .../CMakeLists.txt | 1 + .../src/message_conversions.cpp | 29 +++-- .../src/message_conversions.hpp | 3 +- .../src/robot_maneuvers.cpp | 100 ++++++++++++++++++ .../src/robot_maneuvers.hpp | 47 ++++++++ .../src/ssl_simulation_radio_bridge_node.cpp | 15 ++- 6 files changed, 187 insertions(+), 8 deletions(-) create mode 100644 radio/ateam_ssl_simulation_radio_bridge/src/robot_maneuvers.cpp create mode 100644 radio/ateam_ssl_simulation_radio_bridge/src/robot_maneuvers.hpp diff --git a/radio/ateam_ssl_simulation_radio_bridge/CMakeLists.txt b/radio/ateam_ssl_simulation_radio_bridge/CMakeLists.txt index b68d98e9c..b05372654 100644 --- a/radio/ateam_ssl_simulation_radio_bridge/CMakeLists.txt +++ b/radio/ateam_ssl_simulation_radio_bridge/CMakeLists.txt @@ -18,6 +18,7 @@ find_package(ssl_league_protobufs REQUIRED) add_library(${PROJECT_NAME} SHARED src/ssl_simulation_radio_bridge_node.cpp src/message_conversions.cpp + src/robot_maneuvers.cpp ) set_target_properties(${PROJECT_NAME} PROPERTIES CXX_STANDARD 20) ament_target_dependencies(${PROJECT_NAME} diff --git a/radio/ateam_ssl_simulation_radio_bridge/src/message_conversions.cpp b/radio/ateam_ssl_simulation_radio_bridge/src/message_conversions.cpp index 3d9b1c8c7..9e5f6a874 100644 --- a/radio/ateam_ssl_simulation_radio_bridge/src/message_conversions.cpp +++ b/radio/ateam_ssl_simulation_radio_bridge/src/message_conversions.cpp @@ -19,6 +19,7 @@ // THE SOFTWARE. #include "message_conversions.hpp" +#include "robot_maneuvers.hpp" #include #include #include @@ -52,13 +53,13 @@ double ReplaceNanWithZero(const double val, rclcpp::Logger logger) } RobotControl fromMsg( - const ateam_msgs::msg::RobotMotionCommand & ros_msg, int robot_id, + const ateam_msgs::msg::RobotMotionCommand & ros_msg, ateam_msgs::msg::GameStateRobot robot, rclcpp::Logger logger) { RobotControl robots_control; RobotCommand * proto_robot_command = robots_control.add_robot_commands(); - proto_robot_command->set_id(robot_id); + proto_robot_command->set_id(robot.id); proto_robot_command->set_dribbler_speed(ReplaceNanWithZero(9.5492968 * ros_msg.dribbler_speed, logger)); @@ -78,11 +79,27 @@ RobotControl fromMsg( } RobotMoveCommand * robot_move_command = proto_robot_command->mutable_move_command(); - MoveLocalVelocity * local_velocity_command = robot_move_command->mutable_local_velocity(); - local_velocity_command->set_forward(ReplaceNanWithZero(ros_msg.velocity.x, logger)); - local_velocity_command->set_left(ReplaceNanWithZero(ros_msg.velocity.y, logger)); - local_velocity_command->set_angular(ReplaceNanWithZero(ros_msg.velocity.theta, logger)); + switch(ros_msg.body_control_mode) { + case ateam_msgs::msg::RobotMotionCommand::BCM_OFF: + ateam_ssl_simulation_radio_bridge::robot_maneuvers::local_velocity_maneuver(robot_move_command, ros_msg); + break; + case ateam_msgs::msg::RobotMotionCommand::BCM_GLOBAL_POSITION: + ateam_ssl_simulation_radio_bridge::robot_maneuvers::global_position_maneuver(robot_move_command, ros_msg, robot); + break; + case ateam_msgs::msg::RobotMotionCommand::BCM_GLOBAL_VELOCITY: + ateam_ssl_simulation_radio_bridge::robot_maneuvers::global_velocity_maneuver(robot_move_command, ros_msg); + break; + case ateam_msgs::msg::RobotMotionCommand::BCM_LOCAL_VELOCITY: + ateam_ssl_simulation_radio_bridge::robot_maneuvers::local_velocity_maneuver(robot_move_command, ros_msg); + break; + case ateam_msgs::msg::RobotMotionCommand::BCM_GLOBAL_ACCEL: + ateam_ssl_simulation_radio_bridge::robot_maneuvers::global_acceleration_maneuver(robot_move_command, ros_msg, robot); + break; + case ateam_msgs::msg::RobotMotionCommand::BCM_LOCAL_ACCEL: + ateam_ssl_simulation_radio_bridge::robot_maneuvers::local_acceleration_maneuver(robot_move_command, ros_msg, robot); + break; + } return robots_control; } diff --git a/radio/ateam_ssl_simulation_radio_bridge/src/message_conversions.hpp b/radio/ateam_ssl_simulation_radio_bridge/src/message_conversions.hpp index 4cd2e3afb..7d379c44a 100644 --- a/radio/ateam_ssl_simulation_radio_bridge/src/message_conversions.hpp +++ b/radio/ateam_ssl_simulation_radio_bridge/src/message_conversions.hpp @@ -31,6 +31,7 @@ #include #include +#include namespace ateam_ssl_simulation_radio_bridge::message_conversions { @@ -38,7 +39,7 @@ namespace ateam_ssl_simulation_radio_bridge::message_conversions ateam_radio_msgs::msg::BasicTelemetry fromProto(const RobotFeedback & proto_msg); RobotControl fromMsg( - const ateam_msgs::msg::RobotMotionCommand & ros_msg, int robot_id, + const ateam_msgs::msg::RobotMotionCommand & ros_msg, ateam_msgs::msg::GameStateRobot robot, rclcpp::Logger logger); SimulatorControl fromMsg(const ssl_league_msgs::msg::SimulatorControl & ros_msg); diff --git a/radio/ateam_ssl_simulation_radio_bridge/src/robot_maneuvers.cpp b/radio/ateam_ssl_simulation_radio_bridge/src/robot_maneuvers.cpp new file mode 100644 index 000000000..1b63aab16 --- /dev/null +++ b/radio/ateam_ssl_simulation_radio_bridge/src/robot_maneuvers.cpp @@ -0,0 +1,100 @@ +#include "robot_maneuvers.hpp" + +namespace ateam_ssl_simulation_radio_bridge::robot_maneuvers +{ + + void global_position_maneuver(RobotMoveCommand * robot_move_command, const ateam_msgs::msg::RobotMotionCommand & ros_msg, ateam_msgs::msg::GameStateRobot robot) { + // TODO (chachmu): finish implementing this once we have rust functions/types + // const Vector3f target_pose = Vector3f{ + // ros_msg.pose.x, + // ros_msg.pose.y, + // ros_msg.pose.theta + // } + + // const double robot_angle = from_quaternion( + // robot.pose.orientation.x, + // robot.pose.orientation.y, + // robot.pose.orientation.z, + // robot.pose.orientation.w, + // ) + + // const Vector6f cur_state = Vector6f{ + // robot.pose.position.x, + // robot.pose.position.y, + // robot_angle, + // robot.twist.linear.x, + // robot.twist.linear.y, + // robot.twist.angular.z + // } + + // TrajectoryParams traj_params = generate_trajectory_params(ros_msg); + // BangBangTraj3D traj = BangBangTraj3D::from_target_pose(cur_state, target_pose, traj_params); + + // Vector6f traj_command = traj.state_at(cur_state, 0.0, 0.0); + + // MoveGlobalVelocity * global_velocity_command = robot_move_command->mutable_global_velocity(); + + // global_velocity_command->set_x(traj_command[3]); + // global_velocity_command->set_y(traj_command[4]); + // global_velocity_command->set_angular(traj_command[5]); + } + + void global_velocity_maneuver(RobotMoveCommand * robot_move_command, const ateam_msgs::msg::RobotMotionCommand & ros_msg) { + MoveGlobalVelocity * global_velocity_command = robot_move_command->mutable_global_velocity(); + + global_velocity_command->set_x(ros_msg.velocity.x); + global_velocity_command->set_y(ros_msg.velocity.y); + global_velocity_command->set_angular(ros_msg.velocity.theta); + } + + void local_velocity_maneuver(RobotMoveCommand * robot_move_command, const ateam_msgs::msg::RobotMotionCommand & ros_msg) { + MoveLocalVelocity * local_velocity_command = robot_move_command->mutable_local_velocity(); + + local_velocity_command->set_forward(ros_msg.velocity.x); + local_velocity_command->set_left(ros_msg.velocity.y); + local_velocity_command->set_angular(ros_msg.velocity.theta); + } + + void global_acceleration_maneuver(RobotMoveCommand * robot_move_command, const ateam_msgs::msg::RobotMotionCommand & ros_msg, ateam_msgs::msg::GameStateRobot robot) { + MoveGlobalVelocity * global_velocity_command = robot_move_command->mutable_global_velocity(); + + global_velocity_command->set_x(robot.velocity.linear.x + 0.01 * ros_msg.acceleration.x); + global_velocity_command->set_y(robot.velocity.linear.y + 0.01 * ros_msg.acceleration.y); + global_velocity_command->set_angular(robot.velocity.angular.z + 0.01 * ros_msg.acceleration.theta); + } + + void local_acceleration_maneuver(RobotMoveCommand * robot_move_command, const ateam_msgs::msg::RobotMotionCommand & ros_msg, ateam_msgs::msg::GameStateRobot robot) { + // TODO (chachmu): handle converting to local robot velocity + MoveLocalVelocity * local_velocity_command = robot_move_command->mutable_local_velocity(); + + local_velocity_command->set_forward(robot.velocity.linear.x + 0.01 * ros_msg.acceleration.x); + local_velocity_command->set_left(robot.velocity.linear.y + 0.01 * ros_msg.acceleration.y); + local_velocity_command->set_angular(robot.velocity.angular.z + 0.01 * ros_msg.acceleration.theta); + } + + // TrajectoryParams generate_trajectory_params(const ateam_msgs::msg::RobotMotionCommand & ros_msg) { + // TODO (chachmu): finish implementing this once we have rust functions/types + // let traj_params = TrajectoryParams { + // max_vel_linear: if cmd.max_linear_vel != 0.0 { + // cmd.max_linear_vel + // } else { + // default_params.max_vel_linear + // }, + // max_vel_angular: if cmd.max_angular_vel != 0.0 { + // cmd.max_angular_vel + // } else { + // default_params.max_vel_angular + // }, + // max_accel_linear: if cmd.max_linear_acc != 0.0 { + // cmd.max_linear_acc + // } else { + // default_params.max_accel_linear + // }, + // max_accel_angular: if cmd.max_angular_acc != 0.0 { + // cmd.max_angular_acc + // } else { + // default_params.max_accel_angular + // }, + // }; + // } +} // namespace ateam_ssl_simulation_radio_bridge::robot_maneuvers \ No newline at end of file diff --git a/radio/ateam_ssl_simulation_radio_bridge/src/robot_maneuvers.hpp b/radio/ateam_ssl_simulation_radio_bridge/src/robot_maneuvers.hpp new file mode 100644 index 000000000..1f3e4a70f --- /dev/null +++ b/radio/ateam_ssl_simulation_radio_bridge/src/robot_maneuvers.hpp @@ -0,0 +1,47 @@ +// 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. + +#ifndef ROBOT_MANEUVERS_HPP_ +#define ROBOT_MANEUVERS_HPP_ + +#include +#include + +#include +#include +#include +#include + +namespace ateam_ssl_simulation_radio_bridge::robot_maneuvers +{ + void global_position_maneuver(RobotMoveCommand * robot_move_command, const ateam_msgs::msg::RobotMotionCommand & ros_msg, ateam_msgs::msg::GameStateRobot robot); + + void global_velocity_maneuver(RobotMoveCommand * robot_move_command, const ateam_msgs::msg::RobotMotionCommand & ros_msg); + void local_velocity_maneuver(RobotMoveCommand * robot_move_command, const ateam_msgs::msg::RobotMotionCommand & ros_msg); + + void global_acceleration_maneuver(RobotMoveCommand * robot_move_command, const ateam_msgs::msg::RobotMotionCommand & ros_msg, ateam_msgs::msg::GameStateRobot robot); + void local_acceleration_maneuver(RobotMoveCommand * robot_move_command, const ateam_msgs::msg::RobotMotionCommand & ros_msg, ateam_msgs::msg::GameStateRobot robot); + + // TrajectoryParams generate_trajectory_params(const ateam_msgs::msg::RobotMotionCommand & ros_msg); + + +} // namespace ateam_ssl_simulation_radio_bridge::robot_maneuvers + +#endif // ROBOT_MANEUVERS_HPP_ diff --git a/radio/ateam_ssl_simulation_radio_bridge/src/ssl_simulation_radio_bridge_node.cpp b/radio/ateam_ssl_simulation_radio_bridge/src/ssl_simulation_radio_bridge_node.cpp index 989bae0e5..aa86916db 100644 --- a/radio/ateam_ssl_simulation_radio_bridge/src/ssl_simulation_radio_bridge_node.cpp +++ b/radio/ateam_ssl_simulation_radio_bridge/src/ssl_simulation_radio_bridge_node.cpp @@ -38,6 +38,7 @@ #include #include #include +#include #include #include "message_conversions.hpp" @@ -85,6 +86,10 @@ class SSLSimulationRadioBridgeNode : public rclcpp::Node rclcpp::SystemDefaultsQoS(), this); + world_subscription_ = create_subscription( + std::string(Topics::kWorld), rclcpp::SystemDefaultsQoS(), + std::bind(&SSLSimulationRadioBridgeNode::world_callback, this, std::placeholders::_1)); + send_simulator_control_service_ = create_service("~/send_simulator_control_packet", std::bind(&SSLSimulationRadioBridgeNode::handle_send_simulator_control, this, @@ -160,12 +165,18 @@ class SSLSimulationRadioBridgeNode : public rclcpp::Node std::bind_front(&SSLSimulationRadioBridgeNode::feedback_callback, this)); } + void world_callback(const ateam_msgs::msg::GameStateWorld & msg) { + world_ = msg; + } + void send_command(const ateam_msgs::msg::RobotMotionCommand & msg, const int robot_id) { if (!udp_robot_control_) { return; } - RobotControl robots_control = message_conversions::fromMsg(msg, robot_id, get_logger()); + + const auto robot = world_.our_robots[robot_id]; + RobotControl robots_control = message_conversions::fromMsg(msg, robot, get_logger()); std::vector buffer; buffer.resize(robots_control.ByteSizeLong()); if (robots_control.SerializeToArray(buffer.data(), buffer.size())) { @@ -237,6 +248,8 @@ class SSLSimulationRadioBridgeNode : public rclcpp::Node 16> feedback_publishers_; std::array::SharedPtr, 16> connection_publishers_; + rclcpp::Subscription::SharedPtr world_subscription_; + ateam_msgs::msg::GameStateWorld world_; rclcpp::Service::SharedPtr send_simulator_control_service_; rclcpp::TimerBase::SharedPtr zero_command_timer_; From 849188ec3998ed146860bc03776d6d18f4ae8513 Mon Sep 17 00:00:00 2001 From: Matthew Barulic Date: Tue, 16 Jun 2026 23:39:28 -0400 Subject: [PATCH 02/16] Adds dependency install scrips. --- dep_scripts/apt.bash | 14 ++++++++++++++ dep_scripts/erforce_simulator.bash | 30 ++++++++++++++++++++++++++++++ dep_scripts/ros.bash | 25 +++++++++++++++++++++++++ dep_scripts/rosdep.bash | 10 ++++++++++ dep_scripts/rust.bash | 14 ++++++++++++++ install_dependencies.bash | 12 ++++++++++++ 6 files changed, 105 insertions(+) create mode 100755 dep_scripts/apt.bash create mode 100755 dep_scripts/erforce_simulator.bash create mode 100755 dep_scripts/ros.bash create mode 100755 dep_scripts/rosdep.bash create mode 100755 dep_scripts/rust.bash create mode 100755 install_dependencies.bash diff --git a/dep_scripts/apt.bash b/dep_scripts/apt.bash new file mode 100755 index 000000000..1d78f2ef9 --- /dev/null +++ b/dep_scripts/apt.bash @@ -0,0 +1,14 @@ +#! /usr/bin/env bash + +# Prefer using rosdep for dependencies available via that tool + +set -e + +echo "Installing APT dependencies..." + +sudo apt update +sudo apt upgrade + +sudo apt install -y python3-clang net-tools + +echo "APT dependencies installed." diff --git a/dep_scripts/erforce_simulator.bash b/dep_scripts/erforce_simulator.bash new file mode 100755 index 000000000..7cd4cb5f0 --- /dev/null +++ b/dep_scripts/erforce_simulator.bash @@ -0,0 +1,30 @@ +#! /usr/bin/env bash + +set -e + +if [ -n "$SIMULATORCLI_PATH" ]; then + echo "ER-Force Simulator already installed." + exit 0 +fi + +echo "Installing ER-Force Simulator..." + +SCRIPT_DIR=$(cd -- "$(dirname -- "${BASH_SOURCE[0]}")" &> /dev/null && pwd) + +ERFORCE_DIR=$SCRIPT_DIR/../../../../erforce +mkdir $ERFORCE_DIR +pushd $ERFORCE_DIR +git clone https://github.com/robotics-erlangen/framework.git +sudo apt install -y cmake protobuf-compiler libprotobuf-dev qt6-base-dev libqt6opengl6-dev g++ libusb-1.0-0-dev libsdl2-dev libqt6svg6-dev libssl-dev libglu1-mesa-dev +pushd framework +mkdir build +pushd build +cmake .. +cmake --build . --target project_bullet simulator-cli +echo $"export SIMULATORCLI_PATH=$ERFORCE_DIR/framework/build/bin/simulator-cli\n" >> ~/.bashrc +source ~/.bashrc +popd +popd +popd + +echo "ER-Force Simulator installed." diff --git a/dep_scripts/ros.bash b/dep_scripts/ros.bash new file mode 100755 index 000000000..5b67e63e4 --- /dev/null +++ b/dep_scripts/ros.bash @@ -0,0 +1,25 @@ +#! /usr/bin/env bash + +set -e + +if [ -f /opt/ros/jazzy/setup.bash ]; then + echo "ROS already installed." + exit 0 +fi + +echo "Installing ROS..." + +sudo apt install -y software-properties-common +sudo add-apt-repository universe + +sudo apt update && sudo apt install curl -y +export ROS_APT_SOURCE_VERSION=$(curl -s https://api.github.com/repos/ros-infrastructure/ros-apt-source/releases/latest | grep -F "tag_name" | awk -F'"' '{print $4}') +curl -L -o /tmp/ros2-apt-source.deb "https://github.com/ros-infrastructure/ros-apt-source/releases/download/${ROS_APT_SOURCE_VERSION}/ros2-apt-source_${ROS_APT_SOURCE_VERSION}.$(. /etc/os-release && echo ${UBUNTU_CODENAME:-${VERSION_CODENAME}})_all.deb" +sudo dpkg -i /tmp/ros2-apt-source.deb + +sudo apt update +sudo apt install -y ros-dev-tools + +sudo apt install ros-jazzy-desktop + +echo "ROS installed." diff --git a/dep_scripts/rosdep.bash b/dep_scripts/rosdep.bash new file mode 100755 index 000000000..21a167dd3 --- /dev/null +++ b/dep_scripts/rosdep.bash @@ -0,0 +1,10 @@ +#! /usr/bin/env bash + +set -e + +SCRIPT_DIR=$(cd -- "$(dirname -- "${BASH_SOURCE[0]}")" &> /dev/null && pwd) + +source /opt/ros/jazzy/setup.bash +pushd $SCRIPT_DIR/.. +rosdep install --from-paths . --ignore-src -y +popd diff --git a/dep_scripts/rust.bash b/dep_scripts/rust.bash new file mode 100755 index 000000000..38729d27c --- /dev/null +++ b/dep_scripts/rust.bash @@ -0,0 +1,14 @@ +#! /usr/bin/env bash + +set -e + +if command -v rustc &> /dev/null; then + echo "Rust already installed." + exit 0 +fi + +echo "Installing Rust..." + +curl --proto '=https' --tlsv1.2 -sSf https://sh.rustup.rs | sh + +echo "Rust installed." diff --git a/install_dependencies.bash b/install_dependencies.bash new file mode 100755 index 000000000..9d5c5a032 --- /dev/null +++ b/install_dependencies.bash @@ -0,0 +1,12 @@ +#! /usr/bin/env bash + +set -e + +SCRIPT_DIR=$(cd -- "$(dirname -- "${BASH_SOURCE[0]}")" &> /dev/null && pwd) + +$SCRIPT_DIR/dep_scripts/apt.bash +$SCRIPT_DIR/dep_scripts/ros.bash +$SCRIPT_DIR/dep_scripts/rosdep.bash +$SCRIPT_DIR/dep_scripts/rust.bash +$SCRIPT_DIR/ateam_ui/install_deps.sh +$SCRIPT_DIR/dep_scripts/erforce_simulator.bash From 62f7721ba672399d36ec8f9b8c96649936b3c2b9 Mon Sep 17 00:00:00 2001 From: Matthew Barulic Date: Tue, 16 Jun 2026 23:42:08 -0400 Subject: [PATCH 03/16] Adds ateam_controls package --- .gitmodules | 3 ++ motion/ateam_controls/CMakeLists.txt | 33 +++++++++++++++++++ motion/ateam_controls/LICENSE | 17 ++++++++++ motion/ateam_controls/controls | 1 + motion/ateam_controls/package.xml | 18 ++++++++++ .../scripts/waypoints_test.py | 4 +-- 6 files changed, 74 insertions(+), 2 deletions(-) create mode 100644 motion/ateam_controls/CMakeLists.txt create mode 100644 motion/ateam_controls/LICENSE create mode 160000 motion/ateam_controls/controls create mode 100644 motion/ateam_controls/package.xml diff --git a/.gitmodules b/.gitmodules index 08ea60349..1568efdea 100644 --- a/.gitmodules +++ b/.gitmodules @@ -4,3 +4,6 @@ [submodule "radio/ateam_radio_msgs/software-communication"] path = radio/ateam_radio_msgs/software-communication url = https://github.com/SSL-A-Team/software-communication.git +[submodule "motion/ateam_controls/controls"] + path = motion/ateam_controls/controls + url = https://github.com/SSL-A-Team/controls.git diff --git a/motion/ateam_controls/CMakeLists.txt b/motion/ateam_controls/CMakeLists.txt new file mode 100644 index 000000000..6f938cd28 --- /dev/null +++ b/motion/ateam_controls/CMakeLists.txt @@ -0,0 +1,33 @@ +cmake_minimum_required(VERSION 3.8) +project(ateam_controls) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) + +set(RUST_TARGET_DIR ${CMAKE_CURRENT_SOURCE_DIR}/controls/target) + +if((CMAKE_BUILD_TYPE STREQUAL "Release") OR (CMAKE_BUILD_TYPE STREQUAL "RelWithDebInfo")) + set(IMPORTED_LIB_PATH ${RUST_TARGET_DIR}/release/libateam_controls_c.a) +else() + set(IMPORTED_LIB_PATH ${RUST_TARGET_DIR}/debug/libateam_controls_c.a) +endif() + +add_subdirectory(controls) + + +install( + DIRECTORY controls/ateam-controls-c/include/ + DESTINATION include +) +install( + FILES $ + DESTINATION lib +) + +ament_export_libraries(-l${CMAKE_INSTALL_PREFIX}/lib/libateam_controls_c.a) +ament_export_include_directories(${CMAKE_INSTALL_PREFIX}/include) + +ament_package() diff --git a/motion/ateam_controls/LICENSE b/motion/ateam_controls/LICENSE new file mode 100644 index 000000000..30e8e2ece --- /dev/null +++ b/motion/ateam_controls/LICENSE @@ -0,0 +1,17 @@ +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. diff --git a/motion/ateam_controls/controls b/motion/ateam_controls/controls new file mode 160000 index 000000000..03820d794 --- /dev/null +++ b/motion/ateam_controls/controls @@ -0,0 +1 @@ +Subproject commit 03820d7944b3a30ff235e10f0b0385056f043c19 diff --git a/motion/ateam_controls/package.xml b/motion/ateam_controls/package.xml new file mode 100644 index 000000000..2bf56fbb7 --- /dev/null +++ b/motion/ateam_controls/package.xml @@ -0,0 +1,18 @@ + + + + ateam_controls + 0.0.0 + A-Team on-robot controls library + Matthew Barulic + MIT + + ament_cmake + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/motion/ateam_controls_testing/scripts/waypoints_test.py b/motion/ateam_controls_testing/scripts/waypoints_test.py index c293b0716..db6f0e18f 100755 --- a/motion/ateam_controls_testing/scripts/waypoints_test.py +++ b/motion/ateam_controls_testing/scripts/waypoints_test.py @@ -59,8 +59,8 @@ def publish_waypoint_command(index: int): command_msg.pose.y = waypoint[1] command_msg.pose.theta = waypoint[2] command_msg.kick_request = RobotMotionCommand.KR_DISABLE - command_msg.limit_acc_linear = 4.0 - command_msg.limit_vel_linear = 2.0 + 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_pub.publish(command_msg) From 54d4d4a9d2212f0beb3a24d3e69e0e5001662c59 Mon Sep 17 00:00:00 2001 From: Matthew Barulic Date: Tue, 16 Jun 2026 23:50:26 -0400 Subject: [PATCH 04/16] Makes actions workflows use new dependency script --- .github/workflows/full_ci.yml | 7 +------ .github/workflows/pr_validation.yml | 7 +------ 2 files changed, 2 insertions(+), 12 deletions(-) diff --git a/.github/workflows/full_ci.yml b/.github/workflows/full_ci.yml index 66b66661b..5301665af 100644 --- a/.github/workflows/full_ci.yml +++ b/.github/workflows/full_ci.yml @@ -19,12 +19,7 @@ jobs: - name: Install Dependencies shell: bash run: | - source /opt/ros/jazzy/setup.bash - sudo apt-get update - rosdep update --rosdistro=jazzy - rosdep install --from-paths . --ignore-src -y - ./src/ateam_software/ateam_ui/install_deps.sh - sudo apt install -y python3-clang + ./src/software/install_dependencies.bash - name: Build shell: bash diff --git a/.github/workflows/pr_validation.yml b/.github/workflows/pr_validation.yml index 97e468aa1..94d0f81d6 100644 --- a/.github/workflows/pr_validation.yml +++ b/.github/workflows/pr_validation.yml @@ -18,12 +18,7 @@ jobs: - name: Install Dependencies shell: bash run: | - source /opt/ros/jazzy/setup.bash - sudo apt-get update - rosdep update --rosdistro=jazzy - rosdep install --from-paths . --ignore-src -y - ./src/ateam_software/ateam_ui/install_deps.sh - sudo apt install -y python3-clang + ./src/software/install_dependencies.bash - name: Get Changed Packages id: get-changed-packages From 084b7b0980acb9b4aa72caff1e491fc7d26bb000 Mon Sep 17 00:00:00 2001 From: Matthew Barulic Date: Tue, 16 Jun 2026 23:52:13 -0400 Subject: [PATCH 05/16] Fixes path typo in actions workflows --- .github/workflows/full_ci.yml | 2 +- .github/workflows/pr_validation.yml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/full_ci.yml b/.github/workflows/full_ci.yml index 5301665af..da17e9d86 100644 --- a/.github/workflows/full_ci.yml +++ b/.github/workflows/full_ci.yml @@ -19,7 +19,7 @@ jobs: - name: Install Dependencies shell: bash run: | - ./src/software/install_dependencies.bash + ./src/ateam_software/install_dependencies.bash - name: Build shell: bash diff --git a/.github/workflows/pr_validation.yml b/.github/workflows/pr_validation.yml index 94d0f81d6..bb1d19404 100644 --- a/.github/workflows/pr_validation.yml +++ b/.github/workflows/pr_validation.yml @@ -18,7 +18,7 @@ jobs: - name: Install Dependencies shell: bash run: | - ./src/software/install_dependencies.bash + ./src/ateam_software/install_dependencies.bash - name: Get Changed Packages id: get-changed-packages From b5b340f803929e8307855d401951e241ee44f3f4 Mon Sep 17 00:00:00 2001 From: Matthew Barulic Date: Tue, 16 Jun 2026 23:54:24 -0400 Subject: [PATCH 06/16] Adds yes flag to apt upgrade --- dep_scripts/apt.bash | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/dep_scripts/apt.bash b/dep_scripts/apt.bash index 1d78f2ef9..b5db9278c 100755 --- a/dep_scripts/apt.bash +++ b/dep_scripts/apt.bash @@ -7,7 +7,7 @@ set -e echo "Installing APT dependencies..." sudo apt update -sudo apt upgrade +sudo apt upgrade -y sudo apt install -y python3-clang net-tools From 95eacf98b510fce95b966e4a6652adb05a641e6f Mon Sep 17 00:00:00 2001 From: Matthew Barulic Date: Tue, 16 Jun 2026 23:56:31 -0400 Subject: [PATCH 07/16] Adds rosdep update step --- dep_scripts/rosdep.bash | 1 + 1 file changed, 1 insertion(+) diff --git a/dep_scripts/rosdep.bash b/dep_scripts/rosdep.bash index 21a167dd3..794e7babc 100755 --- a/dep_scripts/rosdep.bash +++ b/dep_scripts/rosdep.bash @@ -6,5 +6,6 @@ SCRIPT_DIR=$(cd -- "$(dirname -- "${BASH_SOURCE[0]}")" &> /dev/null && pwd) source /opt/ros/jazzy/setup.bash pushd $SCRIPT_DIR/.. +rosdep update --rosdistro=jazzy rosdep install --from-paths . --ignore-src -y popd From 861dbf2ca108b17ad0cbad07d5e18321bbf8c872 Mon Sep 17 00:00:00 2001 From: Matthew Barulic Date: Wed, 17 Jun 2026 00:01:35 -0400 Subject: [PATCH 08/16] Makes rust installer noninteractive --- dep_scripts/rust.bash | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/dep_scripts/rust.bash b/dep_scripts/rust.bash index 38729d27c..fbe87e204 100755 --- a/dep_scripts/rust.bash +++ b/dep_scripts/rust.bash @@ -9,6 +9,6 @@ fi echo "Installing Rust..." -curl --proto '=https' --tlsv1.2 -sSf https://sh.rustup.rs | sh +curl --proto '=https' --tlsv1.2 -sSf https://sh.rustup.rs | sh -s -- -y echo "Rust installed." From 0b95105f9982da4fa330b72ca04f8033ccdfbd0e Mon Sep 17 00:00:00 2001 From: Matthew Barulic Date: Wed, 17 Jun 2026 00:10:08 -0400 Subject: [PATCH 09/16] Gates sim install behind flag --- install_dependencies.bash | 24 +++++++++++++++++++++++- 1 file changed, 23 insertions(+), 1 deletion(-) diff --git a/install_dependencies.bash b/install_dependencies.bash index 9d5c5a032..94c5d0507 100755 --- a/install_dependencies.bash +++ b/install_dependencies.bash @@ -1,12 +1,34 @@ #! /usr/bin/env bash +# Run this script to install all dependencies for our software. +# +# Options: +# -s: Install simulator dependencies (erforce_simulator) + + set -e SCRIPT_DIR=$(cd -- "$(dirname -- "${BASH_SOURCE[0]}")" &> /dev/null && pwd) +INSTALL_SIM="false" + +while getopts ":s" opt; do + case $opt in + s) + INSTALL_SIM="true" + ;; + \?) + echo "Invalid option: -$OPTARG" >&2 + ;; + esac +done + $SCRIPT_DIR/dep_scripts/apt.bash $SCRIPT_DIR/dep_scripts/ros.bash $SCRIPT_DIR/dep_scripts/rosdep.bash $SCRIPT_DIR/dep_scripts/rust.bash $SCRIPT_DIR/ateam_ui/install_deps.sh -$SCRIPT_DIR/dep_scripts/erforce_simulator.bash + +if [ "$INSTALL_SIM" = "true" ]; then + $SCRIPT_DIR/dep_scripts/erforce_simulator.bash +fi From 637a3f4f892d1c037e9af59f0f42b60322c97e31 Mon Sep 17 00:00:00 2001 From: Matthew Barulic Date: Wed, 17 Jun 2026 00:19:56 -0400 Subject: [PATCH 10/16] Adds rust install environment update step --- dep_scripts/rust.bash | 1 + 1 file changed, 1 insertion(+) diff --git a/dep_scripts/rust.bash b/dep_scripts/rust.bash index fbe87e204..511b742a7 100755 --- a/dep_scripts/rust.bash +++ b/dep_scripts/rust.bash @@ -10,5 +10,6 @@ fi echo "Installing Rust..." curl --proto '=https' --tlsv1.2 -sSf https://sh.rustup.rs | sh -s -- -y +source $HOME/.cargo/env echo "Rust installed." From 6aa60f7fc6bdfabf073ad2f46a0b18456bde28b1 Mon Sep 17 00:00:00 2001 From: Matthew Barulic Date: Wed, 17 Jun 2026 00:47:32 -0400 Subject: [PATCH 11/16] Fixes controls library exporting --- motion/ateam_controls/CMakeLists.txt | 13 ++++--------- 1 file changed, 4 insertions(+), 9 deletions(-) diff --git a/motion/ateam_controls/CMakeLists.txt b/motion/ateam_controls/CMakeLists.txt index 6f938cd28..709f3612c 100644 --- a/motion/ateam_controls/CMakeLists.txt +++ b/motion/ateam_controls/CMakeLists.txt @@ -7,14 +7,6 @@ endif() find_package(ament_cmake REQUIRED) -set(RUST_TARGET_DIR ${CMAKE_CURRENT_SOURCE_DIR}/controls/target) - -if((CMAKE_BUILD_TYPE STREQUAL "Release") OR (CMAKE_BUILD_TYPE STREQUAL "RelWithDebInfo")) - set(IMPORTED_LIB_PATH ${RUST_TARGET_DIR}/release/libateam_controls_c.a) -else() - set(IMPORTED_LIB_PATH ${RUST_TARGET_DIR}/debug/libateam_controls_c.a) -endif() - add_subdirectory(controls) @@ -27,7 +19,10 @@ install( DESTINATION lib ) -ament_export_libraries(-l${CMAKE_INSTALL_PREFIX}/lib/libateam_controls_c.a) + +_ament_cmake_export_libraries_register_environment_hook() +_ament_cmake_export_libraries_register_package_hook() +list(APPEND _AMENT_EXPORT_LIBRARIES ${CMAKE_INSTALL_PREFIX}/lib/libateam_controls_c.a) ament_export_include_directories(${CMAKE_INSTALL_PREFIX}/include) ament_package() From 7f755f95a47b785f0561949e2ad88bc0bd39abe4 Mon Sep 17 00:00:00 2001 From: Matthew Woodward Date: Wed, 17 Jun 2026 01:11:39 -0400 Subject: [PATCH 12/16] Works but the robot is drunk --- .../launch/bringup_simulation.launch.py | 14 +- .../CMakeLists.txt | 2 + .../package.xml | 1 + .../src/robot_maneuvers.cpp | 191 ++++++++++-------- .../src/robot_maneuvers.hpp | 4 +- 5 files changed, 120 insertions(+), 92 deletions(-) diff --git a/ateam_bringup/launch/bringup_simulation.launch.py b/ateam_bringup/launch/bringup_simulation.launch.py index f565118b7..dd113d445 100644 --- a/ateam_bringup/launch/bringup_simulation.launch.py +++ b/ateam_bringup/launch/bringup_simulation.launch.py @@ -100,11 +100,11 @@ def generate_launch_description(): }.items() ), - IncludeLaunchDescription( - FrontendLaunchDescriptionSource( - PackageLaunchFileSubstitution( - 'ateam_bringup', 'kenobi.launch.xml' - ) - ) - ), + # IncludeLaunchDescription( + # FrontendLaunchDescriptionSource( + # PackageLaunchFileSubstitution( + # 'ateam_bringup', 'kenobi.launch.xml' + # ) + # ) + # ), ]) diff --git a/radio/ateam_ssl_simulation_radio_bridge/CMakeLists.txt b/radio/ateam_ssl_simulation_radio_bridge/CMakeLists.txt index b05372654..beecc5bfe 100644 --- a/radio/ateam_ssl_simulation_radio_bridge/CMakeLists.txt +++ b/radio/ateam_ssl_simulation_radio_bridge/CMakeLists.txt @@ -11,6 +11,7 @@ find_package(rclcpp_components REQUIRED) find_package(ateam_common REQUIRED) find_package(ateam_msgs REQUIRED) find_package(ateam_radio_msgs REQUIRED) +find_package(ateam_controls REQUIRED) find_package(tf2 REQUIRED) find_package(tf2_geometry_msgs REQUIRED) find_package(ssl_league_protobufs REQUIRED) @@ -27,6 +28,7 @@ ament_target_dependencies(${PROJECT_NAME} ateam_common ateam_msgs ateam_radio_msgs + ateam_controls tf2 tf2_geometry_msgs ssl_league_protobufs diff --git a/radio/ateam_ssl_simulation_radio_bridge/package.xml b/radio/ateam_ssl_simulation_radio_bridge/package.xml index 12ebcc556..cbf416fd6 100644 --- a/radio/ateam_ssl_simulation_radio_bridge/package.xml +++ b/radio/ateam_ssl_simulation_radio_bridge/package.xml @@ -14,6 +14,7 @@ ateam_common ateam_msgs ateam_radio_msgs + ateam_controls tf2 tf2_geometry_msgs ssl_league_protobufs diff --git a/radio/ateam_ssl_simulation_radio_bridge/src/robot_maneuvers.cpp b/radio/ateam_ssl_simulation_radio_bridge/src/robot_maneuvers.cpp index 1b63aab16..bfcc3b4cc 100644 --- a/radio/ateam_ssl_simulation_radio_bridge/src/robot_maneuvers.cpp +++ b/radio/ateam_ssl_simulation_radio_bridge/src/robot_maneuvers.cpp @@ -1,100 +1,123 @@ #include "robot_maneuvers.hpp" +#include namespace ateam_ssl_simulation_radio_bridge::robot_maneuvers { - void global_position_maneuver(RobotMoveCommand * robot_move_command, const ateam_msgs::msg::RobotMotionCommand & ros_msg, ateam_msgs::msg::GameStateRobot robot) { - // TODO (chachmu): finish implementing this once we have rust functions/types - // const Vector3f target_pose = Vector3f{ - // ros_msg.pose.x, - // ros_msg.pose.y, - // ros_msg.pose.theta - // } - - // const double robot_angle = from_quaternion( - // robot.pose.orientation.x, - // robot.pose.orientation.y, - // robot.pose.orientation.z, - // robot.pose.orientation.w, - // ) - - // const Vector6f cur_state = Vector6f{ - // robot.pose.position.x, - // robot.pose.position.y, - // robot_angle, - // robot.twist.linear.x, - // robot.twist.linear.y, - // robot.twist.angular.z - // } - - // TrajectoryParams traj_params = generate_trajectory_params(ros_msg); - // BangBangTraj3D traj = BangBangTraj3D::from_target_pose(cur_state, target_pose, traj_params); - - // Vector6f traj_command = traj.state_at(cur_state, 0.0, 0.0); - - // MoveGlobalVelocity * global_velocity_command = robot_move_command->mutable_global_velocity(); - - // global_velocity_command->set_x(traj_command[3]); - // global_velocity_command->set_y(traj_command[4]); - // global_velocity_command->set_angular(traj_command[5]); - } + void global_position_maneuver(RobotMoveCommand * robot_move_command, const ateam_msgs::msg::RobotMotionCommand & ros_msg, ateam_msgs::msg::GameStateRobot robot) { + const Vector3C_t target_pose{ + float(ros_msg.pose.x), + float(ros_msg.pose.y), + float(ros_msg.pose.theta) + }; + + tf2::Quaternion quat; + tf2::fromMsg(robot.pose.orientation, quat); + double yaw, pitch, roll; + tf2::Matrix3x3(quat).getEulerYPR(yaw, pitch, roll); - void global_velocity_maneuver(RobotMoveCommand * robot_move_command, const ateam_msgs::msg::RobotMotionCommand & ros_msg) { - MoveGlobalVelocity * global_velocity_command = robot_move_command->mutable_global_velocity(); - global_velocity_command->set_x(ros_msg.velocity.x); - global_velocity_command->set_y(ros_msg.velocity.y); - global_velocity_command->set_angular(ros_msg.velocity.theta); - } + const double local_x_pos = cos(-yaw) * robot.pose.position.x - sin(-yaw) * robot.pose.position.y; + const double local_y_pos = sin(-yaw) * robot.pose.position.x + cos(-yaw) * robot.pose.position.y; + const double local_x_vel = cos(-yaw) * robot.velocity.linear.x - sin(-yaw) * robot.velocity.linear.y; + const double local_y_vel = sin(-yaw) * robot.velocity.linear.x + cos(-yaw) * robot.velocity.linear.y; - void local_velocity_maneuver(RobotMoveCommand * robot_move_command, const ateam_msgs::msg::RobotMotionCommand & ros_msg) { - MoveLocalVelocity * local_velocity_command = robot_move_command->mutable_local_velocity(); + const Vector6C_t curr_state{ + float(local_x_pos), + float(local_y_pos), + float(yaw), + float(local_x_vel), + float(local_y_vel), + float(robot.velocity.angular.z) + }; + + TrajectoryParams_t traj_params = generate_trajectory_params(ros_msg); - local_velocity_command->set_forward(ros_msg.velocity.x); - local_velocity_command->set_left(ros_msg.velocity.y); - local_velocity_command->set_angular(ros_msg.velocity.theta); - } + BangBangTraj3D_t traj; + ateam_controls_traj_from_target_pose( + curr_state, + target_pose, + traj_params, + &traj + ); - void global_acceleration_maneuver(RobotMoveCommand * robot_move_command, const ateam_msgs::msg::RobotMotionCommand & ros_msg, ateam_msgs::msg::GameStateRobot robot) { - MoveGlobalVelocity * global_velocity_command = robot_move_command->mutable_global_velocity(); + Vector6C_t traj_command; + ateam_controls_traj_state_at( + traj, + curr_state, + 0.0f, + 0.05f, + &traj_command + ); - global_velocity_command->set_x(robot.velocity.linear.x + 0.01 * ros_msg.acceleration.x); - global_velocity_command->set_y(robot.velocity.linear.y + 0.01 * ros_msg.acceleration.y); - global_velocity_command->set_angular(robot.velocity.angular.z + 0.01 * ros_msg.acceleration.theta); - } + MoveLocalVelocity * local_velocity_command = robot_move_command->mutable_local_velocity(); + + local_velocity_command->set_forward(traj_command.data[3]); + local_velocity_command->set_left(traj_command.data[4]); + local_velocity_command->set_angular(traj_command.data[5]); + } + + void global_velocity_maneuver(RobotMoveCommand * robot_move_command, const ateam_msgs::msg::RobotMotionCommand & ros_msg) { + MoveLocalVelocity * local_velocity_command = robot_move_command->mutable_local_velocity(); + + local_velocity_command->set_forward(ros_msg.velocity.x); + local_velocity_command->set_left(ros_msg.velocity.y); + local_velocity_command->set_angular(ros_msg.velocity.theta); + } + + void local_velocity_maneuver(RobotMoveCommand * robot_move_command, const ateam_msgs::msg::RobotMotionCommand & ros_msg) { + MoveLocalVelocity * local_velocity_command = robot_move_command->mutable_local_velocity(); + + local_velocity_command->set_forward(ros_msg.velocity.x); + local_velocity_command->set_left(ros_msg.velocity.y); + local_velocity_command->set_angular(ros_msg.velocity.theta); + } + + void global_acceleration_maneuver(RobotMoveCommand * robot_move_command, const ateam_msgs::msg::RobotMotionCommand & ros_msg, ateam_msgs::msg::GameStateRobot robot) { + MoveLocalVelocity * local_velocity_command = robot_move_command->mutable_local_velocity(); + const double dt = 1.0 / 100.0; - void local_acceleration_maneuver(RobotMoveCommand * robot_move_command, const ateam_msgs::msg::RobotMotionCommand & ros_msg, ateam_msgs::msg::GameStateRobot robot) { - // TODO (chachmu): handle converting to local robot velocity - MoveLocalVelocity * local_velocity_command = robot_move_command->mutable_local_velocity(); + local_velocity_command->set_forward(robot.velocity.linear.x + dt * ros_msg.acceleration.x); + local_velocity_command->set_left(robot.velocity.linear.y + dt * ros_msg.acceleration.y); + local_velocity_command->set_angular(robot.velocity.angular.z + dt * ros_msg.acceleration.theta); + } - local_velocity_command->set_forward(robot.velocity.linear.x + 0.01 * ros_msg.acceleration.x); - local_velocity_command->set_left(robot.velocity.linear.y + 0.01 * ros_msg.acceleration.y); - local_velocity_command->set_angular(robot.velocity.angular.z + 0.01 * ros_msg.acceleration.theta); + void local_acceleration_maneuver(RobotMoveCommand * robot_move_command, const ateam_msgs::msg::RobotMotionCommand & ros_msg, ateam_msgs::msg::GameStateRobot robot) { + + tf2::Quaternion quat; + tf2::fromMsg(robot.pose.orientation, quat); + double yaw, pitch, roll; + tf2::Matrix3x3(quat).getEulerYPR(yaw, pitch, roll); + + // Test this, I might have done it backwards + const double local_x_vel = cos(-yaw) * robot.velocity.linear.x - sin(-yaw) * robot.velocity.linear.y; + const double local_y_vel = sin(-yaw) * robot.velocity.linear.x + cos(-yaw) * robot.velocity.linear.y; + const double dt = 1.0 / 100.0; + + MoveLocalVelocity * local_velocity_command = robot_move_command->mutable_local_velocity(); + + local_velocity_command->set_forward(local_x_vel + dt * ros_msg.acceleration.x); + local_velocity_command->set_left(local_y_vel + dt * ros_msg.acceleration.y); + local_velocity_command->set_angular(robot.velocity.angular.z + dt * ros_msg.acceleration.theta); + } + + TrajectoryParams_t generate_trajectory_params(const ateam_msgs::msg::RobotMotionCommand & ros_msg) { + + TrajectoryParams_t traj_params = ateam_controls_default_traj_params(); + + if (ros_msg.limit_vel_linear != 0.0) { + traj_params.max_vel_linear = ros_msg.limit_vel_linear; + } + if (ros_msg.limit_vel_angular != 0.0) { + traj_params.max_vel_angular = ros_msg.limit_vel_angular; + } + if (ros_msg.limit_acc_linear != 0.0) { + traj_params.max_accel_linear = ros_msg.limit_acc_linear; + } + if (ros_msg.limit_acc_angular != 0.0) { + traj_params.max_accel_angular = ros_msg.limit_acc_angular; } - // TrajectoryParams generate_trajectory_params(const ateam_msgs::msg::RobotMotionCommand & ros_msg) { - // TODO (chachmu): finish implementing this once we have rust functions/types - // let traj_params = TrajectoryParams { - // max_vel_linear: if cmd.max_linear_vel != 0.0 { - // cmd.max_linear_vel - // } else { - // default_params.max_vel_linear - // }, - // max_vel_angular: if cmd.max_angular_vel != 0.0 { - // cmd.max_angular_vel - // } else { - // default_params.max_vel_angular - // }, - // max_accel_linear: if cmd.max_linear_acc != 0.0 { - // cmd.max_linear_acc - // } else { - // default_params.max_accel_linear - // }, - // max_accel_angular: if cmd.max_angular_acc != 0.0 { - // cmd.max_angular_acc - // } else { - // default_params.max_accel_angular - // }, - // }; - // } + return traj_params; + } } // namespace ateam_ssl_simulation_radio_bridge::robot_maneuvers \ No newline at end of file diff --git a/radio/ateam_ssl_simulation_radio_bridge/src/robot_maneuvers.hpp b/radio/ateam_ssl_simulation_radio_bridge/src/robot_maneuvers.hpp index 1f3e4a70f..ff5e0764a 100644 --- a/radio/ateam_ssl_simulation_radio_bridge/src/robot_maneuvers.hpp +++ b/radio/ateam_ssl_simulation_radio_bridge/src/robot_maneuvers.hpp @@ -29,6 +29,8 @@ #include #include +#include "ateam_controls/ateam_controls.h" + namespace ateam_ssl_simulation_radio_bridge::robot_maneuvers { void global_position_maneuver(RobotMoveCommand * robot_move_command, const ateam_msgs::msg::RobotMotionCommand & ros_msg, ateam_msgs::msg::GameStateRobot robot); @@ -39,7 +41,7 @@ namespace ateam_ssl_simulation_radio_bridge::robot_maneuvers void global_acceleration_maneuver(RobotMoveCommand * robot_move_command, const ateam_msgs::msg::RobotMotionCommand & ros_msg, ateam_msgs::msg::GameStateRobot robot); void local_acceleration_maneuver(RobotMoveCommand * robot_move_command, const ateam_msgs::msg::RobotMotionCommand & ros_msg, ateam_msgs::msg::GameStateRobot robot); - // TrajectoryParams generate_trajectory_params(const ateam_msgs::msg::RobotMotionCommand & ros_msg); + TrajectoryParams_t generate_trajectory_params(const ateam_msgs::msg::RobotMotionCommand & ros_msg); } // namespace ateam_ssl_simulation_radio_bridge::robot_maneuvers From ee49062517ba11647f75410b3acd5164a0c5665c Mon Sep 17 00:00:00 2001 From: Matthew Woodward Date: Wed, 17 Jun 2026 03:52:21 -0400 Subject: [PATCH 13/16] Fairly good position control, might do better with i and d --- .../src/message_conversions.cpp | 4 +- .../src/message_conversions.hpp | 4 +- .../src/robot_maneuvers.cpp | 123 ++++++++++++------ .../src/robot_maneuvers.hpp | 13 +- .../src/ssl_simulation_radio_bridge_node.cpp | 5 +- 5 files changed, 104 insertions(+), 45 deletions(-) diff --git a/radio/ateam_ssl_simulation_radio_bridge/src/message_conversions.cpp b/radio/ateam_ssl_simulation_radio_bridge/src/message_conversions.cpp index 9e5f6a874..f66f8cb50 100644 --- a/radio/ateam_ssl_simulation_radio_bridge/src/message_conversions.cpp +++ b/radio/ateam_ssl_simulation_radio_bridge/src/message_conversions.cpp @@ -54,7 +54,7 @@ double ReplaceNanWithZero(const double val, rclcpp::Logger logger) RobotControl fromMsg( const ateam_msgs::msg::RobotMotionCommand & ros_msg, ateam_msgs::msg::GameStateRobot robot, - rclcpp::Logger logger) + ateam_ssl_simulation_radio_bridge::robot_maneuvers::ManeuverInfo & maneuver_info, rclcpp::Logger logger) { RobotControl robots_control; RobotCommand * proto_robot_command = robots_control.add_robot_commands(); @@ -85,7 +85,7 @@ RobotControl fromMsg( ateam_ssl_simulation_radio_bridge::robot_maneuvers::local_velocity_maneuver(robot_move_command, ros_msg); break; case ateam_msgs::msg::RobotMotionCommand::BCM_GLOBAL_POSITION: - ateam_ssl_simulation_radio_bridge::robot_maneuvers::global_position_maneuver(robot_move_command, ros_msg, robot); + ateam_ssl_simulation_radio_bridge::robot_maneuvers::global_position_maneuver(robot_move_command, ros_msg, robot, maneuver_info); break; case ateam_msgs::msg::RobotMotionCommand::BCM_GLOBAL_VELOCITY: ateam_ssl_simulation_radio_bridge::robot_maneuvers::global_velocity_maneuver(robot_move_command, ros_msg); diff --git a/radio/ateam_ssl_simulation_radio_bridge/src/message_conversions.hpp b/radio/ateam_ssl_simulation_radio_bridge/src/message_conversions.hpp index 7d379c44a..826b019ba 100644 --- a/radio/ateam_ssl_simulation_radio_bridge/src/message_conversions.hpp +++ b/radio/ateam_ssl_simulation_radio_bridge/src/message_conversions.hpp @@ -33,6 +33,8 @@ #include #include +#include "robot_maneuvers.hpp" + namespace ateam_ssl_simulation_radio_bridge::message_conversions { @@ -40,7 +42,7 @@ ateam_radio_msgs::msg::BasicTelemetry fromProto(const RobotFeedback & proto_msg) RobotControl fromMsg( const ateam_msgs::msg::RobotMotionCommand & ros_msg, ateam_msgs::msg::GameStateRobot robot, - rclcpp::Logger logger); + ateam_ssl_simulation_radio_bridge::robot_maneuvers::ManeuverInfo & maneuver_info, rclcpp::Logger logger); SimulatorControl fromMsg(const ssl_league_msgs::msg::SimulatorControl & ros_msg); diff --git a/radio/ateam_ssl_simulation_radio_bridge/src/robot_maneuvers.cpp b/radio/ateam_ssl_simulation_radio_bridge/src/robot_maneuvers.cpp index bfcc3b4cc..4cf27f758 100644 --- a/radio/ateam_ssl_simulation_radio_bridge/src/robot_maneuvers.cpp +++ b/radio/ateam_ssl_simulation_radio_bridge/src/robot_maneuvers.cpp @@ -1,10 +1,11 @@ +#include #include "robot_maneuvers.hpp" #include namespace ateam_ssl_simulation_radio_bridge::robot_maneuvers { - void global_position_maneuver(RobotMoveCommand * robot_move_command, const ateam_msgs::msg::RobotMotionCommand & ros_msg, ateam_msgs::msg::GameStateRobot robot) { + void global_position_maneuver(RobotMoveCommand * robot_move_command, const ateam_msgs::msg::RobotMotionCommand & ros_msg, ateam_msgs::msg::GameStateRobot robot, ManeuverInfo & maneuver_info) { const Vector3C_t target_pose{ float(ros_msg.pose.x), float(ros_msg.pose.y), @@ -16,50 +17,103 @@ namespace ateam_ssl_simulation_radio_bridge::robot_maneuvers double yaw, pitch, roll; tf2::Matrix3x3(quat).getEulerYPR(yaw, pitch, roll); + Vector6C_t seed_state = maneuver_info.trajectory_state; + + // Check target point + double target_dist = std::pow(maneuver_info.target_pose.x - ros_msg.pose.x, 2) + std::pow(maneuver_info.target_pose.y - ros_msg.pose.y, 2); + // TODO (chachmu): fix angle wraparound issues and improve this check + bool target_pos_needs_replan = target_dist > 0.01 || abs(maneuver_info.target_pose.theta - ros_msg.pose.theta) > 0.01; + + // Check current position + double current_dist = std::pow(robot.pose.position.x - maneuver_info.trajectory_state.data[0], 2) + std::pow(robot.pose.position.y - maneuver_info.trajectory_state.data[1], 2); + // TODO (chachmu): fix angle wraparound issues and improve this check + bool current_pos_needs_replan = current_dist > 0.5 || abs(yaw - maneuver_info.trajectory_state.data[2]) > 0.2; + // current_pos_needs_replan = false; // REMOVE THIS + + bool maneuver_type_mismatch = maneuver_info.maneuver_type != ateam_msgs::msg::RobotMotionCommand::BCM_GLOBAL_POSITION; + + bool replanning = maneuver_type_mismatch || target_pos_needs_replan || current_pos_needs_replan; + if (replanning) { + // std::cerr << "REPLANNING: " << maneuver_type_mismatch << ", " << target_pos_needs_replan << ", " << current_pos_needs_replan << std::endl; + seed_state = Vector6C_t{ + float(robot.pose.position.x), + float(robot.pose.position.y), + float(yaw), + float(robot.velocity.linear.x), + float(robot.velocity.linear.y), + float(robot.velocity.angular.z) + }; + } - const double local_x_pos = cos(-yaw) * robot.pose.position.x - sin(-yaw) * robot.pose.position.y; - const double local_y_pos = sin(-yaw) * robot.pose.position.x + cos(-yaw) * robot.pose.position.y; - const double local_x_vel = cos(-yaw) * robot.velocity.linear.x - sin(-yaw) * robot.velocity.linear.y; - const double local_y_vel = sin(-yaw) * robot.velocity.linear.x + cos(-yaw) * robot.velocity.linear.y; - - const Vector6C_t curr_state{ - float(local_x_pos), - float(local_y_pos), - float(yaw), - float(local_x_vel), - float(local_y_vel), - float(robot.velocity.angular.z) - }; - TrajectoryParams_t traj_params = generate_trajectory_params(ros_msg); BangBangTraj3D_t traj; ateam_controls_traj_from_target_pose( - curr_state, + seed_state, target_pose, traj_params, &traj ); - Vector6C_t traj_command; + // TODO (chachmu): figure out how to safely calculate dt + std::chrono::duration elapsed = std::chrono::steady_clock::now() - maneuver_info.prev_update_time; + // float dt = elapsed.count(); + float dt = 0.01; + + // const double global_x_err = ros_msg.pose.x - robot.pose.position.x; + // const double global_y_err = ros_msg.pose.y - robot.pose.position.y; + const double global_x_err = seed_state.data[0] - robot.pose.position.x; + const double global_y_err = seed_state.data[1] - robot.pose.position.y; + const double global_theta_err = seed_state.data[2] - yaw; + + const double total_err = std::pow(global_x_err, 2) + std::pow(global_y_err, 2); + // std::cerr << "total_err: " << total_err << ", x: " << global_x_err << ", y: " << global_y_err << ", theta: " << global_theta_err << std::endl; + + const double kPos = 1.2; + const double kAngle = 1.0; + + const double global_x_vel = seed_state.data[3] + (kPos * global_x_err); + const double global_y_vel = seed_state.data[4] + (kPos * global_y_err); + const double theta_vel = seed_state.data[5] + (kAngle * global_theta_err); + + const double local_x_vel = cos(-yaw) * global_x_vel - sin(-yaw) * global_y_vel; + const double local_y_vel = sin(-yaw) * global_x_vel + cos(-yaw) * global_y_vel; + + MoveLocalVelocity * local_velocity_command = robot_move_command->mutable_local_velocity(); + + local_velocity_command->set_forward(local_x_vel); + local_velocity_command->set_left(local_y_vel); + local_velocity_command->set_angular(theta_vel); + + // Update the trajectory state for the next iteration ateam_controls_traj_state_at( traj, - curr_state, + seed_state, 0.0f, - 0.05f, - &traj_command + dt, + &maneuver_info.trajectory_state ); - MoveLocalVelocity * local_velocity_command = robot_move_command->mutable_local_velocity(); - - local_velocity_command->set_forward(traj_command.data[3]); - local_velocity_command->set_left(traj_command.data[4]); - local_velocity_command->set_angular(traj_command.data[5]); + // Only update the maneuver_info once we are sure we haven't errored out of using the maneuver + if (replanning) { + maneuver_info.target_pose = ros_msg.pose; + } + maneuver_info.maneuver_type = ateam_msgs::msg::RobotMotionCommand::BCM_GLOBAL_POSITION; + maneuver_info.trajectory = traj; + maneuver_info.prev_update_time = std::chrono::steady_clock::now(); } - void global_velocity_maneuver(RobotMoveCommand * robot_move_command, const ateam_msgs::msg::RobotMotionCommand & ros_msg) { + void global_velocity_maneuver(RobotMoveCommand * robot_move_command, const ateam_msgs::msg::RobotMotionCommand & ros_msg, ateam_msgs::msg::GameStateRobot robot) { MoveLocalVelocity * local_velocity_command = robot_move_command->mutable_local_velocity(); + tf2::Quaternion quat; + tf2::fromMsg(robot.pose.orientation, quat); + double yaw, pitch, roll; + tf2::Matrix3x3(quat).getEulerYPR(yaw, pitch, roll); + + const double local_x_vel = cos(-yaw) * ros_msg.velocity.x - sin(-yaw) * ros_msg.velocity.y; + const double local_y_vel = sin(-yaw) * ros_msg.velocity.x + cos(-yaw) * ros_msg.velocity.y; + local_velocity_command->set_forward(ros_msg.velocity.x); local_velocity_command->set_left(ros_msg.velocity.y); local_velocity_command->set_angular(ros_msg.velocity.theta); @@ -74,6 +128,7 @@ namespace ateam_ssl_simulation_radio_bridge::robot_maneuvers } void global_acceleration_maneuver(RobotMoveCommand * robot_move_command, const ateam_msgs::msg::RobotMotionCommand & ros_msg, ateam_msgs::msg::GameStateRobot robot) { + // TODO (chachmu): convert the coordinate frames MoveLocalVelocity * local_velocity_command = robot_move_command->mutable_local_velocity(); const double dt = 1.0 / 100.0; @@ -83,21 +138,11 @@ namespace ateam_ssl_simulation_radio_bridge::robot_maneuvers } void local_acceleration_maneuver(RobotMoveCommand * robot_move_command, const ateam_msgs::msg::RobotMotionCommand & ros_msg, ateam_msgs::msg::GameStateRobot robot) { - - tf2::Quaternion quat; - tf2::fromMsg(robot.pose.orientation, quat); - double yaw, pitch, roll; - tf2::Matrix3x3(quat).getEulerYPR(yaw, pitch, roll); - - // Test this, I might have done it backwards - const double local_x_vel = cos(-yaw) * robot.velocity.linear.x - sin(-yaw) * robot.velocity.linear.y; - const double local_y_vel = sin(-yaw) * robot.velocity.linear.x + cos(-yaw) * robot.velocity.linear.y; - const double dt = 1.0 / 100.0; - MoveLocalVelocity * local_velocity_command = robot_move_command->mutable_local_velocity(); + const double dt = 1.0 / 100.0; - local_velocity_command->set_forward(local_x_vel + dt * ros_msg.acceleration.x); - local_velocity_command->set_left(local_y_vel + dt * ros_msg.acceleration.y); + local_velocity_command->set_forward(robot.velocity.linear.x + dt * ros_msg.acceleration.x); + local_velocity_command->set_left(robot.velocity.linear.y + dt * ros_msg.acceleration.y); local_velocity_command->set_angular(robot.velocity.angular.z + dt * ros_msg.acceleration.theta); } diff --git a/radio/ateam_ssl_simulation_radio_bridge/src/robot_maneuvers.hpp b/radio/ateam_ssl_simulation_radio_bridge/src/robot_maneuvers.hpp index ff5e0764a..0529cdebe 100644 --- a/radio/ateam_ssl_simulation_radio_bridge/src/robot_maneuvers.hpp +++ b/radio/ateam_ssl_simulation_radio_bridge/src/robot_maneuvers.hpp @@ -21,6 +21,7 @@ #ifndef ROBOT_MANEUVERS_HPP_ #define ROBOT_MANEUVERS_HPP_ +#include #include #include @@ -33,9 +34,17 @@ namespace ateam_ssl_simulation_radio_bridge::robot_maneuvers { - void global_position_maneuver(RobotMoveCommand * robot_move_command, const ateam_msgs::msg::RobotMotionCommand & ros_msg, ateam_msgs::msg::GameStateRobot robot); + struct ManeuverInfo { + BangBangTraj3D_t trajectory; + Vector6C_t trajectory_state; + ateam_msgs::msg::Twist2D target_pose; + int32_t maneuver_type = 0; + std::chrono::steady_clock::time_point prev_update_time; + }; - void global_velocity_maneuver(RobotMoveCommand * robot_move_command, const ateam_msgs::msg::RobotMotionCommand & ros_msg); + void global_position_maneuver(RobotMoveCommand * robot_move_command, const ateam_msgs::msg::RobotMotionCommand & ros_msg, ateam_msgs::msg::GameStateRobot robot, ManeuverInfo & maneuver_info); + + void global_velocity_maneuver(RobotMoveCommand * robot_move_command, const ateam_msgs::msg::RobotMotionCommand & ros_msg, ateam_msgs::msg::GameStateRobot robot); void local_velocity_maneuver(RobotMoveCommand * robot_move_command, const ateam_msgs::msg::RobotMotionCommand & ros_msg); void global_acceleration_maneuver(RobotMoveCommand * robot_move_command, const ateam_msgs::msg::RobotMotionCommand & ros_msg, ateam_msgs::msg::GameStateRobot robot); diff --git a/radio/ateam_ssl_simulation_radio_bridge/src/ssl_simulation_radio_bridge_node.cpp b/radio/ateam_ssl_simulation_radio_bridge/src/ssl_simulation_radio_bridge_node.cpp index aa86916db..dd2c4d9a0 100644 --- a/radio/ateam_ssl_simulation_radio_bridge/src/ssl_simulation_radio_bridge_node.cpp +++ b/radio/ateam_ssl_simulation_radio_bridge/src/ssl_simulation_radio_bridge_node.cpp @@ -42,6 +42,7 @@ #include #include "message_conversions.hpp" +#include "robot_maneuvers.hpp" namespace ateam_ssl_simulation_radio_bridge { @@ -176,7 +177,8 @@ class SSLSimulationRadioBridgeNode : public rclcpp::Node } const auto robot = world_.our_robots[robot_id]; - RobotControl robots_control = message_conversions::fromMsg(msg, robot, get_logger()); + auto& maneuver_info = manuever_infos_[robot_id]; + RobotControl robots_control = message_conversions::fromMsg(msg, robot, maneuver_info, get_logger()); std::vector buffer; buffer.resize(robots_control.ByteSizeLong()); if (robots_control.SerializeToArray(buffer.data(), buffer.size())) { @@ -250,6 +252,7 @@ class SSLSimulationRadioBridgeNode : public rclcpp::Node 16> connection_publishers_; rclcpp::Subscription::SharedPtr world_subscription_; ateam_msgs::msg::GameStateWorld world_; + std::array manuever_infos_; rclcpp::Service::SharedPtr send_simulator_control_service_; rclcpp::TimerBase::SharedPtr zero_command_timer_; From aafa490a0eb603e339e905be6ccf6a937f57a186 Mon Sep 17 00:00:00 2001 From: Matthew Woodward Date: Sat, 20 Jun 2026 00:40:26 -0400 Subject: [PATCH 14/16] Mostly working for all modes. Needs pid or something for small position error --- .../launch/bringup_simulation.launch.py | 14 +- .../CMakeLists.txt | 2 + .../package.xml | 1 + .../src/message_conversions.cpp | 24 +- .../src/message_conversions.hpp | 2 +- .../src/robot_maneuvers.cpp | 261 +++++++++++++----- .../src/robot_maneuvers.hpp | 37 ++- .../src/ssl_simulation_radio_bridge_node.cpp | 19 +- 8 files changed, 252 insertions(+), 108 deletions(-) diff --git a/ateam_bringup/launch/bringup_simulation.launch.py b/ateam_bringup/launch/bringup_simulation.launch.py index dd113d445..f565118b7 100644 --- a/ateam_bringup/launch/bringup_simulation.launch.py +++ b/ateam_bringup/launch/bringup_simulation.launch.py @@ -100,11 +100,11 @@ def generate_launch_description(): }.items() ), - # IncludeLaunchDescription( - # FrontendLaunchDescriptionSource( - # PackageLaunchFileSubstitution( - # 'ateam_bringup', 'kenobi.launch.xml' - # ) - # ) - # ), + IncludeLaunchDescription( + FrontendLaunchDescriptionSource( + PackageLaunchFileSubstitution( + 'ateam_bringup', 'kenobi.launch.xml' + ) + ) + ), ]) diff --git a/radio/ateam_ssl_simulation_radio_bridge/CMakeLists.txt b/radio/ateam_ssl_simulation_radio_bridge/CMakeLists.txt index beecc5bfe..1f46aa082 100644 --- a/radio/ateam_ssl_simulation_radio_bridge/CMakeLists.txt +++ b/radio/ateam_ssl_simulation_radio_bridge/CMakeLists.txt @@ -12,6 +12,7 @@ find_package(ateam_common REQUIRED) find_package(ateam_msgs REQUIRED) find_package(ateam_radio_msgs REQUIRED) find_package(ateam_controls REQUIRED) +find_package(ateam_geometry REQUIRED) find_package(tf2 REQUIRED) find_package(tf2_geometry_msgs REQUIRED) find_package(ssl_league_protobufs REQUIRED) @@ -29,6 +30,7 @@ ament_target_dependencies(${PROJECT_NAME} ateam_msgs ateam_radio_msgs ateam_controls + ateam_geometry tf2 tf2_geometry_msgs ssl_league_protobufs diff --git a/radio/ateam_ssl_simulation_radio_bridge/package.xml b/radio/ateam_ssl_simulation_radio_bridge/package.xml index cbf416fd6..f33ef804c 100644 --- a/radio/ateam_ssl_simulation_radio_bridge/package.xml +++ b/radio/ateam_ssl_simulation_radio_bridge/package.xml @@ -15,6 +15,7 @@ ateam_msgs ateam_radio_msgs ateam_controls + ateam_geometry tf2 tf2_geometry_msgs ssl_league_protobufs diff --git a/radio/ateam_ssl_simulation_radio_bridge/src/message_conversions.cpp b/radio/ateam_ssl_simulation_radio_bridge/src/message_conversions.cpp index f66f8cb50..0d3de493d 100644 --- a/radio/ateam_ssl_simulation_radio_bridge/src/message_conversions.cpp +++ b/radio/ateam_ssl_simulation_radio_bridge/src/message_conversions.cpp @@ -54,7 +54,7 @@ double ReplaceNanWithZero(const double val, rclcpp::Logger logger) RobotControl fromMsg( const ateam_msgs::msg::RobotMotionCommand & ros_msg, ateam_msgs::msg::GameStateRobot robot, - ateam_ssl_simulation_radio_bridge::robot_maneuvers::ManeuverInfo & maneuver_info, rclcpp::Logger logger) + ateam_ssl_simulation_radio_bridge::robot_maneuvers::ManeuverExecutor & maneuver_executor, rclcpp::Logger logger) { RobotControl robots_control; RobotCommand * proto_robot_command = robots_control.add_robot_commands(); @@ -79,27 +79,7 @@ RobotControl fromMsg( } RobotMoveCommand * robot_move_command = proto_robot_command->mutable_move_command(); - - switch(ros_msg.body_control_mode) { - case ateam_msgs::msg::RobotMotionCommand::BCM_OFF: - ateam_ssl_simulation_radio_bridge::robot_maneuvers::local_velocity_maneuver(robot_move_command, ros_msg); - break; - case ateam_msgs::msg::RobotMotionCommand::BCM_GLOBAL_POSITION: - ateam_ssl_simulation_radio_bridge::robot_maneuvers::global_position_maneuver(robot_move_command, ros_msg, robot, maneuver_info); - break; - case ateam_msgs::msg::RobotMotionCommand::BCM_GLOBAL_VELOCITY: - ateam_ssl_simulation_radio_bridge::robot_maneuvers::global_velocity_maneuver(robot_move_command, ros_msg); - break; - case ateam_msgs::msg::RobotMotionCommand::BCM_LOCAL_VELOCITY: - ateam_ssl_simulation_radio_bridge::robot_maneuvers::local_velocity_maneuver(robot_move_command, ros_msg); - break; - case ateam_msgs::msg::RobotMotionCommand::BCM_GLOBAL_ACCEL: - ateam_ssl_simulation_radio_bridge::robot_maneuvers::global_acceleration_maneuver(robot_move_command, ros_msg, robot); - break; - case ateam_msgs::msg::RobotMotionCommand::BCM_LOCAL_ACCEL: - ateam_ssl_simulation_radio_bridge::robot_maneuvers::local_acceleration_maneuver(robot_move_command, ros_msg, robot); - break; - } + maneuver_executor.execute_maneuver(robot_move_command, ros_msg, robot); return robots_control; } diff --git a/radio/ateam_ssl_simulation_radio_bridge/src/message_conversions.hpp b/radio/ateam_ssl_simulation_radio_bridge/src/message_conversions.hpp index 826b019ba..24ddd9951 100644 --- a/radio/ateam_ssl_simulation_radio_bridge/src/message_conversions.hpp +++ b/radio/ateam_ssl_simulation_radio_bridge/src/message_conversions.hpp @@ -42,7 +42,7 @@ ateam_radio_msgs::msg::BasicTelemetry fromProto(const RobotFeedback & proto_msg) RobotControl fromMsg( const ateam_msgs::msg::RobotMotionCommand & ros_msg, ateam_msgs::msg::GameStateRobot robot, - ateam_ssl_simulation_radio_bridge::robot_maneuvers::ManeuverInfo & maneuver_info, rclcpp::Logger logger); + ateam_ssl_simulation_radio_bridge::robot_maneuvers::ManeuverExecutor & maneuver_executor, rclcpp::Logger logger); SimulatorControl fromMsg(const ssl_league_msgs::msg::SimulatorControl & ros_msg); diff --git a/radio/ateam_ssl_simulation_radio_bridge/src/robot_maneuvers.cpp b/radio/ateam_ssl_simulation_radio_bridge/src/robot_maneuvers.cpp index 4cf27f758..f69defec0 100644 --- a/radio/ateam_ssl_simulation_radio_bridge/src/robot_maneuvers.cpp +++ b/radio/ateam_ssl_simulation_radio_bridge/src/robot_maneuvers.cpp @@ -5,36 +5,62 @@ namespace ateam_ssl_simulation_radio_bridge::robot_maneuvers { - void global_position_maneuver(RobotMoveCommand * robot_move_command, const ateam_msgs::msg::RobotMotionCommand & ros_msg, ateam_msgs::msg::GameStateRobot robot, ManeuverInfo & maneuver_info) { - const Vector3C_t target_pose{ + void ManeuverExecutor::execute_maneuver(RobotMoveCommand * robot_move_command, const ateam_msgs::msg::RobotMotionCommand & ros_msg, ateam_msgs::msg::GameStateRobot robot) { + command_ = ros_msg; + + switch(ros_msg.body_control_mode) { + case ateam_msgs::msg::RobotMotionCommand::BCM_OFF: + bcm_off_maneuver(robot_move_command); + prev_body_command_ = ateam_msgs::msg::Twist2D(); + prev_update_time_ = std::chrono::steady_clock::now(); + return; // Ignores acceleration limits set in message and immediately stops the robot + case ateam_msgs::msg::RobotMotionCommand::BCM_GLOBAL_POSITION: + global_position_maneuver(robot_move_command, ros_msg, robot); + break; + case ateam_msgs::msg::RobotMotionCommand::BCM_GLOBAL_VELOCITY: + global_velocity_maneuver(robot_move_command, ros_msg, robot); + break; + case ateam_msgs::msg::RobotMotionCommand::BCM_LOCAL_VELOCITY: + local_velocity_maneuver(robot_move_command, ros_msg); + break; + case ateam_msgs::msg::RobotMotionCommand::BCM_GLOBAL_ACCEL: + global_acceleration_maneuver(robot_move_command, ros_msg, robot); + break; + case ateam_msgs::msg::RobotMotionCommand::BCM_LOCAL_ACCEL: + local_acceleration_maneuver(robot_move_command, ros_msg, robot); + break; + } + + finalize_command(robot_move_command, robot); + } + + void ManeuverExecutor::global_position_maneuver(RobotMoveCommand * robot_move_command, const ateam_msgs::msg::RobotMotionCommand & ros_msg, ateam_msgs::msg::GameStateRobot robot) { + const Vector3C_t new_target_pose{ float(ros_msg.pose.x), float(ros_msg.pose.y), float(ros_msg.pose.theta) }; - + tf2::Quaternion quat; tf2::fromMsg(robot.pose.orientation, quat); double yaw, pitch, roll; tf2::Matrix3x3(quat).getEulerYPR(yaw, pitch, roll); - Vector6C_t seed_state = maneuver_info.trajectory_state; - - // Check target point - double target_dist = std::pow(maneuver_info.target_pose.x - ros_msg.pose.x, 2) + std::pow(maneuver_info.target_pose.y - ros_msg.pose.y, 2); - // TODO (chachmu): fix angle wraparound issues and improve this check - bool target_pos_needs_replan = target_dist > 0.01 || abs(maneuver_info.target_pose.theta - ros_msg.pose.theta) > 0.01; + // Check if new target is too far from previous + double target_change_dist = std::hypot(target_pose_.x - ros_msg.pose.x, target_pose_.y - ros_msg.pose.y); + bool target_pos_needs_replan = target_change_dist > 0.01 || angles::shortest_angular_distance(target_pose_.theta, ros_msg.pose.theta) > 0.01; - // Check current position - double current_dist = std::pow(robot.pose.position.x - maneuver_info.trajectory_state.data[0], 2) + std::pow(robot.pose.position.y - maneuver_info.trajectory_state.data[1], 2); - // TODO (chachmu): fix angle wraparound issues and improve this check - bool current_pos_needs_replan = current_dist > 0.5 || abs(yaw - maneuver_info.trajectory_state.data[2]) > 0.2; - // current_pos_needs_replan = false; // REMOVE THIS + // Check current position against predicted position + double current_dist = std::hypot(robot.pose.position.x - trajectory_state_.data[0], robot.pose.position.y - trajectory_state_.data[1]); + bool current_pos_needs_replan = current_dist > 0.5 || angles::shortest_angular_distance(yaw, trajectory_state_.data[2]) > 0.2; - bool maneuver_type_mismatch = maneuver_info.maneuver_type != ateam_msgs::msg::RobotMotionCommand::BCM_GLOBAL_POSITION; + bool maneuver_type_mismatch = command_.body_control_mode != ateam_msgs::msg::RobotMotionCommand::BCM_GLOBAL_POSITION; bool replanning = maneuver_type_mismatch || target_pos_needs_replan || current_pos_needs_replan; + + // Use previously predicted state unless we need to replan + Vector6C_t seed_state = trajectory_state_; if (replanning) { - // std::cerr << "REPLANNING: " << maneuver_type_mismatch << ", " << target_pos_needs_replan << ", " << current_pos_needs_replan << std::endl; seed_state = Vector6C_t{ float(robot.pose.position.x), float(robot.pose.position.y), @@ -50,40 +76,36 @@ namespace ateam_ssl_simulation_radio_bridge::robot_maneuvers BangBangTraj3D_t traj; ateam_controls_traj_from_target_pose( seed_state, - target_pose, + new_target_pose, traj_params, &traj ); - // TODO (chachmu): figure out how to safely calculate dt - std::chrono::duration elapsed = std::chrono::steady_clock::now() - maneuver_info.prev_update_time; - // float dt = elapsed.count(); + // TODO (chachmu): The controls process may need to be shuffled around to calculate the trajectory state at the beginning of the function + // so that the actual dt for that timestep can be properly calculated + // float dt = float(get_dt()); float dt = 0.01; - // const double global_x_err = ros_msg.pose.x - robot.pose.position.x; - // const double global_y_err = ros_msg.pose.y - robot.pose.position.y; const double global_x_err = seed_state.data[0] - robot.pose.position.x; const double global_y_err = seed_state.data[1] - robot.pose.position.y; const double global_theta_err = seed_state.data[2] - yaw; - const double total_err = std::pow(global_x_err, 2) + std::pow(global_y_err, 2); - // std::cerr << "total_err: " << total_err << ", x: " << global_x_err << ", y: " << global_y_err << ", theta: " << global_theta_err << std::endl; - - const double kPos = 1.2; - const double kAngle = 1.0; + // const double kPos = 1.2; + // const double kAngle = 1.0; + const double kPos = 0; + const double kAngle = 0; - const double global_x_vel = seed_state.data[3] + (kPos * global_x_err); - const double global_y_vel = seed_state.data[4] + (kPos * global_y_err); - const double theta_vel = seed_state.data[5] + (kAngle * global_theta_err); + ateam_msgs::msg::Twist2D global_vel = ateam_msgs::msg::Twist2D(); + global_vel.x = seed_state.data[3] + (kPos * global_x_err); + global_vel.y = seed_state.data[4] + (kPos * global_y_err); + global_vel.theta = seed_state.data[5] + (kAngle * global_theta_err); - const double local_x_vel = cos(-yaw) * global_x_vel - sin(-yaw) * global_y_vel; - const double local_y_vel = sin(-yaw) * global_x_vel + cos(-yaw) * global_y_vel; + ateam_msgs::msg::Twist2D local_vel = rotate_frame(global_vel, robot.pose); MoveLocalVelocity * local_velocity_command = robot_move_command->mutable_local_velocity(); - - local_velocity_command->set_forward(local_x_vel); - local_velocity_command->set_left(local_y_vel); - local_velocity_command->set_angular(theta_vel); + local_velocity_command->set_forward(local_vel.x); + local_velocity_command->set_left(local_vel.y); + local_velocity_command->set_angular(local_vel.theta); // Update the trajectory state for the next iteration ateam_controls_traj_state_at( @@ -91,62 +113,177 @@ namespace ateam_ssl_simulation_radio_bridge::robot_maneuvers seed_state, 0.0f, dt, - &maneuver_info.trajectory_state + &trajectory_state_ ); + trajectory_ = traj; - // Only update the maneuver_info once we are sure we haven't errored out of using the maneuver + // Only update the target once we are sure we haven't errored out of using the maneuver if (replanning) { - maneuver_info.target_pose = ros_msg.pose; + target_pose_ = ros_msg.pose; } - maneuver_info.maneuver_type = ateam_msgs::msg::RobotMotionCommand::BCM_GLOBAL_POSITION; - maneuver_info.trajectory = traj; - maneuver_info.prev_update_time = std::chrono::steady_clock::now(); } - void global_velocity_maneuver(RobotMoveCommand * robot_move_command, const ateam_msgs::msg::RobotMotionCommand & ros_msg, ateam_msgs::msg::GameStateRobot robot) { + void ManeuverExecutor::global_velocity_maneuver(RobotMoveCommand * robot_move_command, const ateam_msgs::msg::RobotMotionCommand & ros_msg, ateam_msgs::msg::GameStateRobot robot) { MoveLocalVelocity * local_velocity_command = robot_move_command->mutable_local_velocity(); - tf2::Quaternion quat; - tf2::fromMsg(robot.pose.orientation, quat); - double yaw, pitch, roll; - tf2::Matrix3x3(quat).getEulerYPR(yaw, pitch, roll); + const ateam_msgs::msg::Twist2D local_vel = rotate_frame(ros_msg.velocity, robot.pose); + local_velocity_command->set_forward(local_vel.x); + local_velocity_command->set_left(local_vel.y); + local_velocity_command->set_angular(local_vel.theta); + } - const double local_x_vel = cos(-yaw) * ros_msg.velocity.x - sin(-yaw) * ros_msg.velocity.y; - const double local_y_vel = sin(-yaw) * ros_msg.velocity.x + cos(-yaw) * ros_msg.velocity.y; + void ManeuverExecutor::local_velocity_maneuver(RobotMoveCommand * robot_move_command, const ateam_msgs::msg::RobotMotionCommand & ros_msg) { + MoveLocalVelocity * local_velocity_command = robot_move_command->mutable_local_velocity(); local_velocity_command->set_forward(ros_msg.velocity.x); local_velocity_command->set_left(ros_msg.velocity.y); local_velocity_command->set_angular(ros_msg.velocity.theta); } - void local_velocity_maneuver(RobotMoveCommand * robot_move_command, const ateam_msgs::msg::RobotMotionCommand & ros_msg) { + void ManeuverExecutor::bcm_off_maneuver(RobotMoveCommand * robot_move_command) { MoveLocalVelocity * local_velocity_command = robot_move_command->mutable_local_velocity(); - local_velocity_command->set_forward(ros_msg.velocity.x); - local_velocity_command->set_left(ros_msg.velocity.y); - local_velocity_command->set_angular(ros_msg.velocity.theta); + local_velocity_command->set_forward(0.0); + local_velocity_command->set_left(0.0); + local_velocity_command->set_angular(0.0); } - void global_acceleration_maneuver(RobotMoveCommand * robot_move_command, const ateam_msgs::msg::RobotMotionCommand & ros_msg, ateam_msgs::msg::GameStateRobot robot) { - // TODO (chachmu): convert the coordinate frames + void ManeuverExecutor::global_acceleration_maneuver(RobotMoveCommand * robot_move_command, const ateam_msgs::msg::RobotMotionCommand & ros_msg, ateam_msgs::msg::GameStateRobot robot) { MoveLocalVelocity * local_velocity_command = robot_move_command->mutable_local_velocity(); - const double dt = 1.0 / 100.0; + const double dt = get_dt(); - local_velocity_command->set_forward(robot.velocity.linear.x + dt * ros_msg.acceleration.x); - local_velocity_command->set_left(robot.velocity.linear.y + dt * ros_msg.acceleration.y); - local_velocity_command->set_angular(robot.velocity.angular.z + dt * ros_msg.acceleration.theta); + ateam_msgs::msg::Twist2D global_accel = ateam_msgs::msg::Twist2D(); + global_accel.x = robot.velocity.linear.x + dt * ros_msg.acceleration.x; + global_accel.y = robot.velocity.linear.y + dt * ros_msg.acceleration.y; + global_accel.theta = robot.velocity.angular.z + dt * ros_msg.acceleration.theta; + + const ateam_msgs::msg::Twist2D local_accel = rotate_frame(global_accel, robot.pose); + + local_velocity_command->set_forward(local_accel.x); + local_velocity_command->set_left(local_accel.y); + local_velocity_command->set_angular(local_accel.theta); } - void local_acceleration_maneuver(RobotMoveCommand * robot_move_command, const ateam_msgs::msg::RobotMotionCommand & ros_msg, ateam_msgs::msg::GameStateRobot robot) { + void ManeuverExecutor::local_acceleration_maneuver(RobotMoveCommand * robot_move_command, const ateam_msgs::msg::RobotMotionCommand & ros_msg, ateam_msgs::msg::GameStateRobot robot) { MoveLocalVelocity * local_velocity_command = robot_move_command->mutable_local_velocity(); - const double dt = 1.0 / 100.0; + const double dt = get_dt(); local_velocity_command->set_forward(robot.velocity.linear.x + dt * ros_msg.acceleration.x); local_velocity_command->set_left(robot.velocity.linear.y + dt * ros_msg.acceleration.y); local_velocity_command->set_angular(robot.velocity.angular.z + dt * ros_msg.acceleration.theta); } - TrajectoryParams_t generate_trajectory_params(const ateam_msgs::msg::RobotMotionCommand & ros_msg) { + void ManeuverExecutor::finalize_command(RobotMoveCommand * robot_move_command, ateam_msgs::msg::GameStateRobot robot) { + + // TODO (chachmu): Expose default limits from the rust controls package + double limit_vel_linear = 3.0; + double limit_vel_angular = 5.0 * M_PI; + double limit_acc_linear = 3.0; + double limit_acc_angular = 10.0 * M_PI; + + if (command_.limit_vel_linear != 0.0) { + limit_vel_linear = command_.limit_vel_linear; + } + if (command_.limit_vel_angular != 0.0) { + limit_vel_angular = command_.limit_vel_angular; + } + if (command_.limit_acc_linear != 0.0) { + limit_acc_linear = command_.limit_acc_linear; + } + if (command_.limit_acc_angular != 0.0) { + limit_acc_angular = command_.limit_acc_angular; + } + + double dt = get_dt(); + if (dt == 0.0 || std::isnan(dt) || dt > 0.1) { + // This is a bit hacky but should keep dt from going too crazy + dt = 0.01; + } + + if (robot_move_command->has_local_velocity()) { + MoveLocalVelocity * local_velocity_command = robot_move_command->mutable_local_velocity(); + + ateam_msgs::msg::Twist2D local_command = ateam_msgs::msg::Twist2D(); + local_command.x = local_velocity_command->forward(); + local_command.y = local_velocity_command->left(); + local_command.theta = local_velocity_command->angular(); + + local_command = apply_xy_motion_limits(prev_body_command_, local_command, limit_vel_linear, limit_acc_linear, dt); + local_command.theta = apply_1d_motion_limits(prev_body_command_.theta, local_command.theta, limit_vel_angular, limit_acc_angular, dt); + + local_velocity_command->set_forward(local_command.x); + local_velocity_command->set_left(local_command.y); + local_velocity_command->set_angular(local_command.theta); + + prev_body_command_ = local_command; + } else if (robot_move_command->has_global_velocity()) { + MoveGlobalVelocity * global_velocity_command = robot_move_command->mutable_global_velocity(); + + ateam_msgs::msg::Twist2D global_command = ateam_msgs::msg::Twist2D(); + global_command.x = global_velocity_command->x(); + global_command.y = global_velocity_command->y(); + global_command.theta = global_velocity_command->angular(); + + global_command = apply_xy_motion_limits(prev_body_command_, global_command, limit_vel_linear, limit_acc_linear, dt); + global_command.theta = apply_1d_motion_limits(prev_body_command_.theta, global_command.theta, limit_vel_angular, limit_acc_angular, dt); + + global_velocity_command->set_x(global_command.x); + global_velocity_command->set_y(global_command.y); + global_velocity_command->set_angular(global_command.theta); + + prev_body_command_ = rotate_frame(global_command, robot.pose); + } + + prev_update_time_ = std::chrono::steady_clock::now(); + } + + + ateam_msgs::msg::Twist2D ManeuverExecutor::apply_xy_motion_limits(ateam_msgs::msg::Twist2D prev, ateam_msgs::msg::Twist2D command, double vel_limit, double acc_limit, double dt) { + ateam_geometry::Vector prev_xy{prev.x, prev.y}; + ateam_geometry::Vector command_xy{command.x, command.y}; + ateam_geometry::Vector requested_acceleration = command_xy - prev_xy; + + double acceleration_magnitude = std::clamp(ateam_geometry::norm(requested_acceleration), 0.0, dt * acc_limit); + ateam_geometry::Vector acc_limited_command = prev_xy + (acceleration_magnitude * ateam_geometry::normalize(requested_acceleration)); + + double final_magnitude = std::clamp(ateam_geometry::norm(acc_limited_command), 0.0, vel_limit); + ateam_geometry::Vector final_xy = final_magnitude * ateam_geometry::normalize(acc_limited_command); + + command.x = final_xy.x(); + command.y = final_xy.y(); + + return command; + } + + double ManeuverExecutor::apply_1d_motion_limits(double prev, double command, double vel_limit, double acc_limit, double dt) { + double requested_acceleration = command - prev; + double acc_limited_command = prev + std::clamp(requested_acceleration, -dt * acc_limit, dt * acc_limit); + return std::clamp(acc_limited_command, -vel_limit, vel_limit); + } + + double ManeuverExecutor::get_dt() { + const std::chrono::duration elapsed = std::chrono::steady_clock::now() - prev_update_time_; + return elapsed.count(); + } + + ateam_msgs::msg::Twist2D ManeuverExecutor::rotate_frame(ateam_msgs::msg::Twist2D input_frame, geometry_msgs::msg::Pose pose) { + tf2::Quaternion quat; + tf2::fromMsg(pose.orientation, quat); + double yaw, pitch, roll; + tf2::Matrix3x3(quat).getEulerYPR(yaw, pitch, roll); + + return rotate_frame(input_frame, yaw); + } + + ateam_msgs::msg::Twist2D ManeuverExecutor::rotate_frame(ateam_msgs::msg::Twist2D input_frame, double angle) { + ateam_msgs::msg::Twist2D output_frame = ateam_msgs::msg::Twist2D(); + output_frame.x = cos(-angle) * input_frame.x - sin(-angle) * input_frame.y; + output_frame.y = sin(-angle) * input_frame.x + cos(-angle) * input_frame.y; + output_frame.theta = input_frame.theta; + return output_frame; + } + + TrajectoryParams_t ManeuverExecutor::generate_trajectory_params(const ateam_msgs::msg::RobotMotionCommand & ros_msg) { TrajectoryParams_t traj_params = ateam_controls_default_traj_params(); diff --git a/radio/ateam_ssl_simulation_radio_bridge/src/robot_maneuvers.hpp b/radio/ateam_ssl_simulation_radio_bridge/src/robot_maneuvers.hpp index 0529cdebe..c3826397c 100644 --- a/radio/ateam_ssl_simulation_radio_bridge/src/robot_maneuvers.hpp +++ b/radio/ateam_ssl_simulation_radio_bridge/src/robot_maneuvers.hpp @@ -30,28 +30,49 @@ #include #include +#include +#include #include "ateam_controls/ateam_controls.h" namespace ateam_ssl_simulation_radio_bridge::robot_maneuvers { - struct ManeuverInfo { - BangBangTraj3D_t trajectory; - Vector6C_t trajectory_state; - ateam_msgs::msg::Twist2D target_pose; - int32_t maneuver_type = 0; - std::chrono::steady_clock::time_point prev_update_time; - }; + class ManeuverExecutor { - void global_position_maneuver(RobotMoveCommand * robot_move_command, const ateam_msgs::msg::RobotMotionCommand & ros_msg, ateam_msgs::msg::GameStateRobot robot, ManeuverInfo & maneuver_info); + public: + ManeuverExecutor() { + prev_update_time_ = std::chrono::steady_clock::now(); + } + + void execute_maneuver(RobotMoveCommand * robot_move_command, const ateam_msgs::msg::RobotMotionCommand & ros_msg, ateam_msgs::msg::GameStateRobot robot); + + void global_position_maneuver(RobotMoveCommand * robot_move_command, const ateam_msgs::msg::RobotMotionCommand & ros_msg, ateam_msgs::msg::GameStateRobot robot); void global_velocity_maneuver(RobotMoveCommand * robot_move_command, const ateam_msgs::msg::RobotMotionCommand & ros_msg, ateam_msgs::msg::GameStateRobot robot); void local_velocity_maneuver(RobotMoveCommand * robot_move_command, const ateam_msgs::msg::RobotMotionCommand & ros_msg); + void bcm_off_maneuver(RobotMoveCommand * robot_move_command); void global_acceleration_maneuver(RobotMoveCommand * robot_move_command, const ateam_msgs::msg::RobotMotionCommand & ros_msg, ateam_msgs::msg::GameStateRobot robot); void local_acceleration_maneuver(RobotMoveCommand * robot_move_command, const ateam_msgs::msg::RobotMotionCommand & ros_msg, ateam_msgs::msg::GameStateRobot robot); + private: + void finalize_command(RobotMoveCommand * robot_move_command, ateam_msgs::msg::GameStateRobot robot); + + ateam_msgs::msg::Twist2D apply_xy_motion_limits(ateam_msgs::msg::Twist2D prev, ateam_msgs::msg::Twist2D command, double vel_limit, double acc_limit, double dt); + double apply_1d_motion_limits(double prev, double commanded, double vel_limit, double acc_limit, double dt); + + double get_dt(); + ateam_msgs::msg::Twist2D rotate_frame(ateam_msgs::msg::Twist2D input_frame, geometry_msgs::msg::Pose pose); + ateam_msgs::msg::Twist2D rotate_frame(ateam_msgs::msg::Twist2D input_frame, double angle); TrajectoryParams_t generate_trajectory_params(const ateam_msgs::msg::RobotMotionCommand & ros_msg); + BangBangTraj3D_t trajectory_; + Vector6C_t trajectory_state_; // Predicted location of robot + ateam_msgs::msg::RobotMotionCommand command_; // Current command, updates with every new message + ateam_msgs::msg::Twist2D target_pose_; // Current target point, only updates when moving far enough to limit replanning + + ateam_msgs::msg::Twist2D prev_body_command_; + std::chrono::steady_clock::time_point prev_update_time_; + }; } // namespace ateam_ssl_simulation_radio_bridge::robot_maneuvers diff --git a/radio/ateam_ssl_simulation_radio_bridge/src/ssl_simulation_radio_bridge_node.cpp b/radio/ateam_ssl_simulation_radio_bridge/src/ssl_simulation_radio_bridge_node.cpp index dd2c4d9a0..6f8563f34 100644 --- a/radio/ateam_ssl_simulation_radio_bridge/src/ssl_simulation_radio_bridge_node.cpp +++ b/radio/ateam_ssl_simulation_radio_bridge/src/ssl_simulation_radio_bridge_node.cpp @@ -65,7 +65,7 @@ class SSLSimulationRadioBridgeNode : public rclcpp::Node declare_parameter("ssl_sim_blue_port", 10301); declare_parameter("ssl_sim_yellow_port", 10302); - team_color_change_callback(ateam_common::TeamColor::Blue); + team_color_change_callback(ateam_common::TeamColor::Unknown); create_indexed_subscribers ( @@ -177,12 +177,15 @@ class SSLSimulationRadioBridgeNode : public rclcpp::Node } const auto robot = world_.our_robots[robot_id]; - auto& maneuver_info = manuever_infos_[robot_id]; - RobotControl robots_control = message_conversions::fromMsg(msg, robot, maneuver_info, get_logger()); - std::vector buffer; - buffer.resize(robots_control.ByteSizeLong()); - if (robots_control.SerializeToArray(buffer.data(), buffer.size())) { - udp_robot_control_->send(static_cast(buffer.data()), buffer.size()); + // Somehow nonvisible robots have their id set to 0 in the world topic, easier to just not interact with them + if (robot.visible) { + auto& maneuver_executor = manuever_executors_[robot_id]; + RobotControl robots_control = message_conversions::fromMsg(msg, robot, maneuver_executor, get_logger()); + std::vector buffer; + buffer.resize(robots_control.ByteSizeLong()); + if (robots_control.SerializeToArray(buffer.data(), buffer.size())) { + udp_robot_control_->send(static_cast(buffer.data()), buffer.size()); + } } } @@ -252,7 +255,7 @@ class SSLSimulationRadioBridgeNode : public rclcpp::Node 16> connection_publishers_; rclcpp::Subscription::SharedPtr world_subscription_; ateam_msgs::msg::GameStateWorld world_; - std::array manuever_infos_; + std::array manuever_executors_; rclcpp::Service::SharedPtr send_simulator_control_service_; rclcpp::TimerBase::SharedPtr zero_command_timer_; From ceadddaa7a63e62eece129ad01589b857050a3cf Mon Sep 17 00:00:00 2001 From: Matthew Woodward Date: Mon, 22 Jun 2026 23:09:45 -0400 Subject: [PATCH 15/16] Works well with waypoint controller, kenobi replans break everything --- .../src/robot_maneuvers.cpp | 303 +++++++----------- .../src/robot_maneuvers.hpp | 36 ++- .../src/ssl_simulation_radio_bridge_node.cpp | 34 +- 3 files changed, 159 insertions(+), 214 deletions(-) diff --git a/radio/ateam_ssl_simulation_radio_bridge/src/robot_maneuvers.cpp b/radio/ateam_ssl_simulation_radio_bridge/src/robot_maneuvers.cpp index f69defec0..13c499a2e 100644 --- a/radio/ateam_ssl_simulation_radio_bridge/src/robot_maneuvers.cpp +++ b/radio/ateam_ssl_simulation_radio_bridge/src/robot_maneuvers.cpp @@ -6,238 +6,168 @@ namespace ateam_ssl_simulation_radio_bridge::robot_maneuvers { void ManeuverExecutor::execute_maneuver(RobotMoveCommand * robot_move_command, const ateam_msgs::msg::RobotMotionCommand & ros_msg, ateam_msgs::msg::GameStateRobot robot) { - command_ = ros_msg; - switch(ros_msg.body_control_mode) { case ateam_msgs::msg::RobotMotionCommand::BCM_OFF: - bcm_off_maneuver(robot_move_command); - prev_body_command_ = ateam_msgs::msg::Twist2D(); - prev_update_time_ = std::chrono::steady_clock::now(); + bcm_off_maneuver(); return; // Ignores acceleration limits set in message and immediately stops the robot + + // Trajectory Maneuvers case ateam_msgs::msg::RobotMotionCommand::BCM_GLOBAL_POSITION: - global_position_maneuver(robot_move_command, ros_msg, robot); + global_position_maneuver(ros_msg, robot); break; + + // Global Maneuvers case ateam_msgs::msg::RobotMotionCommand::BCM_GLOBAL_VELOCITY: - global_velocity_maneuver(robot_move_command, ros_msg, robot); - break; - case ateam_msgs::msg::RobotMotionCommand::BCM_LOCAL_VELOCITY: - local_velocity_maneuver(robot_move_command, ros_msg); + global_velocity_maneuver(ros_msg); break; case ateam_msgs::msg::RobotMotionCommand::BCM_GLOBAL_ACCEL: - global_acceleration_maneuver(robot_move_command, ros_msg, robot); + global_acceleration_maneuver(ros_msg); + break; + + // Local Maneuvers + case ateam_msgs::msg::RobotMotionCommand::BCM_LOCAL_VELOCITY: + local_velocity_maneuver(ros_msg, robot); break; case ateam_msgs::msg::RobotMotionCommand::BCM_LOCAL_ACCEL: - local_acceleration_maneuver(robot_move_command, ros_msg, robot); + local_acceleration_maneuver(ros_msg, robot); break; } - finalize_command(robot_move_command, robot); + finalize_command(robot_move_command, ros_msg, robot); } - void ManeuverExecutor::global_position_maneuver(RobotMoveCommand * robot_move_command, const ateam_msgs::msg::RobotMotionCommand & ros_msg, ateam_msgs::msg::GameStateRobot robot) { - const Vector3C_t new_target_pose{ - float(ros_msg.pose.x), - float(ros_msg.pose.y), - float(ros_msg.pose.theta) - }; + void ManeuverExecutor::bcm_off_maneuver() { + current_global_command_ = ateam_msgs::msg::Twist2D(); + prev_global_command_ = ateam_msgs::msg::Twist2D(); + prev_update_time_ = std::chrono::steady_clock::now(); + } - tf2::Quaternion quat; - tf2::fromMsg(robot.pose.orientation, quat); - double yaw, pitch, roll; - tf2::Matrix3x3(quat).getEulerYPR(yaw, pitch, roll); + // Trajectory Maneuvers + void ManeuverExecutor::global_position_maneuver(const ateam_msgs::msg::RobotMotionCommand & ros_msg, ateam_msgs::msg::GameStateRobot robot) { - // Check if new target is too far from previous - double target_change_dist = std::hypot(target_pose_.x - ros_msg.pose.x, target_pose_.y - ros_msg.pose.y); - bool target_pos_needs_replan = target_change_dist > 0.01 || angles::shortest_angular_distance(target_pose_.theta, ros_msg.pose.theta) > 0.01; + // Update trajectory state to current time + float dt = float(get_dt()); + ateam_controls_tick_traj(&trajectory_, dt); + + double yaw = get_yaw(robot.pose); // Check current position against predicted position - double current_dist = std::hypot(robot.pose.position.x - trajectory_state_.data[0], robot.pose.position.y - trajectory_state_.data[1]); - bool current_pos_needs_replan = current_dist > 0.5 || angles::shortest_angular_distance(yaw, trajectory_state_.data[2]) > 0.2; + double current_dist = std::hypot(robot.pose.position.x - trajectory_.state.data[0], robot.pose.position.y - trajectory_.state.data[1]); + bool current_pos_needs_replan = current_dist > 0.5 || angles::shortest_angular_distance(yaw, trajectory_.state.data[2]) > 0.3; + + bool target_changed = std::hypot(command_.pose.x - ros_msg.pose.x, command_.pose.y - ros_msg.pose.y) > 0.02 || angles::shortest_angular_distance(command_.pose.theta, ros_msg.pose.theta) > 0.1; - bool maneuver_type_mismatch = command_.body_control_mode != ateam_msgs::msg::RobotMotionCommand::BCM_GLOBAL_POSITION; + // bool maneuver_command_changed = (command_ != ros_msg); + bool maneuver_command_changed = (command_.body_control_mode != ateam_msgs::msg::RobotMotionCommand::BCM_GLOBAL_POSITION || target_changed); - bool replanning = maneuver_type_mismatch || target_pos_needs_replan || current_pos_needs_replan; + bool replanning = maneuver_command_changed || current_pos_needs_replan; - // Use previously predicted state unless we need to replan - Vector6C_t seed_state = trajectory_state_; if (replanning) { - seed_state = Vector6C_t{ + std::cerr << "replanning id " << robot.id << ": (" << maneuver_command_changed << ", " << current_pos_needs_replan << ")" << std::endl; + Vector6C_t starting_state = Vector6C_t{ float(robot.pose.position.x), float(robot.pose.position.y), float(yaw), - float(robot.velocity.linear.x), - float(robot.velocity.linear.y), - float(robot.velocity.angular.z) + // float(robot.velocity.linear.x), + // float(robot.velocity.linear.y), + // float(robot.velocity.angular.z) + float(prev_global_command_.x), + float(prev_global_command_.y), + float(prev_global_command_.theta), }; - } - - TrajectoryParams_t traj_params = generate_trajectory_params(ros_msg); - BangBangTraj3D_t traj; - ateam_controls_traj_from_target_pose( - seed_state, - new_target_pose, - traj_params, - &traj - ); - - // TODO (chachmu): The controls process may need to be shuffled around to calculate the trajectory state at the beginning of the function - // so that the actual dt for that timestep can be properly calculated - // float dt = float(get_dt()); - float dt = 0.01; + const Vector3C_t target_pose{ + float(ros_msg.pose.x), + float(ros_msg.pose.y), + float(ros_msg.pose.theta) + }; - const double global_x_err = seed_state.data[0] - robot.pose.position.x; - const double global_y_err = seed_state.data[1] - robot.pose.position.y; - const double global_theta_err = seed_state.data[2] - yaw; + command_ = ros_msg; + traj_params_ = generate_trajectory_params(ros_msg); + ateam_controls_traj_from_target_pose( + starting_state, + target_pose, + traj_params_, + &trajectory_ + ); + } - // const double kPos = 1.2; - // const double kAngle = 1.0; - const double kPos = 0; - const double kAngle = 0; + ateam_msgs::msg::Twist2D global_target_err = ateam_msgs::msg::Twist2D(); + global_target_err.x = command_.pose.x - robot.pose.position.x; + global_target_err.y = command_.pose.y - robot.pose.position.y; - ateam_msgs::msg::Twist2D global_vel = ateam_msgs::msg::Twist2D(); - global_vel.x = seed_state.data[3] + (kPos * global_x_err); - global_vel.y = seed_state.data[4] + (kPos * global_y_err); - global_vel.theta = seed_state.data[5] + (kAngle * global_theta_err); + const double kpPos = 1.0; + const double kdPos = 0.0; + const double kpAngle = 0.0; - ateam_msgs::msg::Twist2D local_vel = rotate_frame(global_vel, robot.pose); + ateam_msgs::msg::Twist2D global_feedback = ateam_msgs::msg::Twist2D(); - MoveLocalVelocity * local_velocity_command = robot_move_command->mutable_local_velocity(); - local_velocity_command->set_forward(local_vel.x); - local_velocity_command->set_left(local_vel.y); - local_velocity_command->set_angular(local_vel.theta); - - // Update the trajectory state for the next iteration - ateam_controls_traj_state_at( - traj, - seed_state, - 0.0f, - dt, - &trajectory_state_ - ); - trajectory_ = traj; - - // Only update the target once we are sure we haven't errored out of using the maneuver - if (replanning) { - target_pose_ = ros_msg.pose; + // Provide some minor xy feedback to account for slight final position offsets + if (abs(global_target_err.x) < 0.05) { + global_feedback.x += std::clamp(kpPos * global_target_err.x, -0.05, 0.05); + } + if (abs(global_target_err.y) < 0.05) { + global_feedback.y += std::clamp(kpPos * global_target_err.y, -0.05, 0.05); } - } - - void ManeuverExecutor::global_velocity_maneuver(RobotMoveCommand * robot_move_command, const ateam_msgs::msg::RobotMotionCommand & ros_msg, ateam_msgs::msg::GameStateRobot robot) { - MoveLocalVelocity * local_velocity_command = robot_move_command->mutable_local_velocity(); - const ateam_msgs::msg::Twist2D local_vel = rotate_frame(ros_msg.velocity, robot.pose); - local_velocity_command->set_forward(local_vel.x); - local_velocity_command->set_left(local_vel.y); - local_velocity_command->set_angular(local_vel.theta); + current_global_command_.x = trajectory_.state.data[3] + global_feedback.x; + current_global_command_.y = trajectory_.state.data[4] + global_feedback.y; + current_global_command_.theta = trajectory_.state.data[5] + global_feedback.theta; } - void ManeuverExecutor::local_velocity_maneuver(RobotMoveCommand * robot_move_command, const ateam_msgs::msg::RobotMotionCommand & ros_msg) { - MoveLocalVelocity * local_velocity_command = robot_move_command->mutable_local_velocity(); - - local_velocity_command->set_forward(ros_msg.velocity.x); - local_velocity_command->set_left(ros_msg.velocity.y); - local_velocity_command->set_angular(ros_msg.velocity.theta); + // Global Maneuvers + void ManeuverExecutor::global_velocity_maneuver(const ateam_msgs::msg::RobotMotionCommand & ros_msg) { + current_global_command_ = ros_msg.velocity; + std::cerr << "global command, global vel: " << current_global_command_.x << ", " << current_global_command_.y << ", " << current_global_command_.theta << std::endl; } - void ManeuverExecutor::bcm_off_maneuver(RobotMoveCommand * robot_move_command) { - MoveLocalVelocity * local_velocity_command = robot_move_command->mutable_local_velocity(); - - local_velocity_command->set_forward(0.0); - local_velocity_command->set_left(0.0); - local_velocity_command->set_angular(0.0); - } - - void ManeuverExecutor::global_acceleration_maneuver(RobotMoveCommand * robot_move_command, const ateam_msgs::msg::RobotMotionCommand & ros_msg, ateam_msgs::msg::GameStateRobot robot) { - MoveLocalVelocity * local_velocity_command = robot_move_command->mutable_local_velocity(); + void ManeuverExecutor::global_acceleration_maneuver(const ateam_msgs::msg::RobotMotionCommand & ros_msg) { const double dt = get_dt(); - ateam_msgs::msg::Twist2D global_accel = ateam_msgs::msg::Twist2D(); - global_accel.x = robot.velocity.linear.x + dt * ros_msg.acceleration.x; - global_accel.y = robot.velocity.linear.y + dt * ros_msg.acceleration.y; - global_accel.theta = robot.velocity.angular.z + dt * ros_msg.acceleration.theta; - - const ateam_msgs::msg::Twist2D local_accel = rotate_frame(global_accel, robot.pose); + current_global_command_.x = prev_global_command_.x + dt * ros_msg.acceleration.x; + current_global_command_.y = prev_global_command_.y + dt * ros_msg.acceleration.y; + current_global_command_.theta = prev_global_command_.theta + dt * ros_msg.acceleration.theta; + } - local_velocity_command->set_forward(local_accel.x); - local_velocity_command->set_left(local_accel.y); - local_velocity_command->set_angular(local_accel.theta); + // Local Maneuvers + void ManeuverExecutor::local_velocity_maneuver(const ateam_msgs::msg::RobotMotionCommand & ros_msg, ateam_msgs::msg::GameStateRobot robot) { + current_global_command_ = rotate_frame(ros_msg.velocity, -get_yaw(robot.pose)); + std::cerr << "local command, global vel: " << current_global_command_.x << ", " << current_global_command_.y << ", " << current_global_command_.theta << std::endl; } - void ManeuverExecutor::local_acceleration_maneuver(RobotMoveCommand * robot_move_command, const ateam_msgs::msg::RobotMotionCommand & ros_msg, ateam_msgs::msg::GameStateRobot robot) { - MoveLocalVelocity * local_velocity_command = robot_move_command->mutable_local_velocity(); + void ManeuverExecutor::local_acceleration_maneuver(const ateam_msgs::msg::RobotMotionCommand & ros_msg, ateam_msgs::msg::GameStateRobot robot) { + ateam_msgs::msg::Twist2D global_accel = rotate_frame(ros_msg.acceleration, -get_yaw(robot.pose)); const double dt = get_dt(); - local_velocity_command->set_forward(robot.velocity.linear.x + dt * ros_msg.acceleration.x); - local_velocity_command->set_left(robot.velocity.linear.y + dt * ros_msg.acceleration.y); - local_velocity_command->set_angular(robot.velocity.angular.z + dt * ros_msg.acceleration.theta); + current_global_command_.x = prev_global_command_.x + dt * global_accel.x; + current_global_command_.y = prev_global_command_.y + dt * global_accel.y; + current_global_command_.theta = prev_global_command_.theta + dt * global_accel.theta; } - void ManeuverExecutor::finalize_command(RobotMoveCommand * robot_move_command, ateam_msgs::msg::GameStateRobot robot) { - - // TODO (chachmu): Expose default limits from the rust controls package - double limit_vel_linear = 3.0; - double limit_vel_angular = 5.0 * M_PI; - double limit_acc_linear = 3.0; - double limit_acc_angular = 10.0 * M_PI; - - if (command_.limit_vel_linear != 0.0) { - limit_vel_linear = command_.limit_vel_linear; - } - if (command_.limit_vel_angular != 0.0) { - limit_vel_angular = command_.limit_vel_angular; - } - if (command_.limit_acc_linear != 0.0) { - limit_acc_linear = command_.limit_acc_linear; - } - if (command_.limit_acc_angular != 0.0) { - limit_acc_angular = command_.limit_acc_angular; - } - + void ManeuverExecutor::finalize_command(RobotMoveCommand * robot_move_command, const ateam_msgs::msg::RobotMotionCommand & ros_msg, ateam_msgs::msg::GameStateRobot robot) { + command_ = ros_msg; double dt = get_dt(); - if (dt == 0.0 || std::isnan(dt) || dt > 0.1) { - // This is a bit hacky but should keep dt from going too crazy - dt = 0.01; - } - if (robot_move_command->has_local_velocity()) { - MoveLocalVelocity * local_velocity_command = robot_move_command->mutable_local_velocity(); + traj_params_ = generate_trajectory_params(command_); + current_global_command_ = apply_xy_motion_limits(prev_global_command_, current_global_command_, traj_params_.max_vel_linear, traj_params_.max_accel_linear, dt); + current_global_command_.theta = apply_1d_motion_limits(prev_global_command_.theta, current_global_command_.theta, traj_params_.max_vel_angular, traj_params_.max_accel_angular, dt); - ateam_msgs::msg::Twist2D local_command = ateam_msgs::msg::Twist2D(); - local_command.x = local_velocity_command->forward(); - local_command.y = local_velocity_command->left(); - local_command.theta = local_velocity_command->angular(); - - local_command = apply_xy_motion_limits(prev_body_command_, local_command, limit_vel_linear, limit_acc_linear, dt); - local_command.theta = apply_1d_motion_limits(prev_body_command_.theta, local_command.theta, limit_vel_angular, limit_acc_angular, dt); - - local_velocity_command->set_forward(local_command.x); - local_velocity_command->set_left(local_command.y); - local_velocity_command->set_angular(local_command.theta); - - prev_body_command_ = local_command; - } else if (robot_move_command->has_global_velocity()) { - MoveGlobalVelocity * global_velocity_command = robot_move_command->mutable_global_velocity(); - - ateam_msgs::msg::Twist2D global_command = ateam_msgs::msg::Twist2D(); - global_command.x = global_velocity_command->x(); - global_command.y = global_velocity_command->y(); - global_command.theta = global_velocity_command->angular(); - - global_command = apply_xy_motion_limits(prev_body_command_, global_command, limit_vel_linear, limit_acc_linear, dt); - global_command.theta = apply_1d_motion_limits(prev_body_command_.theta, global_command.theta, limit_vel_angular, limit_acc_angular, dt); - - global_velocity_command->set_x(global_command.x); - global_velocity_command->set_y(global_command.y); - global_velocity_command->set_angular(global_command.theta); - - prev_body_command_ = rotate_frame(global_command, robot.pose); + ateam_msgs::msg::Twist2D local_command; + if (use_trajectory_angle()) { + local_command = rotate_frame(current_global_command_, trajectory_.state.data[2]); + } else { + local_command = rotate_frame(current_global_command_, get_yaw(robot.pose)); } + MoveLocalVelocity * local_velocity_command = robot_move_command->mutable_local_velocity(); + local_velocity_command->set_forward(local_command.x); + local_velocity_command->set_left(local_command.y); + local_velocity_command->set_angular(local_command.theta); + + prev_global_command_ = current_global_command_; prev_update_time_ = std::chrono::steady_clock::now(); } - ateam_msgs::msg::Twist2D ManeuverExecutor::apply_xy_motion_limits(ateam_msgs::msg::Twist2D prev, ateam_msgs::msg::Twist2D command, double vel_limit, double acc_limit, double dt) { ateam_geometry::Vector prev_xy{prev.x, prev.y}; ateam_geometry::Vector command_xy{command.x, command.y}; @@ -262,17 +192,18 @@ namespace ateam_ssl_simulation_radio_bridge::robot_maneuvers } double ManeuverExecutor::get_dt() { - const std::chrono::duration elapsed = std::chrono::steady_clock::now() - prev_update_time_; - return elapsed.count(); + // const std::chrono::duration elapsed = std::chrono::steady_clock::now() - prev_update_time_; + // return elapsed.count(); + return 0.01; } - ateam_msgs::msg::Twist2D ManeuverExecutor::rotate_frame(ateam_msgs::msg::Twist2D input_frame, geometry_msgs::msg::Pose pose) { + double ManeuverExecutor::get_yaw(geometry_msgs::msg::Pose pose) { tf2::Quaternion quat; tf2::fromMsg(pose.orientation, quat); double yaw, pitch, roll; tf2::Matrix3x3(quat).getEulerYPR(yaw, pitch, roll); - return rotate_frame(input_frame, yaw); + return yaw; } ateam_msgs::msg::Twist2D ManeuverExecutor::rotate_frame(ateam_msgs::msg::Twist2D input_frame, double angle) { @@ -300,6 +231,18 @@ namespace ateam_ssl_simulation_radio_bridge::robot_maneuvers traj_params.max_accel_angular = ros_msg.limit_acc_angular; } + // std::cerr << "traj limits: " << traj_params.max_vel_linear << ", " << traj_params.max_vel_angular << ", " << traj_params.max_accel_linear << ", " << traj_params.max_accel_angular << std::endl; return traj_params; } + + bool ManeuverExecutor::use_trajectory_angle() { + switch(command_.body_control_mode) { + case ateam_msgs::msg::RobotMotionCommand::BCM_GLOBAL_POSITION: + case ateam_msgs::msg::RobotMotionCommand::BCM_PIVOT: + return true; + + default: + return false; + } + } } // namespace ateam_ssl_simulation_radio_bridge::robot_maneuvers \ No newline at end of file diff --git a/radio/ateam_ssl_simulation_radio_bridge/src/robot_maneuvers.hpp b/radio/ateam_ssl_simulation_radio_bridge/src/robot_maneuvers.hpp index c3826397c..34c4c57b2 100644 --- a/radio/ateam_ssl_simulation_radio_bridge/src/robot_maneuvers.hpp +++ b/radio/ateam_ssl_simulation_radio_bridge/src/robot_maneuvers.hpp @@ -40,37 +40,47 @@ namespace ateam_ssl_simulation_radio_bridge::robot_maneuvers public: ManeuverExecutor() { + traj_params_ = ateam_controls_default_traj_params(); prev_update_time_ = std::chrono::steady_clock::now(); } + void set_command(ateam_msgs::msg::RobotMotionCommand command) { + command_ = command; + } + void execute_maneuver(RobotMoveCommand * robot_move_command, const ateam_msgs::msg::RobotMotionCommand & ros_msg, ateam_msgs::msg::GameStateRobot robot); - void global_position_maneuver(RobotMoveCommand * robot_move_command, const ateam_msgs::msg::RobotMotionCommand & ros_msg, ateam_msgs::msg::GameStateRobot robot); + private: + void bcm_off_maneuver(); - void global_velocity_maneuver(RobotMoveCommand * robot_move_command, const ateam_msgs::msg::RobotMotionCommand & ros_msg, ateam_msgs::msg::GameStateRobot robot); - void local_velocity_maneuver(RobotMoveCommand * robot_move_command, const ateam_msgs::msg::RobotMotionCommand & ros_msg); - void bcm_off_maneuver(RobotMoveCommand * robot_move_command); + // Trajectory Maneuvers + void global_position_maneuver(const ateam_msgs::msg::RobotMotionCommand & ros_msg, ateam_msgs::msg::GameStateRobot robot); - void global_acceleration_maneuver(RobotMoveCommand * robot_move_command, const ateam_msgs::msg::RobotMotionCommand & ros_msg, ateam_msgs::msg::GameStateRobot robot); - void local_acceleration_maneuver(RobotMoveCommand * robot_move_command, const ateam_msgs::msg::RobotMotionCommand & ros_msg, ateam_msgs::msg::GameStateRobot robot); + // Global Maneuvers + void global_velocity_maneuver(const ateam_msgs::msg::RobotMotionCommand & ros_msg); + void global_acceleration_maneuver(const ateam_msgs::msg::RobotMotionCommand & ros_msg); - private: - void finalize_command(RobotMoveCommand * robot_move_command, ateam_msgs::msg::GameStateRobot robot); + // Local Maneuvers + void local_velocity_maneuver(const ateam_msgs::msg::RobotMotionCommand & ros_msg, ateam_msgs::msg::GameStateRobot robot); + void local_acceleration_maneuver(const ateam_msgs::msg::RobotMotionCommand & ros_msg, ateam_msgs::msg::GameStateRobot robot); + + void finalize_command(RobotMoveCommand * robot_move_command, const ateam_msgs::msg::RobotMotionCommand & ros_msg, ateam_msgs::msg::GameStateRobot robot); ateam_msgs::msg::Twist2D apply_xy_motion_limits(ateam_msgs::msg::Twist2D prev, ateam_msgs::msg::Twist2D command, double vel_limit, double acc_limit, double dt); double apply_1d_motion_limits(double prev, double commanded, double vel_limit, double acc_limit, double dt); double get_dt(); - ateam_msgs::msg::Twist2D rotate_frame(ateam_msgs::msg::Twist2D input_frame, geometry_msgs::msg::Pose pose); + double get_yaw(geometry_msgs::msg::Pose pose); ateam_msgs::msg::Twist2D rotate_frame(ateam_msgs::msg::Twist2D input_frame, double angle); TrajectoryParams_t generate_trajectory_params(const ateam_msgs::msg::RobotMotionCommand & ros_msg); + bool use_trajectory_angle(); BangBangTraj3D_t trajectory_; - Vector6C_t trajectory_state_; // Predicted location of robot - ateam_msgs::msg::RobotMotionCommand command_; // Current command, updates with every new message - ateam_msgs::msg::Twist2D target_pose_; // Current target point, only updates when moving far enough to limit replanning + TrajectoryParams_t traj_params_; + ateam_msgs::msg::RobotMotionCommand command_; // Current ros command, updates with every new message - ateam_msgs::msg::Twist2D prev_body_command_; + ateam_msgs::msg::Twist2D current_global_command_; // Velocity to be commanded this frame + ateam_msgs::msg::Twist2D prev_global_command_; // Velocity commanded previous frame std::chrono::steady_clock::time_point prev_update_time_; }; diff --git a/radio/ateam_ssl_simulation_radio_bridge/src/ssl_simulation_radio_bridge_node.cpp b/radio/ateam_ssl_simulation_radio_bridge/src/ssl_simulation_radio_bridge_node.cpp index 6f8563f34..5b6073410 100644 --- a/radio/ateam_ssl_simulation_radio_bridge/src/ssl_simulation_radio_bridge_node.cpp +++ b/radio/ateam_ssl_simulation_radio_bridge/src/ssl_simulation_radio_bridge_node.cpp @@ -64,6 +64,7 @@ class SSLSimulationRadioBridgeNode : public rclcpp::Node declare_parameter("ssl_sim_control_port", 10300); declare_parameter("ssl_sim_blue_port", 10301); declare_parameter("ssl_sim_yellow_port", 10302); + declare_parameter("command_timeout_ms", 100); team_color_change_callback(ateam_common::TeamColor::Unknown); @@ -97,13 +98,10 @@ class SSLSimulationRadioBridgeNode : public rclcpp::Node std::placeholders::_1, std::placeholders::_2)); std::ranges::fill(command_timestamps_, std::chrono::steady_clock::now()); - zero_command_timer_ = + send_command_timer_ = create_wall_timer( - std::chrono::milliseconds( - declare_parameter( - "command_timeout_ms", - 100)), - std::bind(&SSLSimulationRadioBridgeNode::zero_command_timer_callback, this)); + std::chrono::milliseconds(10), + std::bind(&SSLSimulationRadioBridgeNode::send_command_timer_callback, this)); udp_sim_control_ = std::make_unique( @@ -194,7 +192,7 @@ class SSLSimulationRadioBridgeNode : public rclcpp::Node int robot_id) { command_timestamps_[robot_id] = std::chrono::steady_clock::now(); - send_command(*robot_commands_msg, robot_id); + commands_[robot_id] = *robot_commands_msg; } void feedback_callback(const uint8_t * buffer, size_t bytes_received) @@ -219,18 +217,7 @@ class SSLSimulationRadioBridgeNode : public rclcpp::Node } } - void send_zero_command(const int id) - { - ateam_msgs::msg::RobotMotionCommand msg; - msg.dribbler_speed = 0.0; - msg.kick_request = ateam_msgs::msg::RobotMotionCommand::KR_DISABLE; - msg.velocity.x = 0.0; - msg.velocity.y = 0.0; - msg.velocity.theta = 0.0; - send_command(msg, id); - } - - void zero_command_timer_callback() + void send_command_timer_callback() { const auto timeout_duration = std::chrono::milliseconds( get_parameter( @@ -238,8 +225,12 @@ class SSLSimulationRadioBridgeNode : public rclcpp::Node const auto timeout_time = std::chrono::steady_clock::now() - timeout_duration; for (auto id = 0; id < 16; ++id) { if (command_timestamps_[id] < timeout_time) { - send_zero_command(id); + ateam_msgs::msg::RobotMotionCommand zero_command = ateam_msgs::msg::RobotMotionCommand(); + zero_command.kick_request = ateam_msgs::msg::RobotMotionCommand::KR_DISABLE; + commands_[id] = zero_command; } + + send_command(commands_[id], id); } } @@ -255,10 +246,11 @@ class SSLSimulationRadioBridgeNode : public rclcpp::Node 16> connection_publishers_; rclcpp::Subscription::SharedPtr world_subscription_; ateam_msgs::msg::GameStateWorld world_; + std::array commands_; std::array manuever_executors_; rclcpp::Service::SharedPtr send_simulator_control_service_; - rclcpp::TimerBase::SharedPtr zero_command_timer_; + rclcpp::TimerBase::SharedPtr send_command_timer_; std::array command_timestamps_; }; From 6f8d37a24ed0e4315a68f05b261fd7c06b397cf6 Mon Sep 17 00:00:00 2001 From: Matthew Woodward Date: Tue, 23 Jun 2026 23:12:06 -0400 Subject: [PATCH 16/16] Working maneuvers including pivot --- ateam_kenobi/src/skills/capture.cpp | 2 + ateam_msgs/msg/RobotMotionCommand.msg | 1 + motion/ateam_controls/controls | 2 +- .../scripts/pivot_test.py | 128 +++++ .../src/message_conversions.cpp | 5 +- .../src/message_conversions.hpp | 3 +- .../src/pid.hpp | 93 +++ .../src/robot_maneuvers.cpp | 528 +++++++++++------- .../src/robot_maneuvers.hpp | 128 +++-- .../src/ssl_simulation_radio_bridge_node.cpp | 14 +- 10 files changed, 663 insertions(+), 241 deletions(-) create mode 100755 motion/ateam_controls_testing/scripts/pivot_test.py create mode 100644 radio/ateam_ssl_simulation_radio_bridge/src/pid.hpp diff --git a/ateam_kenobi/src/skills/capture.cpp b/ateam_kenobi/src/skills/capture.cpp index dff715ff1..20bc3c788 100644 --- a/ateam_kenobi/src/skills/capture.cpp +++ b/ateam_kenobi/src/skills/capture.cpp @@ -109,11 +109,13 @@ RobotCommand Capture::runCapture(const World & world, const Robot & robot) if(world.ball.visible) { motion::intents::LinearVelocityAngularFacing intent; + intent.frame = ateam_kenobi::motion::Frame::Local; intent.linear = ateam_geometry::Vector{capture_speed_, 0.0}; intent.face_target = world.ball.pos; command.motion_intent = intent; } else { motion::intents::Velocity intent; + intent.frame = ateam_kenobi::motion::Frame::Local; intent.linear = ateam_geometry::Vector{capture_speed_, 0.0}; intent.angular = 0.0; command.motion_intent = intent; diff --git a/ateam_msgs/msg/RobotMotionCommand.msg b/ateam_msgs/msg/RobotMotionCommand.msg index 0b66fc9ad..e2249ec31 100644 --- a/ateam_msgs/msg/RobotMotionCommand.msg +++ b/ateam_msgs/msg/RobotMotionCommand.msg @@ -8,6 +8,7 @@ float64 limit_acc_linear # (m/s/s) float64 limit_acc_angular # (rad/s/s) # pivot parameters +float64 pivot_global_theta # (rad) float64 pivot_orbit_radius # (m) float64 pivot_inset_angle # (rad) diff --git a/motion/ateam_controls/controls b/motion/ateam_controls/controls index 2adc62d7f..f56b0ab3a 160000 --- a/motion/ateam_controls/controls +++ b/motion/ateam_controls/controls @@ -1 +1 @@ -Subproject commit 2adc62d7f057e7551c359f21b213f716710eb4e3 +Subproject commit f56b0ab3a0fb9be7c7df15c15214bb0302d23167 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 000000000..758e6bbb1 --- /dev/null +++ b/motion/ateam_controls_testing/scripts/pivot_test.py @@ -0,0 +1,128 @@ +#! /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 +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 + +linear_threshold = 10 +angular_threshold = 0.0349 + +# x, y, theta, orbit_radius, inset angle, hold time +waypoints = [ + (-0.5, -0.5, 0.0, 0.5, 0.0, 1.0), + (-0.5, -0.5, math.pi, 0.5, 0.0, 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_waypoint_command(index: int): + waypoint = waypoints[index] + command_msg = RobotMotionCommand() + command_msg.body_control_mode = RobotMotionCommand.BCM_PIVOT + command_msg.pose.x = waypoint[0] + command_msg.pose.y = waypoint[1] + command_msg.pose.theta = waypoint[2] + command_msg.kick_request = RobotMotionCommand.KR_DISABLE + command_msg.pivot_global_theta = waypoint[2] + command_msg.pivot_orbit_radius = waypoint[3] + command_msg.pivot_inset_angle = waypoint[4] + 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_pub.publish(command_msg) + + +def is_at_waypoint(index: int): + waypoint = waypoints[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(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 + ) + + +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_waypoint_command(0) + if vision_robot_state_msg is None: + continue + if is_at_waypoint(0): + node.get_logger().info('Reached waypoint 0. Starting cycle.') + break + + while rclpy.ok(): + rclpy.spin_once(node) + 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]: + current_index = (current_index + 1) % len(waypoints) + waypoint_hold_start_time = None + node.get_logger().info(f'Moving to waypoint {current_index}') + publish_waypoint_command(current_index) + + node.destroy_node() + rclpy.shutdown() diff --git a/radio/ateam_ssl_simulation_radio_bridge/src/message_conversions.cpp b/radio/ateam_ssl_simulation_radio_bridge/src/message_conversions.cpp index 0d3de493d..25071396e 100644 --- a/radio/ateam_ssl_simulation_radio_bridge/src/message_conversions.cpp +++ b/radio/ateam_ssl_simulation_radio_bridge/src/message_conversions.cpp @@ -19,12 +19,12 @@ // THE SOFTWARE. #include "message_conversions.hpp" -#include "robot_maneuvers.hpp" #include #include #include #include #include +#include "robot_maneuvers.hpp" namespace ateam_ssl_simulation_radio_bridge::message_conversions { @@ -54,7 +54,8 @@ double ReplaceNanWithZero(const double val, rclcpp::Logger logger) RobotControl fromMsg( const ateam_msgs::msg::RobotMotionCommand & ros_msg, ateam_msgs::msg::GameStateRobot robot, - ateam_ssl_simulation_radio_bridge::robot_maneuvers::ManeuverExecutor & maneuver_executor, rclcpp::Logger logger) + ateam_ssl_simulation_radio_bridge::robot_maneuvers::ManeuverExecutor & maneuver_executor, + rclcpp::Logger logger) { RobotControl robots_control; RobotCommand * proto_robot_command = robots_control.add_robot_commands(); diff --git a/radio/ateam_ssl_simulation_radio_bridge/src/message_conversions.hpp b/radio/ateam_ssl_simulation_radio_bridge/src/message_conversions.hpp index 24ddd9951..f21cef212 100644 --- a/radio/ateam_ssl_simulation_radio_bridge/src/message_conversions.hpp +++ b/radio/ateam_ssl_simulation_radio_bridge/src/message_conversions.hpp @@ -42,7 +42,8 @@ ateam_radio_msgs::msg::BasicTelemetry fromProto(const RobotFeedback & proto_msg) RobotControl fromMsg( const ateam_msgs::msg::RobotMotionCommand & ros_msg, ateam_msgs::msg::GameStateRobot robot, - ateam_ssl_simulation_radio_bridge::robot_maneuvers::ManeuverExecutor & maneuver_executor, rclcpp::Logger logger); + ateam_ssl_simulation_radio_bridge::robot_maneuvers::ManeuverExecutor & maneuver_executor, + rclcpp::Logger logger); SimulatorControl fromMsg(const ssl_league_msgs::msg::SimulatorControl & ros_msg); diff --git a/radio/ateam_ssl_simulation_radio_bridge/src/pid.hpp b/radio/ateam_ssl_simulation_radio_bridge/src/pid.hpp new file mode 100644 index 000000000..e0a262b9a --- /dev/null +++ b/radio/ateam_ssl_simulation_radio_bridge/src/pid.hpp @@ -0,0 +1,93 @@ +// 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. + +#ifndef PID_HPP_ +#define PID_HPP_ + +#include +#include + +class PID +{ +public: + PID() = default; + PID(double p, double i, double d, double i_max = 0, double i_min = 0) + { + set_gains(p, i, d, i_max, i_min); + } + + + double compute_command(const double error, const double dt) + { + if (std::isnan(error)) { + prev_error_ = 0; + return 0; + } + + integral_ += error * dt; + if (use_antiwindup_) { + integral_ = std::clamp(integral_, i_min_, i_max_); + } + + double derivative = (error - prev_error_) / dt; + + double command = (kp_ * error) + (ki_ * integral_) + (kd_ * derivative); + prev_error_ = error; + + return command; + } + + void set_gains(double p, double i, double d, double i_max = 0, double i_min = 0) + { + kp_ = p; + ki_ = i; + kd_ = d; + + if (i_min == 0 && i_max == 0) { + use_antiwindup_ = false; + } else { + use_antiwindup_ = true; + i_min_ = i_min; + i_max_ = i_max; + } + + integral_ = 0; + } + + void reset() + { + integral_ = 0; + prev_error_ = 0; + } + +private: + double kp_ = 0; + double ki_ = 0; + double kd_ = 0; + + bool use_antiwindup_ = false; + double i_max_ = 0; + double i_min_ = 0; + + double prev_error_ = 0; + double integral_ = 0; +}; + +#endif // PID_HPP_ diff --git a/radio/ateam_ssl_simulation_radio_bridge/src/robot_maneuvers.cpp b/radio/ateam_ssl_simulation_radio_bridge/src/robot_maneuvers.cpp index 13c499a2e..03cfb92e0 100644 --- a/radio/ateam_ssl_simulation_radio_bridge/src/robot_maneuvers.cpp +++ b/radio/ateam_ssl_simulation_radio_bridge/src/robot_maneuvers.cpp @@ -1,3 +1,23 @@ +// 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. + #include #include "robot_maneuvers.hpp" #include @@ -5,244 +25,376 @@ namespace ateam_ssl_simulation_radio_bridge::robot_maneuvers { - void ManeuverExecutor::execute_maneuver(RobotMoveCommand * robot_move_command, const ateam_msgs::msg::RobotMotionCommand & ros_msg, ateam_msgs::msg::GameStateRobot robot) { - switch(ros_msg.body_control_mode) { - case ateam_msgs::msg::RobotMotionCommand::BCM_OFF: - bcm_off_maneuver(); - return; // Ignores acceleration limits set in message and immediately stops the robot +void ManeuverExecutor::execute_maneuver( + RobotMoveCommand * robot_move_command, + const ateam_msgs::msg::RobotMotionCommand & ros_msg, ateam_msgs::msg::GameStateRobot robot) +{ + switch(ros_msg.body_control_mode) { + case ateam_msgs::msg::RobotMotionCommand::BCM_OFF: + bcm_off_maneuver(); + return; // Ignores acceleration limits set in message and immediately stops the robot // Trajectory Maneuvers - case ateam_msgs::msg::RobotMotionCommand::BCM_GLOBAL_POSITION: - global_position_maneuver(ros_msg, robot); - break; + case ateam_msgs::msg::RobotMotionCommand::BCM_GLOBAL_POSITION: + case ateam_msgs::msg::RobotMotionCommand::BCM_PIVOT: + trajectory_maneuver(ros_msg, robot); + break; // Global Maneuvers - case ateam_msgs::msg::RobotMotionCommand::BCM_GLOBAL_VELOCITY: - global_velocity_maneuver(ros_msg); - break; - case ateam_msgs::msg::RobotMotionCommand::BCM_GLOBAL_ACCEL: - global_acceleration_maneuver(ros_msg); - break; + case ateam_msgs::msg::RobotMotionCommand::BCM_GLOBAL_VELOCITY: + global_velocity_maneuver(ros_msg); + break; + case ateam_msgs::msg::RobotMotionCommand::BCM_GLOBAL_ACCEL: + global_acceleration_maneuver(ros_msg); + break; // Local Maneuvers - case ateam_msgs::msg::RobotMotionCommand::BCM_LOCAL_VELOCITY: - local_velocity_maneuver(ros_msg, robot); - break; - case ateam_msgs::msg::RobotMotionCommand::BCM_LOCAL_ACCEL: - local_acceleration_maneuver(ros_msg, robot); - break; - } - - finalize_command(robot_move_command, ros_msg, robot); + case ateam_msgs::msg::RobotMotionCommand::BCM_LOCAL_VELOCITY: + local_velocity_maneuver(ros_msg, robot); + break; + case ateam_msgs::msg::RobotMotionCommand::BCM_LOCAL_ACCEL: + local_acceleration_maneuver(ros_msg, robot); + break; } - void ManeuverExecutor::bcm_off_maneuver() { - current_global_command_ = ateam_msgs::msg::Twist2D(); - prev_global_command_ = ateam_msgs::msg::Twist2D(); - prev_update_time_ = std::chrono::steady_clock::now(); - } + finalize_command(robot_move_command, ros_msg, robot); +} - // Trajectory Maneuvers - void ManeuverExecutor::global_position_maneuver(const ateam_msgs::msg::RobotMotionCommand & ros_msg, ateam_msgs::msg::GameStateRobot robot) { +void ManeuverExecutor::bcm_off_maneuver() +{ + current_global_command_ = ateam_msgs::msg::Twist2D(); + prev_global_command_ = ateam_msgs::msg::Twist2D(); + prev_update_time_ = std::chrono::steady_clock::now(); +} - // Update trajectory state to current time - float dt = float(get_dt()); - ateam_controls_tick_traj(&trajectory_, dt); + // Trajectory Maneuvers +void ManeuverExecutor::trajectory_maneuver( + const ateam_msgs::msg::RobotMotionCommand & ros_msg, + ateam_msgs::msg::GameStateRobot robot) +{ + // Update trajectory state to current time + float dt = static_cast(get_dt()); + tick_trajectory(dt); + Vector6C_t state = get_trajectory_state(); - double yaw = get_yaw(robot.pose); + double vision_yaw = get_yaw(robot.pose); // Check current position against predicted position - double current_dist = std::hypot(robot.pose.position.x - trajectory_.state.data[0], robot.pose.position.y - trajectory_.state.data[1]); - bool current_pos_needs_replan = current_dist > 0.5 || angles::shortest_angular_distance(yaw, trajectory_.state.data[2]) > 0.3; - - bool target_changed = std::hypot(command_.pose.x - ros_msg.pose.x, command_.pose.y - ros_msg.pose.y) > 0.02 || angles::shortest_angular_distance(command_.pose.theta, ros_msg.pose.theta) > 0.1; - - // bool maneuver_command_changed = (command_ != ros_msg); - bool maneuver_command_changed = (command_.body_control_mode != ateam_msgs::msg::RobotMotionCommand::BCM_GLOBAL_POSITION || target_changed); - - bool replanning = maneuver_command_changed || current_pos_needs_replan; - - if (replanning) { - std::cerr << "replanning id " << robot.id << ": (" << maneuver_command_changed << ", " << current_pos_needs_replan << ")" << std::endl; - Vector6C_t starting_state = Vector6C_t{ - float(robot.pose.position.x), - float(robot.pose.position.y), - float(yaw), - // float(robot.velocity.linear.x), - // float(robot.velocity.linear.y), - // float(robot.velocity.angular.z) - float(prev_global_command_.x), - float(prev_global_command_.y), - float(prev_global_command_.theta), - }; - - const Vector3C_t target_pose{ - float(ros_msg.pose.x), - float(ros_msg.pose.y), - float(ros_msg.pose.theta) + double current_dist = std::hypot(robot.pose.position.x - state.data[0], + robot.pose.position.y - state.data[1]); + bool current_pos_needs_replan = current_dist > 0.5 || + angles::shortest_angular_distance(vision_yaw, state.data[2]) > 0.3; + + bool maneuver_command_changed = (command_ != ros_msg); + bool replanning = maneuver_command_changed || current_pos_needs_replan; + if (replanning) { + Vector6C_t starting_state; + + // If previous command used trajectory controller initialize from the trajectory state + if (command_uses_trajectory()) { + starting_state = state; + } else { + // Previously commanded velocity is more accurate than vision estimate + starting_state = Vector6C_t{ + static_cast(robot.pose.position.x), + static_cast(robot.pose.position.y), + static_cast(vision_yaw), + static_cast(prev_global_command_.x), + static_cast(prev_global_command_.y), + static_cast(prev_global_command_.theta), }; - - command_ = ros_msg; - traj_params_ = generate_trajectory_params(ros_msg); - ateam_controls_traj_from_target_pose( - starting_state, - target_pose, - traj_params_, - &trajectory_ - ); } - ateam_msgs::msg::Twist2D global_target_err = ateam_msgs::msg::Twist2D(); - global_target_err.x = command_.pose.x - robot.pose.position.x; - global_target_err.y = command_.pose.y - robot.pose.position.y; + command_ = ros_msg; + plan_trajectory(starting_state); + state = get_trajectory_state(); - const double kpPos = 1.0; - const double kdPos = 0.0; - const double kpAngle = 0.0; + pid_x_traj_.reset(); + pid_y_traj_.reset(); + pid_theta_traj_.reset(); - ateam_msgs::msg::Twist2D global_feedback = ateam_msgs::msg::Twist2D(); + pid_x_target_.reset(); + pid_y_target_.reset(); + pid_theta_target_.reset(); + } - // Provide some minor xy feedback to account for slight final position offsets - if (abs(global_target_err.x) < 0.05) { - global_feedback.x += std::clamp(kpPos * global_target_err.x, -0.05, 0.05); - } - if (abs(global_target_err.y) < 0.05) { - global_feedback.y += std::clamp(kpPos * global_target_err.y, -0.05, 0.05); - } + ateam_msgs::msg::Twist2D global_target_err = ateam_msgs::msg::Twist2D(); + global_target_err.x = command_.pose.x - robot.pose.position.x; + global_target_err.y = command_.pose.y - robot.pose.position.y; + global_target_err.theta = angles::shortest_angular_distance(command_.pose.theta, + get_yaw(robot.pose)); + + ateam_msgs::msg::Twist2D global_traj_err = ateam_msgs::msg::Twist2D(); + global_traj_err.x = state.data[0] - robot.pose.position.x; + global_traj_err.y = state.data[1] - robot.pose.position.y; + global_traj_err.theta = angles::shortest_angular_distance(state.data[2], get_yaw(robot.pose)); + + ateam_msgs::msg::Twist2D global_feedforward = ateam_msgs::msg::Twist2D(); + ateam_msgs::msg::Twist2D global_feedback = ateam_msgs::msg::Twist2D(); + + // Default Control Behavior + global_feedforward.x = state.data[3]; + global_feedforward.y = state.data[4]; + global_feedforward.theta = state.data[5]; + + global_feedback.x = pid_x_traj_.compute_command(global_traj_err.x, dt); + global_feedback.y = pid_y_traj_.compute_command(global_traj_err.y, dt); + global_feedback.theta = pid_theta_traj_.compute_command(global_traj_err.theta, dt); + + // Custom trajectory control behavior + switch(command_.body_control_mode) { + case ateam_msgs::msg::RobotMotionCommand::BCM_GLOBAL_POSITION: + { + // Handle slight xy overshoot and final position offset due to vision filter delay + const double threshold = 0.05; + if (abs(global_target_err.x) < threshold) { + global_feedforward.x = 0.0; + global_feedback.x = pid_x_target_.compute_command(global_target_err.x, dt); + } + + if (abs(global_target_err.y) < threshold) { + global_feedforward.y = 0.0; + global_feedback.y = pid_y_target_.compute_command(global_target_err.y, dt); + } + break; + } - current_global_command_.x = trajectory_.state.data[3] + global_feedback.x; - current_global_command_.y = trajectory_.state.data[4] + global_feedback.y; - current_global_command_.theta = trajectory_.state.data[5] + global_feedback.theta; + case ateam_msgs::msg::RobotMotionCommand::BCM_PIVOT: + global_feedback.x = 0.0; + global_feedback.y = 0.0; + break; } + current_global_command_.x = global_feedforward.x + global_feedback.x; + current_global_command_.y = global_feedforward.y + global_feedback.y; + current_global_command_.theta = global_feedforward.theta + global_feedback.theta; +} + // Global Maneuvers - void ManeuverExecutor::global_velocity_maneuver(const ateam_msgs::msg::RobotMotionCommand & ros_msg) { - current_global_command_ = ros_msg.velocity; - std::cerr << "global command, global vel: " << current_global_command_.x << ", " << current_global_command_.y << ", " << current_global_command_.theta << std::endl; - } +void ManeuverExecutor::global_velocity_maneuver(const ateam_msgs::msg::RobotMotionCommand & ros_msg) +{ + current_global_command_ = ros_msg.velocity; +} - void ManeuverExecutor::global_acceleration_maneuver(const ateam_msgs::msg::RobotMotionCommand & ros_msg) { - const double dt = get_dt(); +void ManeuverExecutor::global_acceleration_maneuver( + const ateam_msgs::msg::RobotMotionCommand & ros_msg) +{ + const double dt = get_dt(); - current_global_command_.x = prev_global_command_.x + dt * ros_msg.acceleration.x; - current_global_command_.y = prev_global_command_.y + dt * ros_msg.acceleration.y; - current_global_command_.theta = prev_global_command_.theta + dt * ros_msg.acceleration.theta; - } + current_global_command_.x = prev_global_command_.x + dt * ros_msg.acceleration.x; + current_global_command_.y = prev_global_command_.y + dt * ros_msg.acceleration.y; + current_global_command_.theta = prev_global_command_.theta + dt * ros_msg.acceleration.theta; +} // Local Maneuvers - void ManeuverExecutor::local_velocity_maneuver(const ateam_msgs::msg::RobotMotionCommand & ros_msg, ateam_msgs::msg::GameStateRobot robot) { - current_global_command_ = rotate_frame(ros_msg.velocity, -get_yaw(robot.pose)); - std::cerr << "local command, global vel: " << current_global_command_.x << ", " << current_global_command_.y << ", " << current_global_command_.theta << std::endl; - } - - void ManeuverExecutor::local_acceleration_maneuver(const ateam_msgs::msg::RobotMotionCommand & ros_msg, ateam_msgs::msg::GameStateRobot robot) { - ateam_msgs::msg::Twist2D global_accel = rotate_frame(ros_msg.acceleration, -get_yaw(robot.pose)); - const double dt = get_dt(); +void ManeuverExecutor::local_velocity_maneuver( + const ateam_msgs::msg::RobotMotionCommand & ros_msg, + ateam_msgs::msg::GameStateRobot robot) +{ + current_global_command_ = rotate_frame(ros_msg.velocity, -get_yaw(robot.pose)); +} - current_global_command_.x = prev_global_command_.x + dt * global_accel.x; - current_global_command_.y = prev_global_command_.y + dt * global_accel.y; - current_global_command_.theta = prev_global_command_.theta + dt * global_accel.theta; - } +void ManeuverExecutor::local_acceleration_maneuver( + const ateam_msgs::msg::RobotMotionCommand & ros_msg, ateam_msgs::msg::GameStateRobot robot) +{ + ateam_msgs::msg::Twist2D global_accel = rotate_frame(ros_msg.acceleration, -get_yaw(robot.pose)); + const double dt = get_dt(); - void ManeuverExecutor::finalize_command(RobotMoveCommand * robot_move_command, const ateam_msgs::msg::RobotMotionCommand & ros_msg, ateam_msgs::msg::GameStateRobot robot) { - command_ = ros_msg; - double dt = get_dt(); + current_global_command_.x = prev_global_command_.x + dt * global_accel.x; + current_global_command_.y = prev_global_command_.y + dt * global_accel.y; + current_global_command_.theta = prev_global_command_.theta + dt * global_accel.theta; +} +void ManeuverExecutor::finalize_command( + RobotMoveCommand * robot_move_command, + const ateam_msgs::msg::RobotMotionCommand & ros_msg, ateam_msgs::msg::GameStateRobot robot) +{ + command_ = ros_msg; + double dt = get_dt(); + + ateam_msgs::msg::Twist2D local_command; + if (command_uses_trajectory()) { + // Motion limits for trajectory type maneuvers are mostly handled by the planner + Vector6C_t state = get_trajectory_state(); + local_command = rotate_frame(current_global_command_, state.data[2]); + } else { + // Handle non-trajectory type maneuver motion limits traj_params_ = generate_trajectory_params(command_); - current_global_command_ = apply_xy_motion_limits(prev_global_command_, current_global_command_, traj_params_.max_vel_linear, traj_params_.max_accel_linear, dt); - current_global_command_.theta = apply_1d_motion_limits(prev_global_command_.theta, current_global_command_.theta, traj_params_.max_vel_angular, traj_params_.max_accel_angular, dt); + current_global_command_ = apply_xy_motion_limits(prev_global_command_, current_global_command_, + traj_params_.max_vel_linear, traj_params_.max_accel_linear, dt); + current_global_command_.theta = apply_1d_motion_limits(prev_global_command_.theta, + current_global_command_.theta, traj_params_.max_vel_angular, traj_params_.max_accel_angular, + dt); - ateam_msgs::msg::Twist2D local_command; - if (use_trajectory_angle()) { - local_command = rotate_frame(current_global_command_, trajectory_.state.data[2]); - } else { - local_command = rotate_frame(current_global_command_, get_yaw(robot.pose)); - } + local_command = rotate_frame(current_global_command_, get_yaw(robot.pose)); + } - MoveLocalVelocity * local_velocity_command = robot_move_command->mutable_local_velocity(); - local_velocity_command->set_forward(local_command.x); - local_velocity_command->set_left(local_command.y); - local_velocity_command->set_angular(local_command.theta); + MoveLocalVelocity * local_velocity_command = robot_move_command->mutable_local_velocity(); + local_velocity_command->set_forward(local_command.x); + local_velocity_command->set_left(local_command.y); + local_velocity_command->set_angular(local_command.theta); - prev_global_command_ = current_global_command_; - prev_update_time_ = std::chrono::steady_clock::now(); - } + prev_global_command_ = current_global_command_; + prev_update_time_ = std::chrono::steady_clock::now(); +} - ateam_msgs::msg::Twist2D ManeuverExecutor::apply_xy_motion_limits(ateam_msgs::msg::Twist2D prev, ateam_msgs::msg::Twist2D command, double vel_limit, double acc_limit, double dt) { - ateam_geometry::Vector prev_xy{prev.x, prev.y}; - ateam_geometry::Vector command_xy{command.x, command.y}; - ateam_geometry::Vector requested_acceleration = command_xy - prev_xy; +ateam_msgs::msg::Twist2D ManeuverExecutor::apply_xy_motion_limits( + ateam_msgs::msg::Twist2D prev, + ateam_msgs::msg::Twist2D command, double vel_limit, double acc_limit, double dt) +{ + ateam_geometry::Vector prev_xy{prev.x, prev.y}; + ateam_geometry::Vector command_xy{command.x, command.y}; + ateam_geometry::Vector requested_acceleration = command_xy - prev_xy; - double acceleration_magnitude = std::clamp(ateam_geometry::norm(requested_acceleration), 0.0, dt * acc_limit); - ateam_geometry::Vector acc_limited_command = prev_xy + (acceleration_magnitude * ateam_geometry::normalize(requested_acceleration)); + double acceleration_magnitude = std::clamp(ateam_geometry::norm(requested_acceleration), 0.0, + dt * acc_limit); + ateam_geometry::Vector acc_limited_command = prev_xy + + (acceleration_magnitude * ateam_geometry::normalize(requested_acceleration)); - double final_magnitude = std::clamp(ateam_geometry::norm(acc_limited_command), 0.0, vel_limit); - ateam_geometry::Vector final_xy = final_magnitude * ateam_geometry::normalize(acc_limited_command); + double final_magnitude = std::clamp(ateam_geometry::norm(acc_limited_command), 0.0, vel_limit); + ateam_geometry::Vector final_xy = final_magnitude * + ateam_geometry::normalize(acc_limited_command); - command.x = final_xy.x(); - command.y = final_xy.y(); + command.x = final_xy.x(); + command.y = final_xy.y(); - return command; - } + return command; +} - double ManeuverExecutor::apply_1d_motion_limits(double prev, double command, double vel_limit, double acc_limit, double dt) { - double requested_acceleration = command - prev; - double acc_limited_command = prev + std::clamp(requested_acceleration, -dt * acc_limit, dt * acc_limit); - return std::clamp(acc_limited_command, -vel_limit, vel_limit); - } +double ManeuverExecutor::apply_1d_motion_limits( + double prev, double command, double vel_limit, + double acc_limit, double dt) +{ + double requested_acceleration = command - prev; + double acc_limited_command = prev + std::clamp(requested_acceleration, -dt * acc_limit, + dt * acc_limit); + return std::clamp(acc_limited_command, -vel_limit, vel_limit); +} - double ManeuverExecutor::get_dt() { - // const std::chrono::duration elapsed = std::chrono::steady_clock::now() - prev_update_time_; - // return elapsed.count(); - return 0.01; - } +double ManeuverExecutor::get_dt() +{ + // const std::chrono::duration elapsed = + // std::chrono::steady_clock::now() - prev_update_time_; + // return elapsed.count(); + return 0.01; +} + +double ManeuverExecutor::get_yaw(geometry_msgs::msg::Pose pose) +{ + tf2::Quaternion quat; + tf2::fromMsg(pose.orientation, quat); + double yaw, pitch, roll; + tf2::Matrix3x3(quat).getEulerYPR(yaw, pitch, roll); - double ManeuverExecutor::get_yaw(geometry_msgs::msg::Pose pose) { - tf2::Quaternion quat; - tf2::fromMsg(pose.orientation, quat); - double yaw, pitch, roll; - tf2::Matrix3x3(quat).getEulerYPR(yaw, pitch, roll); + return yaw; +} - return yaw; - } +ateam_msgs::msg::Twist2D ManeuverExecutor::rotate_frame( + ateam_msgs::msg::Twist2D input_frame, + double angle) +{ + ateam_msgs::msg::Twist2D output_frame = ateam_msgs::msg::Twist2D(); + output_frame.x = cos(-angle) * input_frame.x - sin(-angle) * input_frame.y; + output_frame.y = sin(-angle) * input_frame.x + cos(-angle) * input_frame.y; + output_frame.theta = input_frame.theta; + return output_frame; +} + +TrajectoryParams_t ManeuverExecutor::generate_trajectory_params( + const ateam_msgs::msg::RobotMotionCommand & ros_msg) +{ + TrajectoryParams_t traj_params = ateam_controls_default_traj_params(); - ateam_msgs::msg::Twist2D ManeuverExecutor::rotate_frame(ateam_msgs::msg::Twist2D input_frame, double angle) { - ateam_msgs::msg::Twist2D output_frame = ateam_msgs::msg::Twist2D(); - output_frame.x = cos(-angle) * input_frame.x - sin(-angle) * input_frame.y; - output_frame.y = sin(-angle) * input_frame.x + cos(-angle) * input_frame.y; - output_frame.theta = input_frame.theta; - return output_frame; + if (ros_msg.limit_vel_linear != 0.0) { + traj_params.max_vel_linear = ros_msg.limit_vel_linear; + } + if (ros_msg.limit_vel_angular != 0.0) { + traj_params.max_vel_angular = ros_msg.limit_vel_angular; + } + if (ros_msg.limit_acc_linear != 0.0) { + traj_params.max_accel_linear = ros_msg.limit_acc_linear; + } + if (ros_msg.limit_acc_angular != 0.0) { + traj_params.max_accel_angular = ros_msg.limit_acc_angular; } - TrajectoryParams_t ManeuverExecutor::generate_trajectory_params(const ateam_msgs::msg::RobotMotionCommand & ros_msg) { + return traj_params; +} - TrajectoryParams_t traj_params = ateam_controls_default_traj_params(); +bool ManeuverExecutor::command_uses_trajectory() +{ + switch(command_.body_control_mode) { + case ateam_msgs::msg::RobotMotionCommand::BCM_GLOBAL_POSITION: + case ateam_msgs::msg::RobotMotionCommand::BCM_PIVOT: + return true; - if (ros_msg.limit_vel_linear != 0.0) { - traj_params.max_vel_linear = ros_msg.limit_vel_linear; - } - if (ros_msg.limit_vel_angular != 0.0) { - traj_params.max_vel_angular = ros_msg.limit_vel_angular; - } - if (ros_msg.limit_acc_linear != 0.0) { - traj_params.max_accel_linear = ros_msg.limit_acc_linear; - } - if (ros_msg.limit_acc_angular != 0.0) { - traj_params.max_accel_angular = ros_msg.limit_acc_angular; - } + default: + return false; + } +} - // std::cerr << "traj limits: " << traj_params.max_vel_linear << ", " << traj_params.max_vel_angular << ", " << traj_params.max_accel_linear << ", " << traj_params.max_accel_angular << std::endl; - return traj_params; +Vector6C_t ManeuverExecutor::get_trajectory_state() +{ + switch(command_.body_control_mode) { + case ateam_msgs::msg::RobotMotionCommand::BCM_GLOBAL_POSITION: + return position_trajectory_.state; + case ateam_msgs::msg::RobotMotionCommand::BCM_PIVOT: + return pivot_trajectory_.state; + default: + return Vector6C_t(); } +} - bool ManeuverExecutor::use_trajectory_angle() { - switch(command_.body_control_mode) { - case ateam_msgs::msg::RobotMotionCommand::BCM_GLOBAL_POSITION: - case ateam_msgs::msg::RobotMotionCommand::BCM_PIVOT: - return true; +void ManeuverExecutor::plan_trajectory(Vector6C_t starting_state) +{ + switch (command_.body_control_mode) { + case ateam_msgs::msg::RobotMotionCommand::BCM_GLOBAL_POSITION: + { + const Vector3C_t target_pose{ + static_cast(command_.pose.x), + static_cast(command_.pose.y), + static_cast(command_.pose.theta) + }; + traj_params_ = generate_trajectory_params(command_); + ateam_controls_traj_from_target_pose( + starting_state, + target_pose, + traj_params_, + &position_trajectory_ + ); + break; + } + case ateam_msgs::msg::RobotMotionCommand::BCM_PIVOT: + { + pivot_params_ = ateam_controls_default_pivot_params(); + if (command_.limit_vel_angular != 0.0) { + pivot_params_.max_vel_angular = command_.limit_vel_angular; + } + if (command_.limit_acc_angular != 0.0) { + pivot_params_.max_accel_angular = command_.limit_acc_angular; + } + pivot_params_.orbit_radius = command_.pivot_orbit_radius; + pivot_params_.inset_angle = command_.pivot_inset_angle; + ateam_controls_pivot_traj_new( + starting_state, + command_.pivot_global_theta, + pivot_params_, + &pivot_trajectory_ + ); + break; + } + } +} - default: - return false; - } +void ManeuverExecutor::tick_trajectory(float dt) +{ + switch (command_.body_control_mode) { + case ateam_msgs::msg::RobotMotionCommand::BCM_GLOBAL_POSITION: + ateam_controls_traj_tick(&position_trajectory_, dt); + break; + case ateam_msgs::msg::RobotMotionCommand::BCM_PIVOT: + ateam_controls_pivot_traj_tick(&pivot_trajectory_, dt); + break; } -} // namespace ateam_ssl_simulation_radio_bridge::robot_maneuvers \ No newline at end of file +} +} // namespace ateam_ssl_simulation_radio_bridge::robot_maneuvers diff --git a/radio/ateam_ssl_simulation_radio_bridge/src/robot_maneuvers.hpp b/radio/ateam_ssl_simulation_radio_bridge/src/robot_maneuvers.hpp index 34c4c57b2..bd80c4787 100644 --- a/radio/ateam_ssl_simulation_radio_bridge/src/robot_maneuvers.hpp +++ b/radio/ateam_ssl_simulation_radio_bridge/src/robot_maneuvers.hpp @@ -21,68 +21,108 @@ #ifndef ROBOT_MANEUVERS_HPP_ #define ROBOT_MANEUVERS_HPP_ +#include +#include + #include #include #include -#include #include #include -#include -#include #include +#include "pid.hpp" #include "ateam_controls/ateam_controls.h" namespace ateam_ssl_simulation_radio_bridge::robot_maneuvers { - class ManeuverExecutor { - - public: - ManeuverExecutor() { - traj_params_ = ateam_controls_default_traj_params(); - prev_update_time_ = std::chrono::steady_clock::now(); - } - - void set_command(ateam_msgs::msg::RobotMotionCommand command) { - command_ = command; - } - - void execute_maneuver(RobotMoveCommand * robot_move_command, const ateam_msgs::msg::RobotMotionCommand & ros_msg, ateam_msgs::msg::GameStateRobot robot); - - private: - void bcm_off_maneuver(); +class ManeuverExecutor { +public: + ManeuverExecutor() + { + traj_params_ = ateam_controls_default_traj_params(); + prev_update_time_ = std::chrono::steady_clock::now(); + + pid_x_traj_ = PID(3.0, 0.0, 0.0); + pid_y_traj_ = PID(3.0, 0.0, 0.0); + pid_theta_traj_ = PID(0.0, 0.0, 0.0); + + pid_x_target_ = PID(4.0, 0.0, 0.001); + pid_y_target_ = PID(4.0, 0.0, 0.001); + pid_theta_target_ = PID(0.0, 0.0, 0.0); + } + + void set_command(ateam_msgs::msg::RobotMotionCommand command) + { + command_ = command; + } + + void execute_maneuver( + RobotMoveCommand * robot_move_command, + const ateam_msgs::msg::RobotMotionCommand & ros_msg, ateam_msgs::msg::GameStateRobot robot); + +private: + void bcm_off_maneuver(); // Trajectory Maneuvers - void global_position_maneuver(const ateam_msgs::msg::RobotMotionCommand & ros_msg, ateam_msgs::msg::GameStateRobot robot); + void trajectory_maneuver( + const ateam_msgs::msg::RobotMotionCommand & ros_msg, + ateam_msgs::msg::GameStateRobot robot); // Global Maneuvers - void global_velocity_maneuver(const ateam_msgs::msg::RobotMotionCommand & ros_msg); - void global_acceleration_maneuver(const ateam_msgs::msg::RobotMotionCommand & ros_msg); + void global_velocity_maneuver(const ateam_msgs::msg::RobotMotionCommand & ros_msg); + void global_acceleration_maneuver(const ateam_msgs::msg::RobotMotionCommand & ros_msg); // Local Maneuvers - void local_velocity_maneuver(const ateam_msgs::msg::RobotMotionCommand & ros_msg, ateam_msgs::msg::GameStateRobot robot); - void local_acceleration_maneuver(const ateam_msgs::msg::RobotMotionCommand & ros_msg, ateam_msgs::msg::GameStateRobot robot); - - void finalize_command(RobotMoveCommand * robot_move_command, const ateam_msgs::msg::RobotMotionCommand & ros_msg, ateam_msgs::msg::GameStateRobot robot); - - ateam_msgs::msg::Twist2D apply_xy_motion_limits(ateam_msgs::msg::Twist2D prev, ateam_msgs::msg::Twist2D command, double vel_limit, double acc_limit, double dt); - double apply_1d_motion_limits(double prev, double commanded, double vel_limit, double acc_limit, double dt); - - double get_dt(); - double get_yaw(geometry_msgs::msg::Pose pose); - ateam_msgs::msg::Twist2D rotate_frame(ateam_msgs::msg::Twist2D input_frame, double angle); - TrajectoryParams_t generate_trajectory_params(const ateam_msgs::msg::RobotMotionCommand & ros_msg); - bool use_trajectory_angle(); - - BangBangTraj3D_t trajectory_; - TrajectoryParams_t traj_params_; - ateam_msgs::msg::RobotMotionCommand command_; // Current ros command, updates with every new message - - ateam_msgs::msg::Twist2D current_global_command_; // Velocity to be commanded this frame - ateam_msgs::msg::Twist2D prev_global_command_; // Velocity commanded previous frame - std::chrono::steady_clock::time_point prev_update_time_; - }; + void local_velocity_maneuver( + const ateam_msgs::msg::RobotMotionCommand & ros_msg, + ateam_msgs::msg::GameStateRobot robot); + void local_acceleration_maneuver( + const ateam_msgs::msg::RobotMotionCommand & ros_msg, + ateam_msgs::msg::GameStateRobot robot); + + void finalize_command( + RobotMoveCommand * robot_move_command, + const ateam_msgs::msg::RobotMotionCommand & ros_msg, ateam_msgs::msg::GameStateRobot robot); + + ateam_msgs::msg::Twist2D apply_xy_motion_limits( + ateam_msgs::msg::Twist2D prev, + ateam_msgs::msg::Twist2D command, double vel_limit, double acc_limit, double dt); + double apply_1d_motion_limits( + double prev, double commanded, double vel_limit, double acc_limit, + double dt); + + double get_dt(); + double get_yaw(geometry_msgs::msg::Pose pose); + ateam_msgs::msg::Twist2D rotate_frame(ateam_msgs::msg::Twist2D input_frame, double angle); + + TrajectoryParams_t generate_trajectory_params( + const ateam_msgs::msg::RobotMotionCommand & ros_msg); + bool command_uses_trajectory(); + Vector6C_t get_trajectory_state(); + void plan_trajectory(Vector6C_t starting_state); + void tick_trajectory(float dt); + + PID pid_x_traj_; + PID pid_y_traj_; + PID pid_theta_traj_; + + PID pid_x_target_; + PID pid_y_target_; + PID pid_theta_target_; + + ateam_msgs::msg::RobotMotionCommand command_; // Current ros command, updates every frame + ateam_msgs::msg::Twist2D current_global_command_; // Velocity to be commanded this frame + ateam_msgs::msg::Twist2D prev_global_command_; // Velocity commanded previous frame + std::chrono::steady_clock::time_point prev_update_time_; + + BangBangTraj3D_t position_trajectory_; + TrajectoryParams_t traj_params_; + + PivotTrajectory_t pivot_trajectory_; + PivotParams_t pivot_params_; +}; } // namespace ateam_ssl_simulation_radio_bridge::robot_maneuvers diff --git a/radio/ateam_ssl_simulation_radio_bridge/src/ssl_simulation_radio_bridge_node.cpp b/radio/ateam_ssl_simulation_radio_bridge/src/ssl_simulation_radio_bridge_node.cpp index 5b6073410..3785dac43 100644 --- a/radio/ateam_ssl_simulation_radio_bridge/src/ssl_simulation_radio_bridge_node.cpp +++ b/radio/ateam_ssl_simulation_radio_bridge/src/ssl_simulation_radio_bridge_node.cpp @@ -164,7 +164,8 @@ class SSLSimulationRadioBridgeNode : public rclcpp::Node std::bind_front(&SSLSimulationRadioBridgeNode::feedback_callback, this)); } - void world_callback(const ateam_msgs::msg::GameStateWorld & msg) { + void world_callback(const ateam_msgs::msg::GameStateWorld & msg) + { world_ = msg; } @@ -175,10 +176,12 @@ class SSLSimulationRadioBridgeNode : public rclcpp::Node } const auto robot = world_.our_robots[robot_id]; - // Somehow nonvisible robots have their id set to 0 in the world topic, easier to just not interact with them + // Somehow nonvisible robots have their id set to 0 in the world topic + // easier to just not interact with them if (robot.visible) { - auto& maneuver_executor = manuever_executors_[robot_id]; - RobotControl robots_control = message_conversions::fromMsg(msg, robot, maneuver_executor, get_logger()); + auto & maneuver_executor = manuever_executors_[robot_id]; + RobotControl robots_control = message_conversions::fromMsg(msg, robot, maneuver_executor, + get_logger()); std::vector buffer; buffer.resize(robots_control.ByteSizeLong()); if (robots_control.SerializeToArray(buffer.data(), buffer.size())) { @@ -247,7 +250,8 @@ class SSLSimulationRadioBridgeNode : public rclcpp::Node rclcpp::Subscription::SharedPtr world_subscription_; ateam_msgs::msg::GameStateWorld world_; std::array commands_; - std::array manuever_executors_; + std::array manuever_executors_; rclcpp::Service::SharedPtr send_simulator_control_service_; rclcpp::TimerBase::SharedPtr send_command_timer_;