From 5453955a023df0b60a19d23ba7ba81c2bb9887da Mon Sep 17 00:00:00 2001 From: Matthew Barulic Date: Tue, 15 Jul 2025 15:32:58 -0300 Subject: [PATCH 01/35] Adds MotionIntent and MotionExecutor --- ateam_kenobi/CMakeLists.txt | 1 + .../src/core/motion/motion_controller.cpp | 4 + .../src/core/motion/motion_controller.hpp | 14 +- .../src/core/motion/motion_executor.cpp | 239 ++++++++++++++++++ .../src/core/motion/motion_executor.hpp | 66 +++++ .../src/core/motion/motion_intent.hpp | 123 +++++++++ .../src/core/motion/motion_options.hpp | 38 +++ ateam_kenobi/src/core/path_planning/path.hpp | 36 +++ .../src/core/path_planning/path_planner.cpp | 2 +- .../src/core/path_planning/path_planner.hpp | 55 +--- .../core/path_planning/planner_options.hpp | 81 ++++++ .../src/core/play_helpers/easy_move_to.cpp | 8 +- .../src/core/play_helpers/easy_move_to.hpp | 12 +- ateam_kenobi/src/core/types/robot_command.hpp | 53 ++++ ateam_kenobi/src/plays/halt_play.cpp | 3 +- ateam_kenobi/src/plays/halt_play.hpp | 4 - .../plays/test_plays/controls_test_play.cpp | 20 +- .../plays/test_plays/controls_test_play.hpp | 6 +- ateam_kenobi/src/skills/capture.cpp | 4 +- ateam_kenobi/src/skills/dribble.cpp | 2 +- ateam_kenobi/src/skills/intercept.cpp | 2 +- ateam_kenobi/src/skills/line_kick.cpp | 2 +- ateam_kenobi/src/tactics/multi_move_to.hpp | 10 +- ateam_kenobi/test/path_planner_test.cpp | 6 +- .../test/random_path_planner_test.cpp | 9 +- 25 files changed, 691 insertions(+), 109 deletions(-) create mode 100644 ateam_kenobi/src/core/motion/motion_executor.cpp create mode 100644 ateam_kenobi/src/core/motion/motion_executor.hpp create mode 100644 ateam_kenobi/src/core/motion/motion_intent.hpp create mode 100644 ateam_kenobi/src/core/motion/motion_options.hpp create mode 100644 ateam_kenobi/src/core/path_planning/path.hpp create mode 100644 ateam_kenobi/src/core/path_planning/planner_options.hpp create mode 100644 ateam_kenobi/src/core/types/robot_command.hpp diff --git a/ateam_kenobi/CMakeLists.txt b/ateam_kenobi/CMakeLists.txt index f5ceebb31..b2e2c8b6b 100644 --- a/ateam_kenobi/CMakeLists.txt +++ b/ateam_kenobi/CMakeLists.txt @@ -36,6 +36,7 @@ target_sources(kenobi_node_component PRIVATE src/core/path_planning/path_planner.cpp src/core/types/message_conversions.cpp src/core/motion/motion_controller.cpp + src/core/motion/motion_executor.cpp src/core/visualization/overlays.cpp src/core/play_helpers/easy_move_to.cpp src/core/play_helpers/lanes.cpp diff --git a/ateam_kenobi/src/core/motion/motion_controller.cpp b/ateam_kenobi/src/core/motion/motion_controller.cpp index 9d4aaed2c..3d6e70519 100644 --- a/ateam_kenobi/src/core/motion/motion_controller.cpp +++ b/ateam_kenobi/src/core/motion/motion_controller.cpp @@ -34,6 +34,8 @@ #include "ateam_common/robot_constants.hpp" #include "pid.hpp" +namespace ateam_kenobi::motion { + /* // PID gains CREATE_PARAM(double, "motion/pid/x_kp", x_kp, 2); @@ -478,3 +480,5 @@ void MotionController::set_t_pid_gains(double p, double i, double d, double i_mi { t_controller.set_gains(p, i, d, i_max, i_min); } + +} // namespace ateam_kenobi::motion diff --git a/ateam_kenobi/src/core/motion/motion_controller.hpp b/ateam_kenobi/src/core/motion/motion_controller.hpp index 6dc80f719..05ee03db2 100644 --- a/ateam_kenobi/src/core/motion/motion_controller.hpp +++ b/ateam_kenobi/src/core/motion/motion_controller.hpp @@ -30,6 +30,9 @@ #include "core/types/robot.hpp" #include "core/types/world.hpp" #include "pid.hpp" +#include "motion_options.hpp" + +namespace ateam_kenobi::motion { // cause the robot to: always face a point, face in the direction of travel, or stay facing the // same direction @@ -41,15 +44,6 @@ enum class AngleMode no_face }; -struct MotionOptions -{ - /// @brief radius around the end point that will be considered completed - double completion_threshold = .02; // meters - - /// @brief angle around the end point that will be considered completed - double angular_completion_threshold = 0.035; // radians -}; - /** * Generate robot motion commands to follow given trajectory, may handle extra features such as pointing at a location */ @@ -132,4 +126,6 @@ class MotionController PID t_controller; }; +} // namespace ateam_kenobi::motion + #endif // CORE__MOTION__MOTION_CONTROLLER_HPP_ diff --git a/ateam_kenobi/src/core/motion/motion_executor.cpp b/ateam_kenobi/src/core/motion/motion_executor.cpp new file mode 100644 index 000000000..10be53fdb --- /dev/null +++ b/ateam_kenobi/src/core/motion/motion_executor.cpp @@ -0,0 +1,239 @@ +// Copyright 2025 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 "motion_executor.hpp" +#include +#include "core/path_planning/obstacles.hpp" +#include "core/path_planning/escape_velocity.hpp" + +namespace ateam_kenobi::motion +{ + +// helper type for the visitor +template +struct overloads : Ts ... { using Ts::operator() ...; }; + +MotionExecutor::MotionExecutor(rclcpp::Logger logger) +: logger_(std::move(logger)) +{ + +} + +std::array, + 16> MotionExecutor::RunFrame( + std::array, 16> intents, + visualization::Overlays & overlays, const World & world) +{ + const auto current_time = std::chrono::duration_cast>( + world.current_time.time_since_epoch()).count(); + + std::array, 16> results; + + for (size_t i = 0; i < intents.size(); ++i) { + if (!intents[i]) { + results[i] = std::nullopt; + continue; + } + + const auto & robot = world.our_robots[i]; + auto & intent = intents[i].value(); + auto & planner = planners_[i]; + auto & controller = controllers_[i]; + + BodyVelocity body_velocity; + path_planning::Path path; + + bool use_controller_linvel = true; + std::visit(overloads{ + [&](const intents::None &) { + controller.reset_trajectory({robot.pos}); + }, + [&](const intents::linear::VelocityIntent & v) { + body_velocity.linear = v.velocity; + use_controller_linvel = false; + controller.reset_trajectory({robot.pos}); + }, + [&](const intents::linear::PositionIntent & p) { + path = planner.getPath(robot.pos, p.position, world, intent.obstacles, + intent.planner_options); + if(path.empty()) { + return; + } + if(planner.usedCachedPath()) { + controller.update_trajectory(path); + } else { + controller.reset_trajectory(path); + } + }, + [&](const intents::linear::VelocityAtPositionIntent & v) { + path = planner.getPath(robot.pos, v.position, world, intent.obstacles, + intent.planner_options); + if(path.empty()) { + return; + } + if(planner.usedCachedPath()) { + controller.update_trajectory(path, v.velocity); + } else { + controller.reset_trajectory(path, v.velocity); + } + } + }, intent.linear); + + bool use_controller_omega = true; + std::visit(overloads{ + [](const intents::None &) {}, + [&](const intents::angular::VelocityIntent & v) { + body_velocity.angular = v.omega; + use_controller_omega = false; + }, + [&](const intents::angular::HeadingIntent & h) { + controller.face_absolute(h.theta); + }, + [&](const intents::angular::FacingIntent & f) { + controller.face_point(f.target); + }, + [&](const intents::angular::FaceTravelIntent &) { + controller.face_travel(); + } + }, intent.angular); + + if(!path.empty()) { + // TODO(barulicm) change return type of get_command to BodyVelocity + const auto controller_vel = controller.get_command(robot, current_time, + intent.motion_options); + if (use_controller_linvel) { + body_velocity.linear = { + controller_vel.twist.linear.x, + controller_vel.twist.linear.y + }; + } + if (use_controller_omega) { + body_velocity.angular = controller_vel.twist.angular.z; + } + } + + if (intent.enable_escape_velocities && + (std::holds_alternative(intent.linear) || + std::holds_alternative(intent.linear))) + { + auto escape_velocity = GenerateEscapeVelocity(world, robot, intent); + if (escape_velocity) { + body_velocity = escape_velocity.value(); + } + } + + if(intent.callback.has_value()) { + body_velocity = intent.callback.value()(body_velocity, path, robot, world); + } + + DrawOverlays(overlays, world, robot, path, intent); + + results[i] = body_velocity; + } + + return results; +} + +std::optional MotionExecutor::GenerateEscapeVelocity( + const World & world, + const Robot & robot, + const MotionIntent & intent) +{ + std::vector obstacles = intent.obstacles; + if (intent.planner_options.use_default_obstacles) { + path_planning::AddDefaultObstacles(world, obstacles); + } + path_planning::AddRobotObstacles(world.our_robots, robot.id, obstacles); + path_planning::AddRobotObstacles(world.their_robots, obstacles); + + auto vel = path_planning::GenerateEscapeVelocity(robot, obstacles, + intent.planner_options.footprint_inflation); + if (vel) { + return BodyVelocity{ + ateam_geometry::Vector(vel->linear.x, vel->linear.y), + vel->angular.z + }; + } else { + return std::nullopt; + } +} + +void MotionExecutor::DrawOverlays( + visualization::Overlays & overlays, const World & world, + const Robot & robot, const path_planning::Path & path, + const MotionIntent & intent) +{ + (void)world; + const auto name_prefix = "motion/robot_" + std::to_string(robot.id) + "/"; + if(std::holds_alternative(intent.linear) || + std::holds_alternative(intent.linear)) + { + const auto target_point = std::visit(overloads{ + [](const intents::linear::PositionIntent & p) {return p.position;}, + [](const intents::linear::VelocityAtPositionIntent & v) {return v.position;}, + [](const auto &){return ateam_geometry::Point();} + }, intent.linear); + if(path.empty()) { + overlays.drawLine(name_prefix + "path", {robot.pos, target_point}, "Red"); + } else if (path.size() >= 2) { + const auto [closest_index, closest_point] = ProjectRobotOnPath(path, robot); + std::vector path_done(path.begin(), path.begin() + (closest_index -1)); + path_done.push_back(closest_point); + std::vector path_remaining(path.begin() + (closest_index - 1), path.end()); + path_remaining.insert(path_remaining.begin(), robot.pos); + overlays.drawLine(name_prefix + "path_done", path_done, "LightGrey"); + overlays.drawLine(name_prefix + "path_remaining", path_remaining, "Purple"); + const auto & planner = planners_[robot.id]; + if (planner.didTimeOut()) { + overlays.drawLine("afterpath", {path.back(), target_point}, "LightSkyBlue"); + } else if (planner.isPathTruncated()) { + overlays.drawLine("afterpath", {path.back(), target_point}, "LightPink"); + } + } + } +} + +std::pair MotionExecutor::ProjectRobotOnPath( + const path_planning::Path & path, const Robot & robot) +{ + if (path.empty()) + { + return {0, robot.pos}; + } + if (path.size() == 1) { + return {0, path[0]}; + } + auto closest_point = ateam_geometry::nearestPointOnSegment(ateam_geometry::Segment(path[0], path[1]), robot.pos); + size_t closest_index = 1; + double min_distance = CGAL::squared_distance(closest_point, robot.pos); + for (size_t i = 1; i < path.size() - 1; ++i) { + const auto segment = ateam_geometry::Segment(path[i], path[i + 1]); + const auto point_on_segment = ateam_geometry::nearestPointOnSegment(segment, robot.pos); + const auto distance = CGAL::squared_distance(point_on_segment, robot.pos); + if (distance <= min_distance) { + min_distance = distance; + closest_point = point_on_segment; + closest_index = i+1; + } + } + return {closest_index, closest_point}; +} + +} // namespace ateam_kenobi::motion diff --git a/ateam_kenobi/src/core/motion/motion_executor.hpp b/ateam_kenobi/src/core/motion/motion_executor.hpp new file mode 100644 index 000000000..e0d63c6eb --- /dev/null +++ b/ateam_kenobi/src/core/motion/motion_executor.hpp @@ -0,0 +1,66 @@ +// Copyright 2025 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 CORE__MOTION__MOTION_EXECUTOR_HPP_ +#define CORE__MOTION__MOTION_EXECUTOR_HPP_ + +#include +#include +#include +#include "core/path_planning/path_planner.hpp" +#include "core/visualization/overlays.hpp" +#include "motion_controller.hpp" +#include "motion_intent.hpp" + +namespace ateam_kenobi::motion +{ + +class MotionExecutor +{ +public: + MotionExecutor(rclcpp::Logger logger); + + std::array, + 16> RunFrame( + std::array, 16> intents, + visualization::Overlays & overlays, const World & world); + +private: + rclcpp::Logger logger_; + std::array planners_; + std::array controllers_; + + std::optional GenerateEscapeVelocity( + const World & world, const Robot & robot, + const MotionIntent & intent); + + void DrawOverlays( + visualization::Overlays & overlays, const World & world, + const Robot & robot, const path_planning::Path & path, + const MotionIntent & intent); + + std::pair ProjectRobotOnPath( + const path_planning::Path & path, const Robot & robot); + +}; + +} // namespace ateam_kenobi::motion + +#endif // CORE__MOTION__MOTION_EXECUTOR_HPP_ diff --git a/ateam_kenobi/src/core/motion/motion_intent.hpp b/ateam_kenobi/src/core/motion/motion_intent.hpp new file mode 100644 index 000000000..f064c2abe --- /dev/null +++ b/ateam_kenobi/src/core/motion/motion_intent.hpp @@ -0,0 +1,123 @@ +// Copyright 2025 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 CORE__MOTION__MOTION_TARGET_HPP_ +#define CORE__MOTION__MOTION_TARGET_HPP_ + +#include +#include +#include +#include +#include "core/types/world.hpp" +#include "core/types/robot.hpp" +#include "core/path_planning/path.hpp" +#include "core/path_planning/planner_options.hpp" +#include "motion_options.hpp" + +namespace ateam_kenobi::motion +{ + +namespace intents +{ + +struct None {}; + +namespace linear +{ + +enum class Frame +{ + World, + Local +}; + +struct VelocityIntent +{ + ateam_geometry::Vector velocity; + Frame frame = Frame::World; +}; + +struct PositionIntent +{ + ateam_geometry::Point position; +}; + +struct VelocityAtPositionIntent +{ + ateam_geometry::Point position; + ateam_geometry::Vector velocity; // Must be world frame +}; + +} // namespace linear + +namespace angular +{ + +struct VelocityIntent +{ + double omega; +}; + +struct HeadingIntent +{ + double theta; +}; + +struct FacingIntent +{ + ateam_geometry::Point target; +}; + +struct FaceTravelIntent {}; + +} // namespace angular + +} // namespace intents + +/// @brief Intended command velocity in local frame +struct BodyVelocity +{ + ateam_geometry::Vector linear; + double angular; +}; + +struct MotionIntent +{ + using LinearIntent = std::variant; + using AngularIntent = std::variant; + using PostCallback = std::function; + + LinearIntent linear = intents::None{}; + AngularIntent angular = intents::None{}; + std::optional callback; + path_planning::PlannerOptions planner_options; + MotionOptions motion_options; + std::vector obstacles; + bool enable_escape_velocities = true; +}; + +} // namespace ateam_kenobi::motion + +#endif // CORE__MOTION__MOTION_TARGET_HPP_ diff --git a/ateam_kenobi/src/core/motion/motion_options.hpp b/ateam_kenobi/src/core/motion/motion_options.hpp new file mode 100644 index 000000000..562adfb0a --- /dev/null +++ b/ateam_kenobi/src/core/motion/motion_options.hpp @@ -0,0 +1,38 @@ +// Copyright 2025 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 CORE__MOTION__MOTION_OPTIONS_HPP_ +#define CORE__MOTION__MOTION_OPTIONS_HPP_ + +namespace ateam_kenobi::motion +{ + +struct MotionOptions +{ + /// @brief radius around the end point that will be considered completed + double completion_threshold = .02; // meters + + /// @brief angle around the end point that will be considered completed + double angular_completion_threshold = 0.035; // radians +}; + +} // namespace ateam_kenobi::motion + +#endif // CORE__MOTION__MOTION_OPTIONS_HPP_ diff --git a/ateam_kenobi/src/core/path_planning/path.hpp b/ateam_kenobi/src/core/path_planning/path.hpp new file mode 100644 index 000000000..f51d876bc --- /dev/null +++ b/ateam_kenobi/src/core/path_planning/path.hpp @@ -0,0 +1,36 @@ +// Copyright 2025 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 CORE__PATH_PLANNING__PATH_HPP_ +#define CORE__PATH_PLANNING__PATH_HPP_ + +#include +#include + +namespace ateam_kenobi::path_planning +{ + +using Position = ateam_geometry::Point; +using Path = std::vector; + +} // namespace ateam_kenobi::path_planning + +#endif // CORE__PATH_PLANNING__PATH_HPP_ diff --git a/ateam_kenobi/src/core/path_planning/path_planner.cpp b/ateam_kenobi/src/core/path_planning/path_planner.cpp index 79e8238c4..e83cffba1 100644 --- a/ateam_kenobi/src/core/path_planning/path_planner.cpp +++ b/ateam_kenobi/src/core/path_planning/path_planner.cpp @@ -36,7 +36,7 @@ PathPlanner::PathPlanner(stp::Options stp_options) { } -PathPlanner::Path PathPlanner::getPath( +Path PathPlanner::getPath( const Position & start, const Position & goal, const World & world, const std::vector & obstacles, const PlannerOptions & options) diff --git a/ateam_kenobi/src/core/path_planning/path_planner.hpp b/ateam_kenobi/src/core/path_planning/path_planner.hpp index 63dba7f01..5fef6bd94 100644 --- a/ateam_kenobi/src/core/path_planning/path_planner.hpp +++ b/ateam_kenobi/src/core/path_planning/path_planner.hpp @@ -28,66 +28,15 @@ #include #include "core/types/world.hpp" #include "core/stp/base.hpp" +#include "path.hpp" +#include "planner_options.hpp" namespace ateam_kenobi::path_planning { -struct ReplanThresholds -{ - double goal_distance_ = 0.05; - double obstacle_distance_ = kRobotRadius; - double start_distance_ = 0.20; -}; - -struct PlannerOptions -{ - /** - * @brief Max time before planner will give up searching for a path - */ - double search_time_limit = 2e-3; // seconds - - /** - * @brief If true, the planner treats the ball as an obstacle. - */ - bool avoid_ball = true; - - /** - * @brief The size by which the radius of the robot will be augmented during collision checking - * - */ - double footprint_inflation = 0.06; - - double collision_check_resolution = 0.05; - - bool use_default_obstacles = true; - - /** - * @brief If true, any obstacles touching the start point will be ignored for all planning. - * - * Useful if you want to plan a path to escape a virtual obstacle like a keep out zone. - */ - bool ignore_start_obstacle = true; - - bool draw_obstacles = false; - - bool force_replan = false; - - /** - * Any corners sharper than this angle will attempt to be smoothed, time permitting - */ - double corner_smoothing_angle_threshold = 2.36; - - double corner_smoothing_step_size = 0.005; - - ReplanThresholds replan_thresholds; -}; - class PathPlanner : public stp::Base { public: - using Position = ateam_geometry::Point; - using Path = std::vector; - explicit PathPlanner(stp::Options stp_options = {}); Path getPath( diff --git a/ateam_kenobi/src/core/path_planning/planner_options.hpp b/ateam_kenobi/src/core/path_planning/planner_options.hpp new file mode 100644 index 000000000..142c8c0a1 --- /dev/null +++ b/ateam_kenobi/src/core/path_planning/planner_options.hpp @@ -0,0 +1,81 @@ +// Copyright 2025 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 CORE__PATH_PLANNING__PLANNER_OPTIONS_HPP_ +#define CORE__PATH_PLANNING__PLANNER_OPTIONS_HPP_ + +namespace ateam_kenobi::path_planning +{ + +struct ReplanThresholds +{ + double goal_distance_ = 0.05; + double obstacle_distance_ = kRobotRadius; + double start_distance_ = 0.20; +}; + +struct PlannerOptions +{ + /** + * @brief Max time before planner will give up searching for a path + */ + double search_time_limit = 2e-3; // seconds + + /** + * @brief If true, the planner treats the ball as an obstacle. + */ + bool avoid_ball = true; + + /** + * @brief The size by which the radius of the robot will be augmented during collision checking + * + */ + double footprint_inflation = 0.06; + + double collision_check_resolution = 0.05; + + bool use_default_obstacles = true; + + /** + * @brief If true, any obstacles touching the start point will be ignored for all planning. + * + * Useful if you want to plan a path to escape a virtual obstacle like a keep out zone. + */ + bool ignore_start_obstacle = true; + + bool draw_obstacles = false; + + bool force_replan = false; + + /** + * Any corners sharper than this angle will attempt to be smoothed, time permitting + */ + double corner_smoothing_angle_threshold = 2.36; + + double corner_smoothing_step_size = 0.005; + + ReplanThresholds replan_thresholds; +}; + +} // namespace ateam_kenobi::path_planning + + +#endif // CORE__PATH_PLANNING__PLANNER_OPTIONS_HPP_ diff --git a/ateam_kenobi/src/core/play_helpers/easy_move_to.cpp b/ateam_kenobi/src/core/play_helpers/easy_move_to.cpp index 3a93ef594..ce5d16ef1 100644 --- a/ateam_kenobi/src/core/play_helpers/easy_move_to.cpp +++ b/ateam_kenobi/src/core/play_helpers/easy_move_to.cpp @@ -99,7 +99,7 @@ void EasyMoveTo::setPlannerOptions(path_planning::PlannerOptions options) planner_options_ = options; } -void EasyMoveTo::setMotionOptions(MotionOptions options) +void EasyMoveTo::setMotionOptions(motion::MotionOptions options) { motion_options_ = options; } @@ -186,7 +186,7 @@ ateam_msgs::msg::RobotMotionCommand EasyMoveTo::runFrame( return motion_command; } -path_planning::PathPlanner::Path EasyMoveTo::planPath( +path_planning::Path EasyMoveTo::planPath( const Robot & robot, const World & world, const std::vector & obstacles) { @@ -194,7 +194,7 @@ path_planning::PathPlanner::Path EasyMoveTo::planPath( } ateam_msgs::msg::RobotMotionCommand EasyMoveTo::getMotionCommand( - const path_planning::PathPlanner::Path & path, const Robot & robot, const World & world) + const path_planning::Path & path, const Robot & robot, const World & world) { const auto current_time = std::chrono::duration_cast>( world.current_time.time_since_epoch()).count(); @@ -218,7 +218,7 @@ ateam_msgs::msg::RobotMotionCommand EasyMoveTo::getMotionCommand( } void EasyMoveTo::drawTrajectoryOverlay( - const path_planning::PathPlanner::Path & path, + const path_planning::Path & path, const Robot & robot) { auto & overlays = getOverlays(); diff --git a/ateam_kenobi/src/core/play_helpers/easy_move_to.hpp b/ateam_kenobi/src/core/play_helpers/easy_move_to.hpp index b2c7c68dd..2482ea998 100644 --- a/ateam_kenobi/src/core/play_helpers/easy_move_to.hpp +++ b/ateam_kenobi/src/core/play_helpers/easy_move_to.hpp @@ -64,7 +64,7 @@ class EasyMoveTo : public stp::Base const path_planning::PlannerOptions & getPlannerOptions() const; void setPlannerOptions(path_planning::PlannerOptions options); - void setMotionOptions(MotionOptions options); + void setMotionOptions(motion::MotionOptions options); void face_point(std::optional point); void face_absolute(double angle); @@ -99,17 +99,17 @@ class EasyMoveTo : public stp::Base ateam_geometry::Vector target_velocity_; path_planning::PlannerOptions planner_options_; path_planning::PathPlanner path_planner_; - MotionController motion_controller_; - MotionOptions motion_options_; + motion::MotionController motion_controller_; + motion::MotionOptions motion_options_; - path_planning::PathPlanner::Path planPath( + path_planning::Path planPath( const Robot & robot, const World & world, const std::vector & obstacles); ateam_msgs::msg::RobotMotionCommand getMotionCommand( - const path_planning::PathPlanner::Path & path, const Robot & robot, const World & world); + const path_planning::Path & path, const Robot & robot, const World & world); - void drawTrajectoryOverlay(const path_planning::PathPlanner::Path & path, const Robot & robot); + void drawTrajectoryOverlay(const path_planning::Path & path, const Robot & robot); std::optional generateEscapeVelocity( const World & world, diff --git a/ateam_kenobi/src/core/types/robot_command.hpp b/ateam_kenobi/src/core/types/robot_command.hpp new file mode 100644 index 000000000..63328f5d8 --- /dev/null +++ b/ateam_kenobi/src/core/types/robot_command.hpp @@ -0,0 +1,53 @@ +// Copyright 2025 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 CORE__TYPES_ROBOT_COMMAND_HPP_ +#define CORE__TYPES_ROBOT_COMMAND_HPP_ + +#include "motion/motion_target.hpp" + +namespace ateam_kenobi +{ + +enum class KickState : uint8_t +{ + Arm = 0, + Disable = 1, + KickNow = 2, + KickOnTouch = 3, + KickOnCaptured = 4, + ChipNow = 5, + ChipOnTouch = 6, + ChipOnCaptured = 7 +}; + +struct RobotCommand +{ + motion::MotionTarget motion_target; + + KickState kick; + double kick_speed; + + double dribbler_speed; +}; + +} // namespace ateam_kenobi + +#endif // CORE__TYPES_ROBOT_COMMAND_HPP_ diff --git a/ateam_kenobi/src/plays/halt_play.cpp b/ateam_kenobi/src/plays/halt_play.cpp index 94bcd6e63..81e5bf3a5 100644 --- a/ateam_kenobi/src/plays/halt_play.cpp +++ b/ateam_kenobi/src/plays/halt_play.cpp @@ -25,8 +25,7 @@ namespace ateam_kenobi::plays { HaltPlay::HaltPlay(stp::Options stp_options) -: stp::Play(kPlayName, stp_options), - path_planner_(createChild("path_planer")) +: stp::Play(kPlayName, stp_options) { } diff --git a/ateam_kenobi/src/plays/halt_play.hpp b/ateam_kenobi/src/plays/halt_play.hpp index dd86d563f..092270c75 100644 --- a/ateam_kenobi/src/plays/halt_play.hpp +++ b/ateam_kenobi/src/plays/halt_play.hpp @@ -21,8 +21,6 @@ #ifndef PLAYS__HALT_PLAY_HPP_ #define PLAYS__HALT_PLAY_HPP_ -#include "core/path_planning/path_planner.hpp" -#include "core/motion/motion_controller.hpp" #include "core/stp/play.hpp" namespace ateam_kenobi::plays @@ -42,8 +40,6 @@ class HaltPlay : public stp::Play 16> runFrame(const World & world) override; private: - path_planning::PathPlanner path_planner_; - MotionController motion_controller_; }; } // namespace ateam_kenobi::plays diff --git a/ateam_kenobi/src/plays/test_plays/controls_test_play.cpp b/ateam_kenobi/src/plays/test_plays/controls_test_play.cpp index 8349f95f7..c4cea0d1d 100644 --- a/ateam_kenobi/src/plays/test_plays/controls_test_play.cpp +++ b/ateam_kenobi/src/plays/test_plays/controls_test_play.cpp @@ -38,10 +38,10 @@ ControlsTestPlay::ControlsTestPlay(stp::Options stp_options) // Drive in square waypoints = { - {ateam_geometry::Point(-1.0, -1.0), AngleMode::face_absolute, 0.0, 3.0}, - {ateam_geometry::Point(-3.0, -1.0), AngleMode::face_absolute, 0.0, 3.0}, - {ateam_geometry::Point(-3.0, 1.0), AngleMode::face_absolute, 0.0, 3.0}, - {ateam_geometry::Point(-1.0, 1.0), AngleMode::face_absolute, 0.0, 3.0}, + {ateam_geometry::Point(1.0, -1.0), motion::AngleMode::face_absolute, 0.0, 3.0}, + {ateam_geometry::Point(-1.0, -1.0), motion::AngleMode::face_absolute, 0.0, 3.0}, + {ateam_geometry::Point(-1.0, 1.0), motion::AngleMode::face_absolute, 0.0, 3.0}, + {ateam_geometry::Point(1.0, 1.0), motion::AngleMode::face_absolute, 0.0, 3.0}, }; // waypoints = { @@ -98,23 +98,23 @@ std::array, 16> ControlsTestP motion_controller_.reset_trajectory(path, waypoint_vel); switch (waypoints[index].angle_mode) { - case AngleMode::face_absolute: + case motion::AngleMode::face_absolute: motion_controller_.face_absolute(waypoints[index].heading); break; - case AngleMode::face_travel: + case motion::AngleMode::face_travel: motion_controller_.face_travel(); break; - case AngleMode::face_point: + case motion::AngleMode::face_point: // WARNING: face_point not supported in this play. Defaulting to no face. [[fallthrough]]; - case AngleMode::no_face: + case motion::AngleMode::no_face: motion_controller_.no_face(); break; } const auto current_time = std::chrono::duration_cast>( world.current_time.time_since_epoch()).count(); - MotionOptions motion_options; + motion::MotionOptions motion_options; motion_options.completion_threshold = position_threshold; maybe_motion_commands[robot.id] = motion_controller_.get_command( robot, current_time, @@ -163,7 +163,7 @@ bool ControlsTestPlay::isGoalHit(const Robot & robot) const bool position_goal_hit = ateam_geometry::norm(waypoints[target_index].position - robot.pos) < position_threshold; const bool heading_goal_hit = [&]() { - if (waypoints[target_index].angle_mode == AngleMode::face_absolute) { + if (waypoints[target_index].angle_mode == motion::AngleMode::face_absolute) { return std::abs( angles::shortest_angular_distance( waypoints[target_index].heading, diff --git a/ateam_kenobi/src/plays/test_plays/controls_test_play.hpp b/ateam_kenobi/src/plays/test_plays/controls_test_play.hpp index c46c6b7b9..860d84aca 100644 --- a/ateam_kenobi/src/plays/test_plays/controls_test_play.hpp +++ b/ateam_kenobi/src/plays/test_plays/controls_test_play.hpp @@ -46,13 +46,13 @@ class ControlsTestPlay : public stp::Play struct Waypoint { ateam_geometry::Point position; - AngleMode angle_mode; + motion::AngleMode angle_mode; double heading; double hold_time_sec; }; - MotionController motion_controller_; - MotionOptions motion_options_; + motion::MotionController motion_controller_; + motion::MotionOptions motion_options_; int index = 0; std::vector waypoints; diff --git a/ateam_kenobi/src/skills/capture.cpp b/ateam_kenobi/src/skills/capture.cpp index d01394a29..09b81deb3 100644 --- a/ateam_kenobi/src/skills/capture.cpp +++ b/ateam_kenobi/src/skills/capture.cpp @@ -73,7 +73,7 @@ ateam_msgs::msg::RobotMotionCommand Capture::runMoveToBall( { easy_move_to_.face_point(world.ball.pos); - MotionOptions motion_options; + motion::MotionOptions motion_options; motion_options.completion_threshold = 0; easy_move_to_.setMotionOptions(motion_options); path_planning::PlannerOptions planner_options = easy_move_to_.getPlannerOptions(); @@ -120,7 +120,7 @@ ateam_msgs::msg::RobotMotionCommand Capture::runCapture(const World & world, con // planner_options.draw_obstacles = true; easy_move_to_.setPlannerOptions(planner_options); - MotionOptions motion_options; + motion::MotionOptions motion_options; motion_options.completion_threshold = 0; easy_move_to_.setMotionOptions(motion_options); diff --git a/ateam_kenobi/src/skills/dribble.cpp b/ateam_kenobi/src/skills/dribble.cpp index 38983cd8b..88f13ac1a 100644 --- a/ateam_kenobi/src/skills/dribble.cpp +++ b/ateam_kenobi/src/skills/dribble.cpp @@ -187,7 +187,7 @@ ateam_msgs::msg::RobotMotionCommand Dribble::runMoveBehindBall( { easy_move_to_.face_point(target_); - MotionOptions motion_options; + motion::MotionOptions motion_options; motion_options.completion_threshold = 0; easy_move_to_.setMotionOptions(motion_options); path_planning::PlannerOptions planner_options; diff --git a/ateam_kenobi/src/skills/intercept.cpp b/ateam_kenobi/src/skills/intercept.cpp index c99b131d8..615635603 100644 --- a/ateam_kenobi/src/skills/intercept.cpp +++ b/ateam_kenobi/src/skills/intercept.cpp @@ -120,7 +120,7 @@ ateam_msgs::msg::RobotMotionCommand Intercept::runMoveToBall( { easy_move_to_.face_point(world.ball.pos); - MotionOptions motion_options; + motion::MotionOptions motion_options; motion_options.completion_threshold = 0; easy_move_to_.setMotionOptions(motion_options); path_planning::PlannerOptions planner_options; diff --git a/ateam_kenobi/src/skills/line_kick.cpp b/ateam_kenobi/src/skills/line_kick.cpp index 66a70efaa..3f830da02 100644 --- a/ateam_kenobi/src/skills/line_kick.cpp +++ b/ateam_kenobi/src/skills/line_kick.cpp @@ -174,7 +174,7 @@ ateam_msgs::msg::RobotMotionCommand LineKick::RunMoveBehindBall( { easy_move_to_.face_point(target_point_); - MotionOptions motion_options; + motion::MotionOptions motion_options; motion_options.completion_threshold = 0; easy_move_to_.setMotionOptions(motion_options); path_planning::PlannerOptions planner_options = easy_move_to_.getPlannerOptions(); diff --git a/ateam_kenobi/src/tactics/multi_move_to.hpp b/ateam_kenobi/src/tactics/multi_move_to.hpp index 29ab56bca..29248d625 100644 --- a/ateam_kenobi/src/tactics/multi_move_to.hpp +++ b/ateam_kenobi/src/tactics/multi_move_to.hpp @@ -50,23 +50,23 @@ class MultiMoveTo : public stp::Tactic void SetFaceNone() { - angle_mode_ = AngleMode::no_face; + angle_mode_ = motion::AngleMode::no_face; } void SetFaceTravel() { - angle_mode_ = AngleMode::face_travel; + angle_mode_ = motion::AngleMode::face_travel; } void SetFaceAbsolue(double angle) { - angle_mode_ = AngleMode::face_absolute; + angle_mode_ = motion::AngleMode::face_absolute; absolute_angle_reference_ = angle; } void SetFacePoint(const ateam_geometry::Point & point) { - angle_mode_ = AngleMode::face_point; + angle_mode_ = motion::AngleMode::face_point; point_angle_reference_ = point; } @@ -81,7 +81,7 @@ class MultiMoveTo : public stp::Tactic } private: - AngleMode angle_mode_ = AngleMode::no_face; + motion::AngleMode angle_mode_ = motion::AngleMode::no_face; double absolute_angle_reference_ = 0.0; ateam_geometry::Point point_angle_reference_; diff --git a/ateam_kenobi/test/path_planner_test.cpp b/ateam_kenobi/test/path_planner_test.cpp index dbd620ca2..98df6f9ed 100644 --- a/ateam_kenobi/test/path_planner_test.cpp +++ b/ateam_kenobi/test/path_planner_test.cpp @@ -44,7 +44,7 @@ class GetPathTest : public ::testing::Test std::chrono::nanoseconds execution_time; ateam_geometry::Point start; ateam_geometry::Point goal; - PathPlanner::Path expected_path; + Path expected_path; void SetUp() override { @@ -80,7 +80,7 @@ class GetPathTest : public ::testing::Test expected_path.clear(); } - PathPlanner::Path getPath() + Path getPath() { const auto start_time = std::chrono::steady_clock::now(); const auto path = path_planner.getPath(start, goal, world, obstacles, planner_options); @@ -97,7 +97,7 @@ class GetPathTest : public ::testing::Test "PathPlanner took too long to find its path."; } - void printPath(const ateam_kenobi::path_planning::PathPlanner::Path & path) + void printPath(const ateam_kenobi::path_planning::Path & path) { std::cout << "Path:\n"; for (const auto & p : path) { diff --git a/ateam_kenobi/test/random_path_planner_test.cpp b/ateam_kenobi/test/random_path_planner_test.cpp index 3c5e191dd..74d184e9f 100644 --- a/ateam_kenobi/test/random_path_planner_test.cpp +++ b/ateam_kenobi/test/random_path_planner_test.cpp @@ -30,6 +30,7 @@ #include #include "core/path_planning/path_planner.hpp" +using ateam_kenobi::path_planning::Path; using ateam_kenobi::path_planning::PathPlanner; using ateam_kenobi::path_planning::PlannerOptions; @@ -44,7 +45,7 @@ bool isSwitchBack( return angle < 0.0873; // 5 degrees } -bool pathContainsSwitchback(const PathPlanner::Path & path) +bool pathContainsSwitchback(const Path & path) { if (path.empty()) { return false; @@ -61,7 +62,7 @@ bool pathContainsSwitchback(const PathPlanner::Path & path) return false; } -bool pathContainsLoops(const PathPlanner::Path & path) +bool pathContainsLoops(const Path & path) { if (path.size() < 4) { return false; @@ -79,7 +80,7 @@ bool pathContainsLoops(const PathPlanner::Path & path) return false; } -bool isBadPath(const PathPlanner::Path & path) +bool isBadPath(const Path & path) { /* Note: Empty or incomplete paths are not considered bad since there is no guarantee that our * random scenarios are solvable. @@ -101,7 +102,7 @@ void printUsage() void renderSecenario( const ateam_geometry::Point & start, const ateam_geometry::Point & goal, const ateam_geometry::Point & ball_pos, - const ateam_geometry::Point & opponent_pos, const PathPlanner::Path & path) + const ateam_geometry::Point & opponent_pos, const Path & path) { std::stringstream script; script << "import matplotlib.pyplot as plt\n"; From 349ba3a1505cad26406085d3d0855fde2b9fc0ab Mon Sep 17 00:00:00 2001 From: Matthew Barulic Date: Tue, 15 Jul 2025 16:14:18 -0300 Subject: [PATCH 02/35] Updates KenobiNode to use MotionExecutor --- ateam_kenobi/CMakeLists.txt | 58 +++++----- .../core/path_planning/planner_options.hpp | 1 + ateam_kenobi/src/core/play_selector.cpp | 100 +++++++++--------- ateam_kenobi/src/core/stp/play.hpp | 5 +- ateam_kenobi/src/core/types/robot_command.hpp | 10 +- ateam_kenobi/src/kenobi_node.cpp | 41 ++++++- ateam_kenobi/src/plays/all_plays.hpp | 32 +++--- ateam_kenobi/src/plays/halt_play.cpp | 10 +- ateam_kenobi/src/plays/halt_play.hpp | 2 +- 9 files changed, 148 insertions(+), 111 deletions(-) diff --git a/ateam_kenobi/CMakeLists.txt b/ateam_kenobi/CMakeLists.txt index b2e2c8b6b..356a15f11 100644 --- a/ateam_kenobi/CMakeLists.txt +++ b/ateam_kenobi/CMakeLists.txt @@ -51,36 +51,36 @@ target_sources(kenobi_node_component PRIVATE # Play Sources # target_sources(kenobi_node_component PRIVATE - src/plays/free_kick_plays/defense/their_free_kick_play.cpp - src/plays/free_kick_plays/offense/free_kick_on_goal_play.cpp - src/plays/kickoff_plays/defense/their_kickoff_play.cpp - src/plays/kickoff_plays/offense/kickoff_on_goal.cpp - src/plays/kickoff_plays/offense/kickoff_pass_play.cpp - src/plays/kickoff_plays/offense/our_kickoff_prep_play.cpp - src/plays/passing_plays/pass_to_lane_play.cpp - src/plays/passing_plays/pass_to_segment_play.cpp - src/plays/stop_plays/default_stop_play.cpp - src/plays/stop_plays/defensive_stop_play.cpp - src/plays/stop_plays/offensive_stop_play.cpp - src/plays/stop_plays/stop_helpers.cpp - src/plays/test_plays/controls_test_play.cpp - src/plays/test_plays/spinning_a_play.cpp - src/plays/test_plays/test_pass_play.cpp - src/plays/test_plays/test_play.cpp - src/plays/test_plays/triangle_pass_play.cpp - src/plays/test_plays/waypoints_play.cpp - src/plays/util_plays/corner_lineup_play.cpp + # src/plays/free_kick_plays/defense/their_free_kick_play.cpp + # src/plays/free_kick_plays/offense/free_kick_on_goal_play.cpp + # src/plays/kickoff_plays/defense/their_kickoff_play.cpp + # src/plays/kickoff_plays/offense/kickoff_on_goal.cpp + # src/plays/kickoff_plays/offense/kickoff_pass_play.cpp + # src/plays/kickoff_plays/offense/our_kickoff_prep_play.cpp + # src/plays/passing_plays/pass_to_lane_play.cpp + # src/plays/passing_plays/pass_to_segment_play.cpp + # src/plays/stop_plays/default_stop_play.cpp + # src/plays/stop_plays/defensive_stop_play.cpp + # src/plays/stop_plays/offensive_stop_play.cpp + # src/plays/stop_plays/stop_helpers.cpp + # src/plays/test_plays/controls_test_play.cpp + # src/plays/test_plays/spinning_a_play.cpp + # src/plays/test_plays/test_pass_play.cpp + # src/plays/test_plays/test_play.cpp + # src/plays/test_plays/triangle_pass_play.cpp + # src/plays/test_plays/waypoints_play.cpp + # src/plays/util_plays/corner_lineup_play.cpp src/plays/halt_play.cpp - src/plays/kick_on_goal_play.cpp - src/plays/our_ball_placement_play.cpp - src/plays/their_ball_placement_play.cpp - src/plays/wall_play.cpp - src/plays/basic_122.cpp - src/plays/defenders_only_play.cpp - src/plays/our_penalty_play.cpp - src/plays/their_penalty_play.cpp - src/plays/defense_play.cpp - src/plays/extract_play.cpp + # src/plays/kick_on_goal_play.cpp + # src/plays/our_ball_placement_play.cpp + # src/plays/their_ball_placement_play.cpp + # src/plays/wall_play.cpp + # src/plays/basic_122.cpp + # src/plays/defenders_only_play.cpp + # src/plays/our_penalty_play.cpp + # src/plays/their_penalty_play.cpp + # src/plays/defense_play.cpp + # src/plays/extract_play.cpp ) # diff --git a/ateam_kenobi/src/core/path_planning/planner_options.hpp b/ateam_kenobi/src/core/path_planning/planner_options.hpp index 142c8c0a1..34ce82938 100644 --- a/ateam_kenobi/src/core/path_planning/planner_options.hpp +++ b/ateam_kenobi/src/core/path_planning/planner_options.hpp @@ -22,6 +22,7 @@ #ifndef CORE__PATH_PLANNING__PLANNER_OPTIONS_HPP_ #define CORE__PATH_PLANNING__PLANNER_OPTIONS_HPP_ +#include namespace ateam_kenobi::path_planning { diff --git a/ateam_kenobi/src/core/play_selector.cpp b/ateam_kenobi/src/core/play_selector.cpp index 8826d4bdf..520da3949 100644 --- a/ateam_kenobi/src/core/play_selector.cpp +++ b/ateam_kenobi/src/core/play_selector.cpp @@ -43,56 +43,56 @@ PlaySelector::PlaySelector(rclcpp::Node & node) "stp_parameters", node.get_node_parameters_interface()); halt_play_ = addPlay(stp_options); - addPlay(stp_options); - addPlay("TheirLeftLineup", stp_options, 1.0, 1.0); - addPlay("TheirRightLineup", stp_options, 1.0, -1.0); - addPlay("OurLeftLineup", stp_options, -1.0, 1.0); - addPlay("OurRightLineup", stp_options, -1.0, -1.0); - addPlay(stp_options); - addPlay(stp_options); - addPlay(stp_options); - addPlay(stp_options); - addPlay(stp_options); - addPlay(stp_options); - addPlay(stp_options); - addPlay(stp_options); - addPlay(stp_options); - addPlay(stp_options); - addPlay(stp_options); - addPlay(stp_options); - addPlay(stp_options); - addPlay(stp_options); - addPlay(stp_options); - addPlay(stp_options); - addPlay(stp_options); - addPlay(stp_options); - addPlay(stp_options); - addPlay(stp_options); - addPlay(stp_options); - addPlay(stp_options); - addPlay(stp_options); - addPlay( - "PassLeftForwardPlay", stp_options, play_helpers::lanes::Lane::Left, - PassToLanePlay::PassDirection::Forward); - addPlay( - "PassCenterForwardPlay", stp_options, play_helpers::lanes::Lane::Center, - PassToLanePlay::PassDirection::Forward); - addPlay( - "PassRightForwardPlay", stp_options, play_helpers::lanes::Lane::Right, - PassToLanePlay::PassDirection::Forward); - addPlay( - "PassLeftBackwardPlay", stp_options, play_helpers::lanes::Lane::Left, - PassToLanePlay::PassDirection::Backward); - addPlay( - "PassCenterBackwardPlay", stp_options, play_helpers::lanes::Lane::Center, - PassToLanePlay::PassDirection::Backward); - addPlay( - "PassRightBackwardPlay", stp_options, play_helpers::lanes::Lane::Right, - PassToLanePlay::PassDirection::Backward); - addPlay(stp_options); - addPlay(stp_options); - addPlay(stp_options); - addPlay(stp_options); + // addPlay(stp_options); + // addPlay("TheirLeftLineup", stp_options, 1.0, 1.0); + // addPlay("TheirRightLineup", stp_options, 1.0, -1.0); + // addPlay("OurLeftLineup", stp_options, -1.0, 1.0); + // addPlay("OurRightLineup", stp_options, -1.0, -1.0); + // addPlay(stp_options); + // addPlay(stp_options); + // addPlay(stp_options); + // addPlay(stp_options); + // addPlay(stp_options); + // addPlay(stp_options); + // addPlay(stp_options); + // addPlay(stp_options); + // addPlay(stp_options); + // addPlay(stp_options); + // addPlay(stp_options); + // addPlay(stp_options); + // addPlay(stp_options); + // addPlay(stp_options); + // addPlay(stp_options); + // addPlay(stp_options); + // addPlay(stp_options); + // addPlay(stp_options); + // addPlay(stp_options); + // addPlay(stp_options); + // addPlay(stp_options); + // addPlay(stp_options); + // addPlay(stp_options); + // addPlay( + // "PassLeftForwardPlay", stp_options, play_helpers::lanes::Lane::Left, + // PassToLanePlay::PassDirection::Forward); + // addPlay( + // "PassCenterForwardPlay", stp_options, play_helpers::lanes::Lane::Center, + // PassToLanePlay::PassDirection::Forward); + // addPlay( + // "PassRightForwardPlay", stp_options, play_helpers::lanes::Lane::Right, + // PassToLanePlay::PassDirection::Forward); + // addPlay( + // "PassLeftBackwardPlay", stp_options, play_helpers::lanes::Lane::Left, + // PassToLanePlay::PassDirection::Backward); + // addPlay( + // "PassCenterBackwardPlay", stp_options, play_helpers::lanes::Lane::Center, + // PassToLanePlay::PassDirection::Backward); + // addPlay( + // "PassRightBackwardPlay", stp_options, play_helpers::lanes::Lane::Right, + // PassToLanePlay::PassDirection::Backward); + // addPlay(stp_options); + // addPlay(stp_options); + // addPlay(stp_options); + // addPlay(stp_options); } stp::Play * PlaySelector::getPlay(const World & world, ateam_msgs::msg::PlaybookState & state_msg) diff --git a/ateam_kenobi/src/core/stp/play.hpp b/ateam_kenobi/src/core/stp/play.hpp index 47bdde3db..f14ad1718 100644 --- a/ateam_kenobi/src/core/stp/play.hpp +++ b/ateam_kenobi/src/core/stp/play.hpp @@ -25,10 +25,10 @@ #include #include #include -#include #include "base.hpp" #include "play_score.hpp" #include "core/types/world.hpp" +#include "core/types/robot_command.hpp" namespace ateam_kenobi::stp { @@ -93,8 +93,7 @@ class Play : public Base */ virtual void exit() {} - virtual std::array, 16> runFrame( - const World & world) = 0; + virtual std::array, 16> runFrame(const World & world) = 0; bool isEnabled() const { diff --git a/ateam_kenobi/src/core/types/robot_command.hpp b/ateam_kenobi/src/core/types/robot_command.hpp index 63328f5d8..3a67ad3d3 100644 --- a/ateam_kenobi/src/core/types/robot_command.hpp +++ b/ateam_kenobi/src/core/types/robot_command.hpp @@ -21,7 +21,7 @@ #ifndef CORE__TYPES_ROBOT_COMMAND_HPP_ #define CORE__TYPES_ROBOT_COMMAND_HPP_ -#include "motion/motion_target.hpp" +#include "core/motion/motion_intent.hpp" namespace ateam_kenobi { @@ -40,12 +40,12 @@ enum class KickState : uint8_t struct RobotCommand { - motion::MotionTarget motion_target; + motion::MotionIntent motion_intent; - KickState kick; - double kick_speed; + KickState kick = KickState::Arm; + double kick_speed = 0.0; - double dribbler_speed; + double dribbler_speed = 0.0; }; } // namespace ateam_kenobi diff --git a/ateam_kenobi/src/kenobi_node.cpp b/ateam_kenobi/src/kenobi_node.cpp index 44035069a..ff66ae965 100644 --- a/ateam_kenobi/src/kenobi_node.cpp +++ b/ateam_kenobi/src/kenobi_node.cpp @@ -51,6 +51,7 @@ #include "core/ballsense_emulator.hpp" #include "core/ballsense_filter.hpp" #include "core/motion/world_to_body_vel.hpp" +#include "core/motion/motion_executor.hpp" #include "plays/halt_play.hpp" #include "core/defense_area_enforcement.hpp" #include "core/joystick_enforcer.hpp" @@ -71,6 +72,7 @@ class KenobiNode : public rclcpp::Node play_selector_(*this), joystick_enforcer_(*this), overlays_(""), + motion_executor_(get_logger().get_child("motion")), game_controller_listener_(*this) { initialize_robot_ids(); @@ -180,6 +182,8 @@ class KenobiNode : public rclcpp::Node JoystickEnforcer joystick_enforcer_; visualization::Overlays overlays_; FpsTracker fps_tracker_; + motion::MotionExecutor motion_executor_; + visualization::Overlays overlays_; rclcpp::Publisher::SharedPtr overlay_publisher_; rclcpp::Publisher::SharedPtr play_info_publisher_; rclcpp::Subscription::SharedPtr ball_subscription_; @@ -457,10 +461,41 @@ class KenobiNode : public rclcpp::Node RCLCPP_ERROR(get_logger(), "No play selected!"); return {}; } - const auto motion_commands = play->runFrame(world); - overlays_.merge(play->getOverlays()); + const auto commands = play->runFrame(world); + std::array, 16> motion_intents; + std::ranges::transform( + commands, motion_intents.begin(), + [](const std::optional & cmd) -> std::optional{ + if (cmd.has_value()) { + return cmd->motion_intent; + } else { + return std::nullopt; + } + }); + const auto motion_commands = motion_executor_.RunFrame(motion_intents, overlays_, world); + + std::array, 16> ros_commands; + for(auto id = 0ul; id < commands.size(); ++id) { + auto & maybe_cmd = commands[id]; + auto & maybe_motion_cmd = motion_commands[id]; + if (!maybe_cmd || !maybe_motion_cmd) { + ros_commands[id] = std::nullopt; + } else { + const auto & cmd = maybe_cmd.value(); + const auto & motion_cmd = maybe_motion_cmd.value(); + auto & ros_cmd = ros_commands[id].emplace(); + ros_cmd.dribbler_speed = cmd.dribbler_speed; + ros_cmd.kick_request = static_cast(cmd.kick); + ros_cmd.kick_speed = cmd.kick_speed; + ros_cmd.twist.linear.x = motion_cmd.linear.x(); + ros_cmd.twist.linear.y = motion_cmd.linear.y(); + ros_cmd.twist.angular.z = motion_cmd.angular; + ros_cmd.twist_frame = ateam_msgs::msg::RobotMotionCommand::FRAME_BODY; + } + } + overlays_.merge(play->getOverlays()); overlay_publisher_->publish(overlays_.getMsg()); overlays_.clear(); play->getOverlays().clear(); @@ -472,7 +507,7 @@ class KenobiNode : public rclcpp::Node play->getPlayInfo().clear(); play_info_publisher_->publish(play_info_msg); - return motion_commands; + return ros_commands; } void send_all_motion_commands( diff --git a/ateam_kenobi/src/plays/all_plays.hpp b/ateam_kenobi/src/plays/all_plays.hpp index 4b51dc4fc..b7075a362 100644 --- a/ateam_kenobi/src/plays/all_plays.hpp +++ b/ateam_kenobi/src/plays/all_plays.hpp @@ -23,21 +23,21 @@ #define PLAYS__ALL_PLAYS_HPP_ #include "halt_play.hpp" -#include "kick_on_goal_play.hpp" -#include "our_ball_placement_play.hpp" -#include "their_ball_placement_play.hpp" -#include "wall_play.hpp" -#include "basic_122.hpp" -#include "defenders_only_play.hpp" -#include "our_penalty_play.hpp" -#include "their_penalty_play.hpp" -#include "defense_play.hpp" -#include "extract_play.hpp" -#include "passing_plays/all_passing_plays.hpp" -#include "free_kick_plays/all_free_kick_plays.hpp" -#include "kickoff_plays/all_kickoff_plays.hpp" -#include "stop_plays/all_stop_plays.hpp" -#include "test_plays/all_test_plays.hpp" -#include "util_plays/all_util_plays.hpp" +// #include "kick_on_goal_play.hpp" +// #include "our_ball_placement_play.hpp" +// #include "their_ball_placement_play.hpp" +// #include "wall_play.hpp" +// #include "basic_122.hpp" +// #include "defenders_only_play.hpp" +// #include "our_penalty_play.hpp" +// #include "their_penalty_play.hpp" +// #include "defense_play.hpp" +// #include "extract_play.hpp" +// #include "passing_plays/all_passing_plays.hpp" +// #include "free_kick_plays/all_free_kick_plays.hpp" +// #include "kickoff_plays/all_kickoff_plays.hpp" +// #include "stop_plays/all_stop_plays.hpp" +// #include "test_plays/all_test_plays.hpp" +// #include "util_plays/all_util_plays.hpp" #endif // PLAYS__ALL_PLAYS_HPP_ diff --git a/ateam_kenobi/src/plays/halt_play.cpp b/ateam_kenobi/src/plays/halt_play.cpp index 81e5bf3a5..09ccc14d8 100644 --- a/ateam_kenobi/src/plays/halt_play.cpp +++ b/ateam_kenobi/src/plays/halt_play.cpp @@ -47,14 +47,16 @@ void HaltPlay::reset() { } -std::array, 16> HaltPlay::runFrame( +std::array, 16> HaltPlay::runFrame( const World &) { - std::array, 16> halt_motion_commands; + std::array, 16> halt_motion_commands; + RobotCommand command; + command.motion_intent.linear = motion::intents::linear::VelocityIntent{ateam_geometry::Vector{0.0, 0.0}}; + command.motion_intent.angular = motion::intents::angular::VelocityIntent{0.0}; for (size_t i = 0; i < 16; ++i) { - halt_motion_commands[i] = ateam_msgs::msg::RobotMotionCommand{}; + halt_motion_commands[i] = command; } - return halt_motion_commands; } } // namespace ateam_kenobi::plays diff --git a/ateam_kenobi/src/plays/halt_play.hpp b/ateam_kenobi/src/plays/halt_play.hpp index 092270c75..fac6a40d6 100644 --- a/ateam_kenobi/src/plays/halt_play.hpp +++ b/ateam_kenobi/src/plays/halt_play.hpp @@ -36,7 +36,7 @@ class HaltPlay : public stp::Play void reset() override; - std::array, + std::array, 16> runFrame(const World & world) override; private: From c0b675aac611e0af5dc3a34459ca4e9b80533b23 Mon Sep 17 00:00:00 2001 From: Matthew Barulic Date: Tue, 2 Sep 2025 21:13:52 -0400 Subject: [PATCH 03/35] Patches up breaks from merge --- ateam_kenobi/src/core/play_helpers/easy_move_to.cpp | 2 +- ateam_kenobi/src/core/play_helpers/easy_move_to.hpp | 2 +- ateam_kenobi/src/kenobi_node.cpp | 1 - 3 files changed, 2 insertions(+), 3 deletions(-) diff --git a/ateam_kenobi/src/core/play_helpers/easy_move_to.cpp b/ateam_kenobi/src/core/play_helpers/easy_move_to.cpp index ce5d16ef1..7d38d8521 100644 --- a/ateam_kenobi/src/core/play_helpers/easy_move_to.cpp +++ b/ateam_kenobi/src/core/play_helpers/easy_move_to.cpp @@ -268,7 +268,7 @@ std::optional EasyMoveTo::generateEscapeVel } std::pair EasyMoveTo::ProjectRobotOnPath( - const path_planning::PathPlanner::Path & path, const Robot & robot) + const path_planning::Path & path, const Robot & robot) { if (path.empty()) { return {0, robot.pos}; diff --git a/ateam_kenobi/src/core/play_helpers/easy_move_to.hpp b/ateam_kenobi/src/core/play_helpers/easy_move_to.hpp index 2482ea998..b274020f4 100644 --- a/ateam_kenobi/src/core/play_helpers/easy_move_to.hpp +++ b/ateam_kenobi/src/core/play_helpers/easy_move_to.hpp @@ -117,7 +117,7 @@ class EasyMoveTo : public stp::Base std::vector obstacles); std::pair ProjectRobotOnPath( - const path_planning::PathPlanner::Path & path, + const path_planning::Path & path, const Robot & robot); }; diff --git a/ateam_kenobi/src/kenobi_node.cpp b/ateam_kenobi/src/kenobi_node.cpp index ff66ae965..1470d1bfa 100644 --- a/ateam_kenobi/src/kenobi_node.cpp +++ b/ateam_kenobi/src/kenobi_node.cpp @@ -183,7 +183,6 @@ class KenobiNode : public rclcpp::Node visualization::Overlays overlays_; FpsTracker fps_tracker_; motion::MotionExecutor motion_executor_; - visualization::Overlays overlays_; rclcpp::Publisher::SharedPtr overlay_publisher_; rclcpp::Publisher::SharedPtr play_info_publisher_; rclcpp::Subscription::SharedPtr ball_subscription_; From ae51676073e430af44a6eda50e7af663c84806c7 Mon Sep 17 00:00:00 2001 From: Matthew Barulic Date: Tue, 2 Sep 2025 23:05:52 -0400 Subject: [PATCH 04/35] Updates WallPlay to use motion intents --- ateam_kenobi/CMakeLists.txt | 28 +++--- ateam_kenobi/src/plays/all_plays.hpp | 2 +- ateam_kenobi/src/plays/wall_play.cpp | 26 +++--- ateam_kenobi/src/plays/wall_play.hpp | 4 +- ateam_kenobi/src/skills/goalie.cpp | 60 +++++++------ ateam_kenobi/src/skills/goalie.hpp | 25 +++--- ateam_kenobi/src/skills/line_kick.cpp | 118 ++++++++++---------------- ateam_kenobi/src/skills/line_kick.hpp | 10 +-- 8 files changed, 117 insertions(+), 156 deletions(-) diff --git a/ateam_kenobi/CMakeLists.txt b/ateam_kenobi/CMakeLists.txt index 356a15f11..2f9968f83 100644 --- a/ateam_kenobi/CMakeLists.txt +++ b/ateam_kenobi/CMakeLists.txt @@ -74,7 +74,7 @@ target_sources(kenobi_node_component PRIVATE # src/plays/kick_on_goal_play.cpp # src/plays/our_ball_placement_play.cpp # src/plays/their_ball_placement_play.cpp - # src/plays/wall_play.cpp + src/plays/wall_play.cpp # src/plays/basic_122.cpp # src/plays/defenders_only_play.cpp # src/plays/our_penalty_play.cpp @@ -87,12 +87,12 @@ target_sources(kenobi_node_component PRIVATE # Tactic Sources # target_sources(kenobi_node_component PRIVATE - src/tactics/blockers.cpp - src/tactics/defenders.cpp - src/tactics/multi_move_to.cpp - src/tactics/pass_to_segment.cpp - src/tactics/pass.cpp - src/tactics/standard_defense.cpp + # src/tactics/blockers.cpp + # src/tactics/defenders.cpp + # src/tactics/multi_move_to.cpp + # src/tactics/pass_to_segment.cpp + # src/tactics/pass.cpp + # src/tactics/standard_defense.cpp ) # @@ -101,14 +101,14 @@ target_sources(kenobi_node_component PRIVATE target_sources(kenobi_node_component PRIVATE src/skills/goalie.cpp src/skills/kick_skill.cpp - src/skills/lane_idler.cpp + # src/skills/lane_idler.cpp src/skills/line_kick.cpp - src/skills/pass_receiver.cpp - src/skills/pivot_kick.cpp - src/skills/dribble.cpp - src/skills/extract.cpp - src/skills/capture.cpp - src/skills/universal_kick.cpp + # src/skills/pass_receiver.cpp + # src/skills/pivot_kick.cpp + # src/skills/dribble.cpp + # src/skills/extract.cpp + # src/skills/capture.cpp + # src/skills/universal_kick.cpp ) target_include_directories(kenobi_node_component PUBLIC diff --git a/ateam_kenobi/src/plays/all_plays.hpp b/ateam_kenobi/src/plays/all_plays.hpp index b7075a362..712e037b2 100644 --- a/ateam_kenobi/src/plays/all_plays.hpp +++ b/ateam_kenobi/src/plays/all_plays.hpp @@ -26,7 +26,7 @@ // #include "kick_on_goal_play.hpp" // #include "our_ball_placement_play.hpp" // #include "their_ball_placement_play.hpp" -// #include "wall_play.hpp" +#include "wall_play.hpp" // #include "basic_122.hpp" // #include "defenders_only_play.hpp" // #include "our_penalty_play.hpp" diff --git a/ateam_kenobi/src/plays/wall_play.cpp b/ateam_kenobi/src/plays/wall_play.cpp index 1bead908e..fd1ec5199 100644 --- a/ateam_kenobi/src/plays/wall_play.cpp +++ b/ateam_kenobi/src/plays/wall_play.cpp @@ -51,12 +51,10 @@ std::vector get_equally_spaced_points_on_segment( return points_on_segment; } - WallPlay::WallPlay(stp::Options stp_options) : stp::Play(kPlayName, stp_options), goalie_skill_(createChild("goalie")) { - createIndexedChildren(easy_move_tos_, "EasyMoveTo"); } stp::PlayScore WallPlay::getScore(const World &) @@ -66,18 +64,15 @@ stp::PlayScore WallPlay::getScore(const World &) void WallPlay::reset() { - for (auto & move_to : easy_move_tos_) { - move_to.reset(); - } goalie_skill_.reset(); } -std::array, 16> WallPlay::runFrame( +std::array, 16> WallPlay::runFrame( const World & world) { - std::array, 16> maybe_motion_commands; + std::array, 16> commands; - goalie_skill_.runFrame(world, maybe_motion_commands); + goalie_skill_.runFrame(world, commands); auto current_available_robots = play_helpers::getAvailableRobots(world); play_helpers::removeGoalie(current_available_robots, world); @@ -115,22 +110,21 @@ std::array, 16> WallPlay::run continue; } - auto & easy_move_to = easy_move_tos_.at(robot.id); - const auto & target_position = positions_to_assign.at(ind); + RobotCommand command; + command.motion_intent.linear = motion::intents::linear::PositionIntent{target_position}; + command.motion_intent.angular = motion::intents::angular::FacingIntent{world.ball.pos}; + auto viz_circle = ateam_geometry::makeCircle(target_position, kRobotRadius); getOverlays().drawCircle( "destination_" + std::to_string( robot.id), viz_circle, "blue", "transparent"); - - easy_move_to.setTargetPosition(target_position); - easy_move_to.face_point(world.ball.pos); - - maybe_motion_commands.at(robot.id) = easy_move_to.runFrame(robot, world); + + commands.at(robot.id) = command; } - return maybe_motion_commands; + return commands; } } // namespace ateam_kenobi::plays diff --git a/ateam_kenobi/src/plays/wall_play.hpp b/ateam_kenobi/src/plays/wall_play.hpp index 9eba641d7..88f7e464b 100644 --- a/ateam_kenobi/src/plays/wall_play.hpp +++ b/ateam_kenobi/src/plays/wall_play.hpp @@ -45,11 +45,9 @@ class WallPlay : public stp::Play void reset() override; - std::array, - 16> runFrame(const World & world) override; + std::array, 16> runFrame(const World & world) override; private: - std::array easy_move_tos_; skills::Goalie goalie_skill_; }; } // namespace ateam_kenobi::plays diff --git a/ateam_kenobi/src/skills/goalie.cpp b/ateam_kenobi/src/skills/goalie.cpp index 7a02662be..23faf5874 100644 --- a/ateam_kenobi/src/skills/goalie.cpp +++ b/ateam_kenobi/src/skills/goalie.cpp @@ -36,7 +36,6 @@ namespace ateam_kenobi::skills Goalie::Goalie(stp::Options stp_options) : stp::Skill(stp_options), - easy_move_to_(createChild("EasyMoveTo")), kick_(createChild("Kick")) { reset(); @@ -48,16 +47,13 @@ Goalie::Goalie(stp::Options stp_options) void Goalie::reset() { - easy_move_to_.reset(); prev_ball_in_def_area_ = false; } void Goalie::runFrame( const World & world, - std::array, - 16> & motion_commands) + std::array, 16> & motion_commands) { - easy_move_to_.setPlannerOptions(default_planner_options_); Ball ball_state = world.ball; if(world.ball.visible) { const auto maybe_closest_enemy = getClosestEnemyRobotToBall(world); @@ -80,7 +76,8 @@ void Goalie::runFrame( return; } - ateam_msgs::msg::RobotMotionCommand motion_command; + RobotCommand motion_command; + motion_command.motion_intent.planner_options = default_planner_options_; const auto ball_in_def_area = isBallInDefenseArea(world, ball_state); if(ball_in_def_area && !prev_ball_in_def_area_) { @@ -154,9 +151,9 @@ bool Goalie::isBallInDefenseArea(const World & world, const Ball & ball_state) return CGAL::do_intersect(ball_state.pos, our_defense_area); } -ateam_msgs::msg::RobotMotionCommand Goalie::runDefaultBehavior( +RobotCommand Goalie::runDefaultBehavior( const World & world, - const Robot & goalie, const Ball & ball_state) + const Robot &, const Ball & ball_state) { auto goal_line_offset = 0.25; if (world.referee_info.running_command == ateam_common::GameCommand::PreparePenaltyTheirs || @@ -173,15 +170,15 @@ ateam_msgs::msg::RobotMotionCommand Goalie::runDefaultBehavior( -(world.field.field_length / 2) + goal_line_offset, -world.field.goal_width / 2)); - easy_move_to_.setTargetPosition( - ateam_geometry::nearestPointOnSegment( - goalie_line, - ball_state.pos)); - easy_move_to_.face_absolute(M_PI_2); - return easy_move_to_.runFrame(goalie, world, getCustomObstacles(world)); + RobotCommand command; + command.motion_intent.planner_options = default_planner_options_; + command.motion_intent.linear = motion::intents::linear::PositionIntent{ateam_geometry::nearestPointOnSegment(goalie_line, ball_state.pos)}; + command.motion_intent.angular = motion::intents::angular::HeadingIntent{M_PI_2}; + command.motion_intent.obstacles = getCustomObstacles(world); + return command; } -ateam_msgs::msg::RobotMotionCommand Goalie::runBlockShot( +RobotCommand Goalie::runBlockShot( const World & world, const Robot & goalie, const Ball & ball_state) { @@ -236,16 +233,18 @@ ateam_msgs::msg::RobotMotionCommand Goalie::runBlockShot( shot_point_on_extended_goalie_line = segment->source(); } - easy_move_to_.setTargetPosition( + RobotCommand command; + command.motion_intent.planner_options = default_planner_options_; + command.motion_intent.linear = motion::intents::linear::PositionIntent{ ateam_geometry::nearestPointOnSegment( goalie_line, - shot_point_on_extended_goalie_line)); - easy_move_to_.face_absolute(M_PI_2); - - return easy_move_to_.runFrame(goalie, world, getCustomObstacles(world)); + shot_point_on_extended_goalie_line)}; + command.motion_intent.angular = motion::intents::angular::HeadingIntent{M_PI_2}; + command.motion_intent.obstacles = getCustomObstacles(world); + return command; } -ateam_msgs::msg::RobotMotionCommand Goalie::runBlockBall( +RobotCommand Goalie::runBlockBall( const World & world, const Robot & goalie, const Ball & ball_state) { @@ -271,17 +270,16 @@ ateam_msgs::msg::RobotMotionCommand Goalie::runBlockBall( target_point = segment->source(); } - auto planner_options = default_planner_options_; - planner_options.footprint_inflation = -0.05; - easy_move_to_.setPlannerOptions(planner_options); - - easy_move_to_.setTargetPosition(target_point); - easy_move_to_.face_absolute(M_PI_2); - - return easy_move_to_.runFrame(goalie, world, getCustomObstacles(world)); + RobotCommand command; + command.motion_intent.planner_options = default_planner_options_; + command.motion_intent.planner_options.footprint_inflation = -0.05; + command.motion_intent.linear = motion::intents::linear::PositionIntent{target_point}; + command.motion_intent.angular = motion::intents::angular::HeadingIntent{M_PI_2}; + command.motion_intent.obstacles = getCustomObstacles(world); + return command; } -ateam_msgs::msg::RobotMotionCommand Goalie::runClearBall( +RobotCommand Goalie::runClearBall( const World & world, const Robot & goalie, const Ball & ball_state) { @@ -324,7 +322,7 @@ ateam_msgs::msg::RobotMotionCommand Goalie::runClearBall( } -ateam_msgs::msg::RobotMotionCommand Goalie::runSideEjectBall( +RobotCommand Goalie::runSideEjectBall( const World & world, const Robot & goalie) { auto left_count = 0; diff --git a/ateam_kenobi/src/skills/goalie.hpp b/ateam_kenobi/src/skills/goalie.hpp index 34e77f988..f7afefd13 100644 --- a/ateam_kenobi/src/skills/goalie.hpp +++ b/ateam_kenobi/src/skills/goalie.hpp @@ -23,8 +23,8 @@ #include #include -#include "core/play_helpers/easy_move_to.hpp" #include "core/types/world.hpp" +#include "core/types/robot_command.hpp" #include "core/stp/skill.hpp" #include "pivot_kick.hpp" #include "line_kick.hpp" @@ -38,12 +38,9 @@ class Goalie : public stp::Skill void reset(); - void runFrame( - const World & world, std::array, - 16> & motion_commands); + void runFrame(const World & world, std::array, 16> & motion_commands); private: - play_helpers::EasyMoveTo easy_move_to_; LineKick kick_; std::optional last_enemy_id_closest_to_ball_; std::chrono::steady_clock::time_point ball_entered_def_area_time_; @@ -56,37 +53,37 @@ class Goalie : public stp::Skill /** * @brief Default behavior of robot staying in line with ball - * @return ateam_msgs::msg::RobotMotionCommand + * @return RobotCommand */ - ateam_msgs::msg::RobotMotionCommand runDefaultBehavior( + RobotCommand runDefaultBehavior( const World & world, const Robot & goalie, const Ball & ball_state); /** * @brief Block a possible shot when opponents have the ball - * @return ateam_msgs::msg::RobotMotionCommand + * @return RobotCommand */ - ateam_msgs::msg::RobotMotionCommand runBlockShot( + RobotCommand runBlockShot( const World & world, const Robot & goalie, const Ball & ball_state); /** * @brief Block ball when headed towards goal - * @return ateam_msgs::msg::RobotMotionCommand + * @return RobotCommand */ - ateam_msgs::msg::RobotMotionCommand runBlockBall( + RobotCommand runBlockBall( const World & world, const Robot & goalie, const Ball & ball_state); /** * @brief Kick ball out of defense area - * @return ateam_msgs::msg::RobotMotionCommand + * @return RobotCommand */ - ateam_msgs::msg::RobotMotionCommand runClearBall( + RobotCommand runClearBall( const World & world, const Robot & goalie, const Ball & ball_state); - ateam_msgs::msg::RobotMotionCommand runSideEjectBall(const World & world, const Robot & goalie); + RobotCommand runSideEjectBall(const World & world, const Robot & goalie); std::vector getCustomObstacles(const World & world); std::optional getClosestEnemyRobotToBall(const World & world); diff --git a/ateam_kenobi/src/skills/line_kick.cpp b/ateam_kenobi/src/skills/line_kick.cpp index 3f830da02..722515c2e 100644 --- a/ateam_kenobi/src/skills/line_kick.cpp +++ b/ateam_kenobi/src/skills/line_kick.cpp @@ -39,7 +39,7 @@ ateam_geometry::Point LineKick::GetAssignmentPoint(const World & world) return GetPreKickPosition(world); } -ateam_msgs::msg::RobotMotionCommand LineKick::RunFrame(const World & world, const Robot & robot) +RobotCommand LineKick::RunFrame(const World & world, const Robot & robot) { const auto pre_kick_position = GetPreKickPosition(world); @@ -59,10 +59,10 @@ ateam_msgs::msg::RobotMotionCommand LineKick::RunFrame(const World & world, cons return RunKickBall(world, robot); case State::Done: getPlayInfo()["state"] = "Done"; - return ateam_msgs::msg::RobotMotionCommand{}; + return RobotCommand{}; default: RCLCPP_ERROR(getLogger(), "Unhandled state in line kick!"); - return ateam_msgs::msg::RobotMotionCommand{}; + return RobotCommand{}; } } @@ -168,28 +168,37 @@ bool LineKick::IsBallMoving(const World & world) return ateam_geometry::norm(world.ball.vel) > 0.1 * GetKickSpeed(); } -ateam_msgs::msg::RobotMotionCommand LineKick::RunMoveBehindBall( +RobotCommand LineKick::RunMoveBehindBall( const World & world, const Robot & robot) { - easy_move_to_.face_point(target_point_); + RobotCommand command; - motion::MotionOptions motion_options; - motion_options.completion_threshold = 0; - easy_move_to_.setMotionOptions(motion_options); - path_planning::PlannerOptions planner_options = easy_move_to_.getPlannerOptions(); - planner_options.footprint_inflation = std::min(0.015, pre_kick_offset); + const auto prekick_position = GetPreKickPosition(world); - planner_options.draw_obstacles = true; - const auto robot_to_prekick = GetPreKickPosition(world) - robot.pos; - double obstacle_radius_multiplier = 1.8; + command.motion_intent.angular = motion::intents::angular::FacingIntent{target_point_}; + auto vel = ateam_geometry::Vector(0.002, 0); + if (ateam_geometry::norm(world.ball.vel) > 0) { + vel = world.ball.vel; + } + command.motion_intent.linear = + motion::intents::linear::VelocityAtPositionIntent{prekick_position + (0.1 * world.ball.vel), + vel}; - const auto ball_to_target = target_point_ - world.ball.pos; + command.motion_intent.motion_options.completion_threshold = 0.0; + command.motion_intent.planner_options.draw_obstacles = true; + command.motion_intent.planner_options.footprint_inflation = std::min(0.015, pre_kick_offset); - // Cowabunga it is + // TODO(barulicm): add accel limit support to motion intent + // easy_move_to_.setMaxAccel(1.5); + // easy_move_to_.setMaxDecel(1.5); + + double obstacle_radius_multiplier = 1.8; + const auto robot_to_prekick = prekick_position - robot.pos; + const auto ball_to_target = target_point_ - world.ball.pos; if (this->cowabunga && ateam_geometry::norm(robot_to_prekick) < 2.5 * kRobotDiameter) { - planner_options.footprint_inflation = -0.1; + command.motion_intent.planner_options.footprint_inflation = -0.1; obstacle_radius_multiplier = 5.0; getPlayInfo()["COWABUNGA MODE"] = "COWABUNGA"; } else { @@ -198,86 +207,51 @@ ateam_msgs::msg::RobotMotionCommand LineKick::RunMoveBehindBall( // Add additional obstacles to better avoid ball const auto angle = std::atan2(ball_to_target.y(), ball_to_target.x()); - std::vector obstacles; - - // Front Obstacle - obstacles.push_back( + command.motion_intent.obstacles = { + // Front Obstacle ateam_geometry::makeDisk( world.ball.pos + kBallDiameter * ateam_geometry::Vector( std::cos(angle), std::sin(angle)), - kBallRadius * obstacle_radius_multiplier - )); - - // Left Obstacle - obstacles.push_back( + kBallRadius * obstacle_radius_multiplier), + // Left Obstacle ateam_geometry::makeDisk( world.ball.pos + kBallDiameter * ateam_geometry::Vector( std::cos(angle + M_PI / 2), std::sin(angle + M_PI / 2)), - kBallRadius * obstacle_radius_multiplier - )); - - // Right Obstacle - obstacles.push_back( + kBallRadius * obstacle_radius_multiplier), + // Right Obstacle ateam_geometry::makeDisk( world.ball.pos + kBallDiameter * ateam_geometry::Vector( std::cos(angle - M_PI / 2), std::sin(angle - M_PI / 2)), - kBallRadius * obstacle_radius_multiplier - )); - - easy_move_to_.setMaxVelocity(move_to_ball_velocity); - easy_move_to_.setPlannerOptions(planner_options); - easy_move_to_.setMaxAccel(1.5); - easy_move_to_.setMaxDecel(1.5); - auto vel = ateam_geometry::Vector(0.002, 0); - if (ateam_geometry::norm(world.ball.vel) > 0) { - vel = world.ball.vel; - } - easy_move_to_.setTargetPosition(GetPreKickPosition(world) + (0.1 * world.ball.vel), vel); + kBallRadius * obstacle_radius_multiplier) + }; - auto command = easy_move_to_.runFrame(robot, world, obstacles); return command; } -ateam_msgs::msg::RobotMotionCommand LineKick::RunFaceBall(const World & world, const Robot & robot) +RobotCommand LineKick::RunFaceBall(const World &, const Robot &) { - easy_move_to_.setTargetPosition(robot.pos); - easy_move_to_.face_point(target_point_); - - auto command = easy_move_to_.runFrame(robot, world); - command.twist.linear.x = 0; - command.twist.linear.y = 0; + RobotCommand command; + command.motion_intent.angular = motion::intents::angular::FacingIntent{target_point_}; return command; } -ateam_msgs::msg::RobotMotionCommand LineKick::RunKickBall(const World & world, const Robot & robot) +RobotCommand LineKick::RunKickBall(const World & world, const Robot &) { - path_planning::PlannerOptions planner_options = easy_move_to_.getPlannerOptions(); - planner_options.avoid_ball = false; - planner_options.footprint_inflation = 0.0; - easy_move_to_.setPlannerOptions(planner_options); + RobotCommand command; + + command.motion_intent.linear = + motion::intents::linear::VelocityIntent{ateam_geometry::Vector{kick_drive_velocity, 0.0}, + motion::intents::linear::Frame::Local}; + + command.motion_intent.angular = motion::intents::angular::FacingIntent{world.ball.pos}; - // Handle robot angle - easy_move_to_.face_point(target_point_); - const auto ball_to_target = target_point_ - world.ball.pos; - easy_move_to_.face_point(world.ball.pos); - easy_move_to_.setTargetPosition(world.ball.pos + 0.1 * ateam_geometry::normalize( - ball_to_target), kick_drive_velocity * ateam_geometry::normalize(ball_to_target)); - auto command = easy_move_to_.runFrame(robot, world); - - // Override the velocity to move directly into the ball - command.twist.linear.x = std::cos(robot.theta) * kick_drive_velocity; - command.twist.linear.y = std::sin(robot.theta) * kick_drive_velocity; - command.twist_frame = ateam_msgs::msg::RobotMotionCommand::FRAME_WORLD; - // command.twist.linear.x = kick_drive_velocity; - // command.twist.linear.y = 0; - // command.twist_frame = ateam_msgs::msg::RobotMotionCommand::FRAME_BODY; if(KickOrChip() == KickSkill::KickChip::Kick) { - command.kick_request = ateam_msgs::msg::RobotMotionCommand::KR_KICK_TOUCH; + command.kick = KickState::KickOnTouch; } else { - command.kick_request = ateam_msgs::msg::RobotMotionCommand::KR_CHIP_TOUCH; + command.kick = KickState::ChipOnTouch; } command.kick_speed = GetKickSpeed(); diff --git a/ateam_kenobi/src/skills/line_kick.hpp b/ateam_kenobi/src/skills/line_kick.hpp index 1b031c6dd..f7e7e3664 100644 --- a/ateam_kenobi/src/skills/line_kick.hpp +++ b/ateam_kenobi/src/skills/line_kick.hpp @@ -22,10 +22,10 @@ #ifndef SKILLS__LINE_KICK_HPP_ #define SKILLS__LINE_KICK_HPP_ -#include #include #include "kick_skill.hpp" #include "core/types/world.hpp" +#include "core/types/robot_command.hpp" #include "core/play_helpers/easy_move_to.hpp" namespace ateam_kenobi::skills @@ -59,7 +59,7 @@ class LineKick : public KickSkill ateam_geometry::Point GetAssignmentPoint(const World & world); - ateam_msgs::msg::RobotMotionCommand RunFrame(const World & world, const Robot & robot); + RobotCommand RunFrame(const World & world, const Robot & robot); bool IsDone() const { @@ -176,9 +176,9 @@ class LineKick : public KickSkill bool IsRobotFacingBall(const Robot & robot); bool IsBallMoving(const World & world); - ateam_msgs::msg::RobotMotionCommand RunMoveBehindBall(const World & world, const Robot & robot); - ateam_msgs::msg::RobotMotionCommand RunFaceBall(const World & world, const Robot & robot); - ateam_msgs::msg::RobotMotionCommand RunKickBall(const World & world, const Robot & robot); + RobotCommand RunMoveBehindBall(const World & world, const Robot & robot); + RobotCommand RunFaceBall(const World & world, const Robot & robot); + RobotCommand RunKickBall(const World & world, const Robot & robot); }; } // namespace ateam_kenobi::skills From 107f418f1eaba8af399a6d11e3a5d0f22861a245 Mon Sep 17 00:00:00 2001 From: Matthew Barulic Date: Tue, 2 Sep 2025 23:31:19 -0400 Subject: [PATCH 05/35] Changes to get WallPlay working in sim --- ateam_kenobi/src/core/motion/motion_executor.cpp | 4 +++- ateam_kenobi/src/core/play_selector.cpp | 2 +- ateam_kenobi/src/skills/goalie.cpp | 2 +- 3 files changed, 5 insertions(+), 3 deletions(-) diff --git a/ateam_kenobi/src/core/motion/motion_executor.cpp b/ateam_kenobi/src/core/motion/motion_executor.cpp index 10be53fdb..6e22b3598 100644 --- a/ateam_kenobi/src/core/motion/motion_executor.cpp +++ b/ateam_kenobi/src/core/motion/motion_executor.cpp @@ -22,6 +22,7 @@ #include #include "core/path_planning/obstacles.hpp" #include "core/path_planning/escape_velocity.hpp" +#include "world_to_body_vel.hpp" namespace ateam_kenobi::motion { @@ -116,9 +117,10 @@ std::array, if(!path.empty()) { // TODO(barulicm) change return type of get_command to BodyVelocity - const auto controller_vel = controller.get_command(robot, current_time, + auto controller_vel = controller.get_command(robot, current_time, intent.motion_options); if (use_controller_linvel) { + ConvertWorldVelsToBodyVels(controller_vel, robot); body_velocity.linear = { controller_vel.twist.linear.x, controller_vel.twist.linear.y diff --git a/ateam_kenobi/src/core/play_selector.cpp b/ateam_kenobi/src/core/play_selector.cpp index 520da3949..bd988d5da 100644 --- a/ateam_kenobi/src/core/play_selector.cpp +++ b/ateam_kenobi/src/core/play_selector.cpp @@ -51,7 +51,7 @@ PlaySelector::PlaySelector(rclcpp::Node & node) // addPlay(stp_options); // addPlay(stp_options); // addPlay(stp_options); - // addPlay(stp_options); + addPlay(stp_options); // addPlay(stp_options); // addPlay(stp_options); // addPlay(stp_options); diff --git a/ateam_kenobi/src/skills/goalie.cpp b/ateam_kenobi/src/skills/goalie.cpp index 23faf5874..7fd18e9fd 100644 --- a/ateam_kenobi/src/skills/goalie.cpp +++ b/ateam_kenobi/src/skills/goalie.cpp @@ -262,7 +262,7 @@ RobotCommand Goalie::runBlockBall( return runDefaultBehavior(world, goalie, ball_state); } - ateam_geometry::Point target_point; + ateam_geometry::Point target_point{0.0, 0.0}; if (const auto * point = boost::get(&*maybe_intersection)) { target_point = *point; From f63e87547042b5903b7a7262fd36bfcc9ceac3a2 Mon Sep 17 00:00:00 2001 From: Matthew Barulic Date: Tue, 2 Sep 2025 23:36:18 -0400 Subject: [PATCH 06/35] Updates motion executor overlay implementation --- .../src/core/motion/motion_executor.cpp | 18 +++++++++++------- 1 file changed, 11 insertions(+), 7 deletions(-) diff --git a/ateam_kenobi/src/core/motion/motion_executor.cpp b/ateam_kenobi/src/core/motion/motion_executor.cpp index 6e22b3598..f27f80e99 100644 --- a/ateam_kenobi/src/core/motion/motion_executor.cpp +++ b/ateam_kenobi/src/core/motion/motion_executor.cpp @@ -193,15 +193,19 @@ void MotionExecutor::DrawOverlays( [](const auto &){return ateam_geometry::Point();} }, intent.linear); if(path.empty()) { - overlays.drawLine(name_prefix + "path", {robot.pos, target_point}, "Red"); - } else if (path.size() >= 2) { + overlays.drawLine("path", {robot.pos, target_point}, "Red"); + } else if(path.size() == 1) { + overlays.drawLine("path", {robot.pos, target_point}, "Purple"); + } else { const auto [closest_index, closest_point] = ProjectRobotOnPath(path, robot); - std::vector path_done(path.begin(), path.begin() + (closest_index -1)); + std::vector path_done(path.begin(), path.begin() + (closest_index)); path_done.push_back(closest_point); - std::vector path_remaining(path.begin() + (closest_index - 1), path.end()); - path_remaining.insert(path_remaining.begin(), robot.pos); - overlays.drawLine(name_prefix + "path_done", path_done, "LightGrey"); - overlays.drawLine(name_prefix + "path_remaining", path_remaining, "Purple"); + std::vector path_remaining(path.begin() + (closest_index), + path.end()); + path_remaining.insert(path_remaining.begin(), closest_point); + const auto translucent_purple = "#8000805F"; + overlays.drawLine("path_done", path_done, translucent_purple); + overlays.drawLine("path_remaining", path_remaining, "Purple"); const auto & planner = planners_[robot.id]; if (planner.didTimeOut()) { overlays.drawLine("afterpath", {path.back(), target_point}, "LightSkyBlue"); From 7119336366de424d4677cf37c29603a66ee850d7 Mon Sep 17 00:00:00 2001 From: Matthew Barulic Date: Tue, 2 Sep 2025 23:41:40 -0400 Subject: [PATCH 07/35] Re-adds accidentally removed overlay name prefixes --- ateam_kenobi/src/core/motion/motion_executor.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/ateam_kenobi/src/core/motion/motion_executor.cpp b/ateam_kenobi/src/core/motion/motion_executor.cpp index f27f80e99..154c1aa35 100644 --- a/ateam_kenobi/src/core/motion/motion_executor.cpp +++ b/ateam_kenobi/src/core/motion/motion_executor.cpp @@ -193,9 +193,9 @@ void MotionExecutor::DrawOverlays( [](const auto &){return ateam_geometry::Point();} }, intent.linear); if(path.empty()) { - overlays.drawLine("path", {robot.pos, target_point}, "Red"); + overlays.drawLine(name_prefix + "path", {robot.pos, target_point}, "Red"); } else if(path.size() == 1) { - overlays.drawLine("path", {robot.pos, target_point}, "Purple"); + overlays.drawLine(name_prefix + "path", {robot.pos, target_point}, "Purple"); } else { const auto [closest_index, closest_point] = ProjectRobotOnPath(path, robot); std::vector path_done(path.begin(), path.begin() + (closest_index)); @@ -204,13 +204,13 @@ void MotionExecutor::DrawOverlays( path.end()); path_remaining.insert(path_remaining.begin(), closest_point); const auto translucent_purple = "#8000805F"; - overlays.drawLine("path_done", path_done, translucent_purple); - overlays.drawLine("path_remaining", path_remaining, "Purple"); + overlays.drawLine(name_prefix + "path_done", path_done, translucent_purple); + overlays.drawLine(name_prefix + "path_remaining", path_remaining, "Purple"); const auto & planner = planners_[robot.id]; if (planner.didTimeOut()) { - overlays.drawLine("afterpath", {path.back(), target_point}, "LightSkyBlue"); + overlays.drawLine(name_prefix + "afterpath", {path.back(), target_point}, "LightSkyBlue"); } else if (planner.isPathTruncated()) { - overlays.drawLine("afterpath", {path.back(), target_point}, "LightPink"); + overlays.drawLine(name_prefix + "afterpath", {path.back(), target_point}, "LightPink"); } } } From dc28360909a903d4d098850ff4e014912cc88d1d Mon Sep 17 00:00:00 2001 From: Matthew Barulic Date: Sun, 14 Sep 2025 21:54:08 -0400 Subject: [PATCH 08/35] Migrates Capture, PivotKick, and UniversalKick to motion intents --- ateam_kenobi/CMakeLists.txt | 6 +- ateam_kenobi/src/skills/capture.cpp | 67 ++++++++++------------ ateam_kenobi/src/skills/capture.hpp | 19 ++---- ateam_kenobi/src/skills/pivot_kick.cpp | 47 ++++++++------- ateam_kenobi/src/skills/pivot_kick.hpp | 21 ++----- ateam_kenobi/src/skills/universal_kick.cpp | 10 +--- ateam_kenobi/src/skills/universal_kick.hpp | 4 +- 7 files changed, 67 insertions(+), 107 deletions(-) diff --git a/ateam_kenobi/CMakeLists.txt b/ateam_kenobi/CMakeLists.txt index 2f9968f83..9c9d392bf 100644 --- a/ateam_kenobi/CMakeLists.txt +++ b/ateam_kenobi/CMakeLists.txt @@ -104,11 +104,11 @@ target_sources(kenobi_node_component PRIVATE # src/skills/lane_idler.cpp src/skills/line_kick.cpp # src/skills/pass_receiver.cpp - # src/skills/pivot_kick.cpp + src/skills/pivot_kick.cpp # src/skills/dribble.cpp # src/skills/extract.cpp - # src/skills/capture.cpp - # src/skills/universal_kick.cpp + src/skills/capture.cpp + src/skills/universal_kick.cpp ) target_include_directories(kenobi_node_component PUBLIC diff --git a/ateam_kenobi/src/skills/capture.cpp b/ateam_kenobi/src/skills/capture.cpp index 09b81deb3..5ea6e2fb2 100644 --- a/ateam_kenobi/src/skills/capture.cpp +++ b/ateam_kenobi/src/skills/capture.cpp @@ -30,18 +30,16 @@ namespace ateam_kenobi::skills { Capture::Capture(stp::Options stp_options) -: stp::Skill(stp_options), - easy_move_to_(createChild("EasyMoveTo")) +: stp::Skill(stp_options) {} void Capture::Reset() { done_ = false; ball_detected_filter_ = 0; - easy_move_to_.reset(); } -ateam_msgs::msg::RobotMotionCommand Capture::runFrame(const World & world, const Robot & robot) +RobotCommand Capture::runFrame(const World & world, const Robot & robot) { chooseState(world, robot); @@ -52,7 +50,7 @@ ateam_msgs::msg::RobotMotionCommand Capture::runFrame(const World & world, const return runCapture(world, robot); default: std::cerr << "Unhandled state in Capture!\n"; - return ateam_msgs::msg::RobotMotionCommand{}; + return RobotCommand{}; } } @@ -67,21 +65,21 @@ void Capture::chooseState(const World & world, const Robot & robot) } } -ateam_msgs::msg::RobotMotionCommand Capture::runMoveToBall( +RobotCommand Capture::runMoveToBall( const World & world, const Robot & robot) { - easy_move_to_.face_point(world.ball.pos); + RobotCommand command; - motion::MotionOptions motion_options; - motion_options.completion_threshold = 0; - easy_move_to_.setMotionOptions(motion_options); - path_planning::PlannerOptions planner_options = easy_move_to_.getPlannerOptions(); - planner_options.avoid_ball = false; + command.motion_intent.linear = motion::intents::linear::VelocityAtPositionIntent{ + world.ball.pos, + ateam_geometry::normalize(world.ball.pos - robot.pos) * capture_speed_ + }; - easy_move_to_.setPlannerOptions(planner_options); - easy_move_to_.setTargetPosition(world.ball.pos, - capture_speed_ * 0.8 * ateam_geometry::normalize(world.ball.pos - robot.pos)); + command.motion_intent.angular = motion::intents::angular::FacingIntent{world.ball.pos}; + + command.motion_intent.motion_options.completion_threshold = 0.0; + command.motion_intent.planner_options.avoid_ball = false; const auto distance_to_ball = CGAL::approximate_sqrt(CGAL::squared_distance(robot.pos, world.ball.pos)); @@ -91,12 +89,13 @@ ateam_msgs::msg::RobotMotionCommand Capture::runMoveToBall( const auto max_decel_vel = std::sqrt((2.0 * decel_limit_ * decel_distance) + (capture_speed_ * capture_speed_)); - easy_move_to_.setMaxVelocity(std::min(max_decel_vel, max_speed_)); + // TODO(barulicm): Set max velocity to std::min(max_decel_vel, max_speed_) + (void)max_decel_vel; - return easy_move_to_.runFrame(robot, world); + return command; } -ateam_msgs::msg::RobotMotionCommand Capture::runCapture(const World & world, const Robot & robot) +RobotCommand Capture::runCapture(const World & world, const Robot & robot) { // TODO(chachmu): Should we filter this over a few frames to make sure we have the ball settled? if (robot.breakbeam_ball_detected) { @@ -111,32 +110,24 @@ ateam_msgs::msg::RobotMotionCommand Capture::runCapture(const World & world, con } } - /* TODO(chachmu): If we disable default obstacles do we need to check if the target is off the - * field? - */ - path_planning::PlannerOptions planner_options = easy_move_to_.getPlannerOptions(); - planner_options.avoid_ball = false; - planner_options.footprint_inflation = 0.0; - // planner_options.draw_obstacles = true; - easy_move_to_.setPlannerOptions(planner_options); + RobotCommand command; - motion::MotionOptions motion_options; - motion_options.completion_threshold = 0; - easy_move_to_.setMotionOptions(motion_options); + command.motion_intent.planner_options.avoid_ball = false; + command.motion_intent.planner_options.footprint_inflation = 0.0; + command.motion_intent.motion_options.completion_threshold = 0.0; + + // TODO(barulicm): Set max velocity to capture_speed_ - easy_move_to_.setMaxVelocity(capture_speed_); if(world.ball.visible) { - easy_move_to_.face_point(world.ball.pos); + command.motion_intent.angular = motion::intents::angular::FacingIntent{world.ball.pos}; } else { - easy_move_to_.face_absolute(robot.theta); + command.motion_intent.angular = motion::intents::angular::HeadingIntent{robot.theta}; } - easy_move_to_.setTargetPosition(world.ball.pos); - auto command = easy_move_to_.runFrame(robot, world); - - command.twist.linear.x = capture_speed_; - command.twist.linear.y = 0.0; - command.twist_frame = ateam_msgs::msg::RobotMotionCommand::FRAME_BODY; + command.motion_intent.linear = motion::intents::linear::VelocityIntent{ + ateam_geometry::Vector{capture_speed_, 0.0}, + motion::intents::linear::Frame::Local + }; command.dribbler_speed = kDefaultDribblerSpeed; diff --git a/ateam_kenobi/src/skills/capture.hpp b/ateam_kenobi/src/skills/capture.hpp index ce3be9b25..d6447191d 100644 --- a/ateam_kenobi/src/skills/capture.hpp +++ b/ateam_kenobi/src/skills/capture.hpp @@ -23,9 +23,9 @@ #define SKILLS__CAPTURE_HPP_ #include -#include "core/play_helpers/easy_move_to.hpp" #include #include "core/stp/skill.hpp" +#include "core/types/robot_command.hpp" #include "core/types/world.hpp" @@ -50,17 +50,7 @@ class Capture : public stp::Skill } - ateam_msgs::msg::RobotMotionCommand runFrame(const World & world, const Robot & robot); - - /** - * @brief Set the default obstacles planner option on the internal EasyMoveTo - */ - void SetUseDefaultObstacles(bool use_obstacles) - { - path_planning::PlannerOptions options = easy_move_to_.getPlannerOptions(); - options.use_default_obstacles = use_obstacles; - easy_move_to_.setPlannerOptions(options); - } + RobotCommand runFrame(const World & world, const Robot & robot); /** * @brief Set the capture speed used to approach the ball in the final phase @@ -73,7 +63,6 @@ class Capture : public stp::Skill } private: - play_helpers::EasyMoveTo easy_move_to_; bool done_ = false; int ball_detected_filter_ = 0; double approach_radius_ = 0.3; // m @@ -90,8 +79,8 @@ class Capture : public stp::Skill void chooseState(const World & world, const Robot & robot); - ateam_msgs::msg::RobotMotionCommand runMoveToBall(const World & world, const Robot & robot); - ateam_msgs::msg::RobotMotionCommand runCapture(const World & world, const Robot & robot); + RobotCommand runMoveToBall(const World & world, const Robot & robot); + RobotCommand runCapture(const World & world, const Robot & robot); }; } // namespace ateam_kenobi::skills diff --git a/ateam_kenobi/src/skills/pivot_kick.cpp b/ateam_kenobi/src/skills/pivot_kick.cpp index 5be8ac64a..2621cc92e 100644 --- a/ateam_kenobi/src/skills/pivot_kick.cpp +++ b/ateam_kenobi/src/skills/pivot_kick.cpp @@ -40,13 +40,13 @@ ateam_geometry::Point PivotKick::GetAssignmentPoint(const World & world) return world.ball.pos; } -ateam_msgs::msg::RobotMotionCommand PivotKick::RunFrame(const World & world, const Robot & robot) +RobotCommand PivotKick::RunFrame(const World & world, const Robot & robot) { getOverlays().drawLine("PivotKick_line", {world.ball.pos, target_point_}, "#FFFF007F"); if (done_) { getPlayInfo()["State"] = "Done"; - return ateam_msgs::msg::RobotMotionCommand{}; + return RobotCommand{}; } if (prev_state_ == State::Capture) { @@ -82,28 +82,28 @@ ateam_msgs::msg::RobotMotionCommand PivotKick::RunFrame(const World & world, con if (ateam_geometry::norm(world.ball.vel) > 0.1 * GetKickSpeed()) { done_ = true; getPlayInfo()["State"] = "Done"; - return ateam_msgs::msg::RobotMotionCommand{}; + return RobotCommand{}; } getPlayInfo()["State"] = "Kick"; - return KickBall(world, robot); + return KickBall(); } -ateam_msgs::msg::RobotMotionCommand PivotKick::Capture( +RobotCommand PivotKick::Capture( const World & world, const Robot & robot) { return capture_.runFrame(world, robot); } -ateam_msgs::msg::RobotMotionCommand PivotKick::Pivot(const Robot & robot) +RobotCommand PivotKick::Pivot(const Robot & robot) { const auto robot_to_target = target_point_ - robot.pos; const auto robot_to_target_angle = std::atan2(robot_to_target.y(), robot_to_target.x()); const auto angle_error = angles::shortest_angular_distance(robot.theta, robot_to_target_angle); - ateam_msgs::msg::RobotMotionCommand command; + RobotCommand command; const double vel = robot.prev_command_omega; const double dt = 0.01; @@ -129,7 +129,8 @@ ateam_msgs::msg::RobotMotionCommand PivotKick::Pivot(const Robot & robot) trapezoidal_vel = std::copysign(min_angular_vel, angle_error); } - command.twist.angular.z = std::clamp(trapezoidal_vel, -pivot_speed_, pivot_speed_); + const auto angular_vel = std::clamp(trapezoidal_vel, -pivot_speed_, pivot_speed_); + command.motion_intent.angular = motion::intents::angular::VelocityIntent{angular_vel}; /* rotate in a circle with diameter 0.0427 + 0.18 = 0.2227 (This might be tunable to use 8cm for * real robots) @@ -139,34 +140,32 @@ ateam_msgs::msg::RobotMotionCommand PivotKick::Pivot(const Robot & robot) // double diameter = kBallDiameter + kRobotDiameter; double diameter = (2 * .095) * 1.05; double circumference = M_PI * diameter; - double velocity = circumference * (command.twist.angular.z / (2 * M_PI)); + double velocity = circumference * (angular_vel / (2 * M_PI)); + + command.motion_intent.linear = motion::intents::linear::VelocityIntent{ + ateam_geometry::Vector(0, -velocity), + motion::intents::linear::Frame::Local}; - command.twist.linear.x = 0.0; - command.twist.linear.y = -velocity; - command.twist_frame = ateam_msgs::msg::RobotMotionCommand::FRAME_BODY; command.dribbler_speed = kDefaultDribblerSpeed; + return command; } -ateam_msgs::msg::RobotMotionCommand PivotKick::KickBall(const World & world, const Robot & robot) +RobotCommand PivotKick::KickBall() { - easy_move_to_.setTargetPosition(robot.pos); - easy_move_to_.face_point(target_point_); - path_planning::PlannerOptions planner_options; - planner_options.avoid_ball = false; - planner_options.footprint_inflation = 0.0; - planner_options.use_default_obstacles = false; - easy_move_to_.setPlannerOptions(planner_options); - auto command = easy_move_to_.runFrame(robot, world); + RobotCommand command; + + command.motion_intent.angular = motion::intents::angular::FacingIntent{target_point_}; + command.dribbler_speed = kDefaultDribblerSpeed; + command.kick_speed = GetKickSpeed(); if (IsAllowedToKick()) { if(KickOrChip() == KickSkill::KickChip::Kick) { - command.kick_request = ateam_msgs::msg::RobotMotionCommand::KR_KICK_TOUCH; + command.kick = KickState::KickOnTouch; } else { - command.kick_request = ateam_msgs::msg::RobotMotionCommand::KR_CHIP_TOUCH; + command.kick = KickState::ChipOnTouch; } - command.kick_speed = GetKickSpeed(); } return command; diff --git a/ateam_kenobi/src/skills/pivot_kick.hpp b/ateam_kenobi/src/skills/pivot_kick.hpp index fa7a63b02..e4dc2e545 100644 --- a/ateam_kenobi/src/skills/pivot_kick.hpp +++ b/ateam_kenobi/src/skills/pivot_kick.hpp @@ -22,9 +22,9 @@ #ifndef SKILLS__PIVOT_KICK_HPP_ #define SKILLS__PIVOT_KICK_HPP_ -#include #include #include "kick_skill.hpp" +#include "core/types/robot_command.hpp" #include "core/types/world.hpp" #include "core/play_helpers/easy_move_to.hpp" #include "skills/capture.hpp" @@ -60,24 +60,13 @@ class PivotKick : public KickSkill ateam_geometry::Point GetAssignmentPoint(const World & world); - ateam_msgs::msg::RobotMotionCommand RunFrame(const World & world, const Robot & robot); + RobotCommand RunFrame(const World & world, const Robot & robot); bool IsDone() const { return done_; } - /** - * @brief Set the default obstacles planner option on the internal EasyMoveTo - */ - void SetUseDefaultObstacles(bool use_obstacles) - { - path_planning::PlannerOptions options = easy_move_to_.getPlannerOptions(); - options.use_default_obstacles = use_obstacles; - easy_move_to_.setPlannerOptions(options); - capture_.SetUseDefaultObstacles(use_obstacles); - } - void SetCaptureSpeed(double speed) { capture_.SetCaptureSpeed(speed); @@ -109,11 +98,11 @@ class PivotKick : public KickSkill }; State prev_state_ = State::Capture; - ateam_msgs::msg::RobotMotionCommand Capture(const World & world, const Robot & robot); + RobotCommand Capture(const World & world, const Robot & robot); - ateam_msgs::msg::RobotMotionCommand Pivot(const Robot & robot); + RobotCommand Pivot(const Robot & robot); - ateam_msgs::msg::RobotMotionCommand KickBall(const World & world, const Robot & robot); + RobotCommand KickBall(); }; } // namespace ateam_kenobi::skills diff --git a/ateam_kenobi/src/skills/universal_kick.cpp b/ateam_kenobi/src/skills/universal_kick.cpp index e11c66a6d..532cee54a 100644 --- a/ateam_kenobi/src/skills/universal_kick.cpp +++ b/ateam_kenobi/src/skills/universal_kick.cpp @@ -81,12 +81,6 @@ bool UniversalKick::IsDone() const } } -void UniversalKick::SetUseDefaultObstacles(bool use_obstacles) -{ - pivot_kick_.SetUseDefaultObstacles(use_obstacles); - line_kick_.SetUseDefaultObstacles(use_obstacles); -} - void UniversalKick::SetKickChip(KickSkill::KickChip kc) { pivot_kick_.SetKickChip(kc); @@ -98,7 +92,7 @@ void UniversalKick::SetPreferredKickType(KickType type) preferred_type_ = type; } -ateam_msgs::msg::RobotMotionCommand UniversalKick::RunFrame( +RobotCommand UniversalKick::RunFrame( const World & world, const Robot & robot) { @@ -123,7 +117,7 @@ ateam_msgs::msg::RobotMotionCommand UniversalKick::RunFrame( case KickType::Unset: default: last_used_ = KickType::Unset; - return ateam_msgs::msg::RobotMotionCommand{}; + return RobotCommand{}; } } diff --git a/ateam_kenobi/src/skills/universal_kick.hpp b/ateam_kenobi/src/skills/universal_kick.hpp index b7b1c6f64..d4f53354e 100644 --- a/ateam_kenobi/src/skills/universal_kick.hpp +++ b/ateam_kenobi/src/skills/universal_kick.hpp @@ -50,13 +50,11 @@ class UniversalKick : public KickSkill bool IsDone() const; - void SetUseDefaultObstacles(bool use_obstacles); - void SetKickChip(KickSkill::KickChip kc); void SetPreferredKickType(KickType type); - ateam_msgs::msg::RobotMotionCommand RunFrame(const World & world, const Robot & robot); + RobotCommand RunFrame(const World & world, const Robot & robot); /* * Pivot-only functions From fa757f231f97fcc88b8fabbb59c925b2c94cffaf Mon Sep 17 00:00:00 2001 From: Matthew Barulic Date: Sun, 14 Sep 2025 22:05:15 -0400 Subject: [PATCH 09/35] Migrates LaneIdler to motion intents --- ateam_kenobi/CMakeLists.txt | 2 +- ateam_kenobi/src/skills/lane_idler.cpp | 17 ++++++----------- ateam_kenobi/src/skills/lane_idler.hpp | 7 ++----- 3 files changed, 9 insertions(+), 17 deletions(-) diff --git a/ateam_kenobi/CMakeLists.txt b/ateam_kenobi/CMakeLists.txt index 9c9d392bf..5e8d9594a 100644 --- a/ateam_kenobi/CMakeLists.txt +++ b/ateam_kenobi/CMakeLists.txt @@ -101,7 +101,7 @@ target_sources(kenobi_node_component PRIVATE target_sources(kenobi_node_component PRIVATE src/skills/goalie.cpp src/skills/kick_skill.cpp - # src/skills/lane_idler.cpp + src/skills/lane_idler.cpp src/skills/line_kick.cpp # src/skills/pass_receiver.cpp src/skills/pivot_kick.cpp diff --git a/ateam_kenobi/src/skills/lane_idler.cpp b/ateam_kenobi/src/skills/lane_idler.cpp index b3346d955..66ae52641 100644 --- a/ateam_kenobi/src/skills/lane_idler.cpp +++ b/ateam_kenobi/src/skills/lane_idler.cpp @@ -27,25 +27,20 @@ namespace ateam_kenobi::skills { LaneIdler::LaneIdler(stp::Options stp_options) -: stp::Skill(stp_options), - easy_move_to_(createChild("EasyMoveTo")) +: stp::Skill(stp_options) {} -void LaneIdler::Reset() -{ - easy_move_to_.reset(); -} - ateam_geometry::Point LaneIdler::GetAssignmentPoint(const World & world) { return GetIdlingPosition(world); } -ateam_msgs::msg::RobotMotionCommand LaneIdler::RunFrame(const World & world, const Robot & robot) +RobotCommand LaneIdler::RunFrame(const World & world) { - easy_move_to_.setTargetPosition(GetIdlingPosition(world)); - easy_move_to_.face_point(world.ball.pos); - return easy_move_to_.runFrame(robot, world, extra_obstacles_); + RobotCommand command; + command.motion_intent.linear = motion::intents::linear::PositionIntent{GetIdlingPosition(world)}; + command.motion_intent.angular = motion::intents::angular::FacingIntent{world.ball.pos}; + return command; } ateam_geometry::Point LaneIdler::GetIdlingPosition(const World & world) diff --git a/ateam_kenobi/src/skills/lane_idler.hpp b/ateam_kenobi/src/skills/lane_idler.hpp index dc00215a6..df1ce8191 100644 --- a/ateam_kenobi/src/skills/lane_idler.hpp +++ b/ateam_kenobi/src/skills/lane_idler.hpp @@ -24,7 +24,7 @@ #include #include #include "core/stp/skill.hpp" -#include "core/play_helpers/easy_move_to.hpp" +#include "core/types/robot_command.hpp" #include "core/play_helpers/lanes.hpp" namespace ateam_kenobi::skills @@ -35,11 +35,9 @@ class LaneIdler : public stp::Skill public: explicit LaneIdler(stp::Options stp_options); - void Reset(); - ateam_geometry::Point GetAssignmentPoint(const World & world); - ateam_msgs::msg::RobotMotionCommand RunFrame(const World & world, const Robot & robot); + RobotCommand RunFrame(const World & world); void SetLane(play_helpers::lanes::Lane lane) { @@ -53,7 +51,6 @@ class LaneIdler : public stp::Skill private: play_helpers::lanes::Lane lane_ = play_helpers::lanes::Lane::Center; - play_helpers::EasyMoveTo easy_move_to_; std::vector extra_obstacles_; ateam_geometry::Point GetIdlingPosition(const World & world); From 4608d542cf6ae6e488de858ca2d45fd704f8339f Mon Sep 17 00:00:00 2001 From: Matthew Barulic Date: Sun, 14 Sep 2025 22:35:54 -0400 Subject: [PATCH 10/35] Migrates Dribble to motion intents --- ateam_kenobi/CMakeLists.txt | 2 +- ateam_kenobi/src/skills/dribble.cpp | 97 +++++++++++++---------------- ateam_kenobi/src/skills/dribble.hpp | 12 ++-- 3 files changed, 50 insertions(+), 61 deletions(-) diff --git a/ateam_kenobi/CMakeLists.txt b/ateam_kenobi/CMakeLists.txt index 5e8d9594a..f0fe737ca 100644 --- a/ateam_kenobi/CMakeLists.txt +++ b/ateam_kenobi/CMakeLists.txt @@ -105,7 +105,7 @@ target_sources(kenobi_node_component PRIVATE src/skills/line_kick.cpp # src/skills/pass_receiver.cpp src/skills/pivot_kick.cpp - # src/skills/dribble.cpp + src/skills/dribble.cpp # src/skills/extract.cpp src/skills/capture.cpp src/skills/universal_kick.cpp diff --git a/ateam_kenobi/src/skills/dribble.cpp b/ateam_kenobi/src/skills/dribble.cpp index 88f13ac1a..95b493bf2 100644 --- a/ateam_kenobi/src/skills/dribble.cpp +++ b/ateam_kenobi/src/skills/dribble.cpp @@ -30,8 +30,7 @@ namespace ateam_kenobi::skills { Dribble::Dribble(stp::Options stp_options) -: stp::Skill(stp_options), - easy_move_to_(createChild("EasyMoveTo")) +: stp::Skill(stp_options) { back_away_duration_ = std::chrono::seconds(1); } @@ -40,10 +39,9 @@ void Dribble::reset() { done_ = false; ball_detected_filter_ = 0; - easy_move_to_.reset(); } -ateam_msgs::msg::RobotMotionCommand Dribble::runFrame(const World & world, const Robot & robot) +RobotCommand Dribble::runFrame(const World & world, const Robot & robot) { const auto start_position = getStartPosition(world); @@ -57,14 +55,14 @@ ateam_msgs::msg::RobotMotionCommand Dribble::runFrame(const World & world, const return runMoveBehindBall(world, robot); case State::Dribble: getPlayInfo()["State"] = "Dribble to Point"; - return runDribble(world, robot); + return runDribble(robot); case State::BackAway: getPlayInfo()["State"] = "Back Away"; return runBackAway(world, robot); default: std::cerr << "Unhandled state in dribble!\n"; - return ateam_msgs::msg::RobotMotionCommand{}; + return RobotCommand{}; } } @@ -181,80 +179,73 @@ bool Dribble::robotHasBall(const Robot & robot) return false; } -ateam_msgs::msg::RobotMotionCommand Dribble::runMoveBehindBall( +RobotCommand Dribble::runMoveBehindBall( const World & world, const Robot & robot) { - easy_move_to_.face_point(target_); - - motion::MotionOptions motion_options; - motion_options.completion_threshold = 0; - easy_move_to_.setMotionOptions(motion_options); - path_planning::PlannerOptions planner_options; - planner_options.footprint_inflation = 0.06; - planner_options.draw_obstacles = true; - planner_options.ignore_start_obstacle = false; - easy_move_to_.setPlannerOptions(planner_options); - easy_move_to_.setTargetPosition(getStartPosition(world)); - easy_move_to_.setMaxVelocity(1.5); - - auto command = easy_move_to_.runFrame(robot, world); + RobotCommand command; + + command.motion_intent.angular = motion::intents::angular::FacingIntent{target_}; + + command.motion_intent.motion_options.completion_threshold = 0.0; + command.motion_intent.planner_options.footprint_inflation = 0.06; + command.motion_intent.planner_options.draw_obstacles = true; + command.motion_intent.planner_options.ignore_start_obstacle = false; + + // TODO(barulicm): Set max velocity to 1.5 + if (ateam_geometry::norm(robot.pos - world.ball.pos) < 0.5) { command.dribbler_speed = 130; } return command; } -ateam_msgs::msg::RobotMotionCommand Dribble::runDribble(const World & world, const Robot & robot) +RobotCommand Dribble::runDribble(const Robot & robot) { - path_planning::PlannerOptions planner_options; - planner_options.avoid_ball = false; - planner_options.footprint_inflation = 0.0; - planner_options.use_default_obstacles = false; - planner_options.draw_obstacles = true; - easy_move_to_.setPlannerOptions(planner_options); + RobotCommand command; + + command.motion_intent.planner_options.avoid_ball = false; + command.motion_intent.planner_options.footprint_inflation = 0.0; + command.motion_intent.planner_options.use_default_obstacles = false; + command.motion_intent.planner_options.draw_obstacles = true; - easy_move_to_.setMaxVelocity(0.35); - easy_move_to_.face_point(target_); + command.motion_intent.angular = motion::intents::angular::FacingIntent{target_}; - // Offset the robot position so the ball is on the target point const auto robot_to_target = target_ - robot.pos; - easy_move_to_.setTargetPosition( - target_ - - (kRobotRadius * ateam_geometry::normalize(robot_to_target))); - auto command = easy_move_to_.runFrame(robot, world); + const auto position_target = target_ - (kRobotRadius * ateam_geometry::normalize(robot_to_target)); + command.motion_intent.linear = motion::intents::linear::PositionIntent{position_target}; + + // TODO(barulicm): Set max velocity to 0.35 command.dribbler_speed = 130; return command; } -ateam_msgs::msg::RobotMotionCommand Dribble::runBackAway(const World & world, const Robot & robot) +RobotCommand Dribble::runBackAway(const World & world, const Robot & robot) { - path_planning::PlannerOptions planner_options; - planner_options.avoid_ball = false; - planner_options.footprint_inflation = 0.0; - planner_options.use_default_obstacles = false; - planner_options.draw_obstacles = true; - easy_move_to_.setPlannerOptions(planner_options); - - auto command = ateam_msgs::msg::RobotMotionCommand(); - command.dribbler_speed = 0; + RobotCommand command; - easy_move_to_.setMaxVelocity(0.35); - easy_move_to_.face_absolute(robot.theta); + command.motion_intent.planner_options.avoid_ball = false; + command.motion_intent.planner_options.footprint_inflation = 0.0; + command.motion_intent.planner_options.use_default_obstacles = false; + command.motion_intent.planner_options.draw_obstacles = true; + + command.dribbler_speed = 0; - const auto ball_to_robot = robot.pos - world.ball.pos; - easy_move_to_.setTargetPosition(robot.pos + - (0.2 * ateam_geometry::normalize(ball_to_robot))); + // TODO(barulicm): Set max velocity to 0.35 // Wait for the dribbler to wind down before moving if ((std::chrono::steady_clock::now() - back_away_duration_.value()) > back_away_start_) { if (!world.ball.visible) { - command.twist.linear.x = -0.35; - command.twist_frame = ateam_msgs::msg::RobotMotionCommand::FRAME_BODY; + command.motion_intent.linear = motion::intents::linear::VelocityIntent{ + ateam_geometry::Vector{-0.35, 0.0}, + motion::intents::linear::Frame::Local}; } else { - command = easy_move_to_.runFrame(robot, world); + const auto ball_to_robot = robot.pos - world.ball.pos; + const auto position_target = robot.pos + + (0.2 * ateam_geometry::normalize(ball_to_robot)); + command.motion_intent.linear = motion::intents::linear::PositionIntent{position_target}; } } diff --git a/ateam_kenobi/src/skills/dribble.hpp b/ateam_kenobi/src/skills/dribble.hpp index a523b38d2..b9d58cd2f 100644 --- a/ateam_kenobi/src/skills/dribble.hpp +++ b/ateam_kenobi/src/skills/dribble.hpp @@ -22,10 +22,9 @@ #ifndef SKILLS__DRIBBLE_HPP_ #define SKILLS__DRIBBLE_HPP_ -#include -#include "core/play_helpers/easy_move_to.hpp" #include #include "core/stp/skill.hpp" +#include "core/types/robot_command.hpp" #include "core/types/world.hpp" @@ -56,13 +55,12 @@ class Dribble : public stp::Skill } - ateam_msgs::msg::RobotMotionCommand runFrame(const World & world, const Robot & robot); + RobotCommand runFrame(const World & world, const Robot & robot); private: const double kOffset = kRobotRadius + kBallRadius + 0.07; ateam_geometry::Point target_; double target_threshold_ = 0.1; - play_helpers::EasyMoveTo easy_move_to_; int ball_detected_filter_ = 0; bool done_ = false; @@ -86,9 +84,9 @@ class Dribble : public stp::Skill bool isRobotSettled(const World & world, const Robot & robot); bool robotHasBall(const Robot & robot); - ateam_msgs::msg::RobotMotionCommand runMoveBehindBall(const World & world, const Robot & robot); - ateam_msgs::msg::RobotMotionCommand runDribble(const World & world, const Robot & robot); - ateam_msgs::msg::RobotMotionCommand runBackAway(const World & world, const Robot & robot); + RobotCommand runMoveBehindBall(const World & world, const Robot & robot); + RobotCommand runDribble(const Robot & robot); + RobotCommand runBackAway(const World & world, const Robot & robot); }; } // namespace ateam_kenobi::skills From f65cefecd6dd538d80d78113657cd6cc5335d08a Mon Sep 17 00:00:00 2001 From: Matthew Barulic Date: Sun, 14 Sep 2025 22:49:42 -0400 Subject: [PATCH 11/35] Migrates Extract skill to motion intents --- ateam_kenobi/CMakeLists.txt | 2 +- ateam_kenobi/src/skills/extract.cpp | 46 ++++++++++++----------------- ateam_kenobi/src/skills/extract.hpp | 5 ++-- 3 files changed, 22 insertions(+), 31 deletions(-) diff --git a/ateam_kenobi/CMakeLists.txt b/ateam_kenobi/CMakeLists.txt index f0fe737ca..843ba44de 100644 --- a/ateam_kenobi/CMakeLists.txt +++ b/ateam_kenobi/CMakeLists.txt @@ -106,7 +106,7 @@ target_sources(kenobi_node_component PRIVATE # src/skills/pass_receiver.cpp src/skills/pivot_kick.cpp src/skills/dribble.cpp - # src/skills/extract.cpp + src/skills/extract.cpp src/skills/capture.cpp src/skills/universal_kick.cpp ) diff --git a/ateam_kenobi/src/skills/extract.cpp b/ateam_kenobi/src/skills/extract.cpp index d54b88f6c..bc6a76029 100644 --- a/ateam_kenobi/src/skills/extract.cpp +++ b/ateam_kenobi/src/skills/extract.cpp @@ -24,18 +24,16 @@ namespace ateam_kenobi::skills { Extract::Extract(stp::Options stp_options) -: stp::Skill(stp_options), - easy_move_to_(createChild("easy_move_to")) +: stp::Skill(stp_options) {} void Extract::Reset() { - easy_move_to_.reset(); ballsense_count_ = 0; rip_start_time_.reset(); } -ateam_msgs::msg::RobotMotionCommand Extract::RunFrame(const World & world, const Robot & robot) +RobotCommand Extract::RunFrame(const World & world, const Robot & robot) { if (robot.breakbeam_ball_detected) { ballsense_count_++; @@ -51,41 +49,35 @@ ateam_msgs::msg::RobotMotionCommand Extract::RunFrame(const World & world, const const auto ball_far = ateam_geometry::norm(robot.pos - world.ball.pos) > 0.4; + RobotCommand command; + if (should_rip) { if (!rip_start_time_) { rip_start_time_ = std::chrono::steady_clock::now(); } - auto planner_options = easy_move_to_.getPlannerOptions(); - planner_options.avoid_ball = false; - planner_options.footprint_inflation = -0.1; - easy_move_to_.setPlannerOptions(planner_options); - easy_move_to_.setTargetPosition( - robot.pos + - (ateam_geometry::normalize(robot.pos - world.ball.pos) * 0.25)); - easy_move_to_.setMaxAngularVelocity(2.0); + command.motion_intent.planner_options.avoid_ball = false; + command.motion_intent.planner_options.footprint_inflation = -0.1; + command.motion_intent.linear = motion::intents::linear::PositionIntent{ + robot.pos + ateam_geometry::normalize(world.ball.pos - robot.pos) * 0.25 + }; + // TODO(barulicm): Set max angular velocity to 2.0 } else if (ball_far) { - auto planner_options = easy_move_to_.getPlannerOptions(); - planner_options.avoid_ball = false; - planner_options.footprint_inflation = path_planning::PlannerOptions{}.footprint_inflation; - easy_move_to_.setPlannerOptions(planner_options); - easy_move_to_.setTargetPosition(world.ball.pos); - easy_move_to_.setMaxVelocity(2.0); + command.motion_intent.planner_options.avoid_ball = false; + command.motion_intent.linear = motion::intents::linear::PositionIntent{world.ball.pos}; + // TODO(barulicm): Set max angular velocity to 2.0 } else { - auto planner_options = easy_move_to_.getPlannerOptions(); - planner_options.avoid_ball = false; - planner_options.footprint_inflation = -0.1; - easy_move_to_.setPlannerOptions(planner_options); - easy_move_to_.setTargetPosition(world.ball.pos); - easy_move_to_.setMaxVelocity(0.35); + command.motion_intent.planner_options.avoid_ball = false; + command.motion_intent.planner_options.footprint_inflation = -0.1; + command.motion_intent.linear = motion::intents::linear::PositionIntent{world.ball.pos}; + // TODO(barulicm): Set max velocity to 0.35 } - easy_move_to_.face_point(world.ball.pos); - auto command = easy_move_to_.runFrame(robot, world); + command.motion_intent.angular = motion::intents::angular::FacingIntent{world.ball.pos}; + if (robot.breakbeam_ball_detected) { command.dribbler_speed = kDefaultDribblerSpeed; } return command; } - } // namespace ateam_kenobi::skills diff --git a/ateam_kenobi/src/skills/extract.hpp b/ateam_kenobi/src/skills/extract.hpp index b68631246..bbe395958 100644 --- a/ateam_kenobi/src/skills/extract.hpp +++ b/ateam_kenobi/src/skills/extract.hpp @@ -22,7 +22,7 @@ #define SKILLS__EXTRACT_HPP_ #include "core/stp/skill.hpp" -#include "core/play_helpers/easy_move_to.hpp" +#include "core/types/robot_command.hpp" namespace ateam_kenobi::skills { @@ -34,7 +34,7 @@ class Extract : public stp::Skill void Reset(); - ateam_msgs::msg::RobotMotionCommand RunFrame(const World & world, const Robot & robot); + RobotCommand RunFrame(const World & world, const Robot & robot); ateam_geometry::Point GetAssignmentPoint(const World & world) { @@ -43,7 +43,6 @@ class Extract : public stp::Skill private: const int kBallsenseCountThreshold_ = 10; - play_helpers::EasyMoveTo easy_move_to_; int ballsense_count_ = 0; std::optional rip_start_time_; }; From 15df9cd9615f15eedbd7e8387dbd68a359cf9285 Mon Sep 17 00:00:00 2001 From: Matthew Barulic Date: Sun, 14 Sep 2025 23:10:20 -0400 Subject: [PATCH 12/35] Migrates PassReceiver to motion intents --- ateam_kenobi/CMakeLists.txt | 3 +- .../src/core/motion/frame_conversions.cpp | 46 ++++++++++ .../src/core/motion/frame_conversions.hpp | 41 +++++++++ ateam_kenobi/src/skills/pass_receiver.cpp | 92 +++++++++---------- ateam_kenobi/src/skills/pass_receiver.hpp | 14 ++- 5 files changed, 140 insertions(+), 56 deletions(-) create mode 100644 ateam_kenobi/src/core/motion/frame_conversions.cpp create mode 100644 ateam_kenobi/src/core/motion/frame_conversions.hpp diff --git a/ateam_kenobi/CMakeLists.txt b/ateam_kenobi/CMakeLists.txt index 843ba44de..267865e49 100644 --- a/ateam_kenobi/CMakeLists.txt +++ b/ateam_kenobi/CMakeLists.txt @@ -35,6 +35,7 @@ target_sources(kenobi_node_component PRIVATE src/core/path_planning/obstacles.cpp src/core/path_planning/path_planner.cpp src/core/types/message_conversions.cpp + src/core/motion/frame_conversions.cpp src/core/motion/motion_controller.cpp src/core/motion/motion_executor.cpp src/core/visualization/overlays.cpp @@ -103,7 +104,7 @@ target_sources(kenobi_node_component PRIVATE src/skills/kick_skill.cpp src/skills/lane_idler.cpp src/skills/line_kick.cpp - # src/skills/pass_receiver.cpp + src/skills/pass_receiver.cpp src/skills/pivot_kick.cpp src/skills/dribble.cpp src/skills/extract.cpp diff --git a/ateam_kenobi/src/core/motion/frame_conversions.cpp b/ateam_kenobi/src/core/motion/frame_conversions.cpp new file mode 100644 index 000000000..31052d4de --- /dev/null +++ b/ateam_kenobi/src/core/motion/frame_conversions.cpp @@ -0,0 +1,46 @@ +// Copyright 2025 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 "frame_conversions.hpp" +#include + +namespace ateam_kenobi::motion +{ + +ateam_geometry::Vector WorldToLocalFrame( + const ateam_geometry::Vector & world_vector, + const Robot & robot) +{ + CGAL::Aff_transformation_2 transformation(CGAL::ROTATION, + std::sin(-robot.theta), std::cos(-robot.theta)); + return world_vector.transform(transformation); +} + +ateam_geometry::Vector LocalToWorldFrame( + const ateam_geometry::Vector & local_vector, + const Robot & robot) +{ + CGAL::Aff_transformation_2 transformation(CGAL::ROTATION, + std::sin(robot.theta), std::cos(robot.theta)); + return local_vector.transform(transformation); +} + +} // namespace ateam_kenobi::motion diff --git a/ateam_kenobi/src/core/motion/frame_conversions.hpp b/ateam_kenobi/src/core/motion/frame_conversions.hpp new file mode 100644 index 000000000..008c59d0e --- /dev/null +++ b/ateam_kenobi/src/core/motion/frame_conversions.hpp @@ -0,0 +1,41 @@ +// Copyright 2025 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 CORE__MOTION__FRAME_CONVERSIONS_HPP_ +#define CORE__MOTION__FRAME_CONVERSIONS_HPP_ + +#include +#include "core/types/robot.hpp" + +namespace ateam_kenobi::motion +{ + +ateam_geometry::Vector WorldToLocalFrame( + const ateam_geometry::Vector & world_vector, + const Robot & robot); + +ateam_geometry::Vector LocalToWorldFrame( + const ateam_geometry::Vector & local_vector, + const Robot & robot); + +} // namespace ateam_kenobi::motion + +#endif // CORE__MOTION__FRAME_CONVERSIONS_HPP_ diff --git a/ateam_kenobi/src/skills/pass_receiver.cpp b/ateam_kenobi/src/skills/pass_receiver.cpp index ab537556b..383d27694 100644 --- a/ateam_kenobi/src/skills/pass_receiver.cpp +++ b/ateam_kenobi/src/skills/pass_receiver.cpp @@ -19,27 +19,26 @@ // THE SOFTWARE. #include "pass_receiver.hpp" -#include "core/play_helpers/available_robots.hpp" #include +#include "core/play_helpers/available_robots.hpp" +#include "core/motion/motion_intent.hpp" +#include "core/motion/frame_conversions.hpp" namespace ateam_kenobi::skills { PassReceiver::PassReceiver(stp::Options stp_options) -: stp::Skill(stp_options), - easy_move_to_(createChild("EasyMoveTo")) +: stp::Skill(stp_options) {} void PassReceiver::reset() { done_ = false; kick_happened_ = false; - easy_move_to_.reset(); } -ateam_msgs::msg::RobotMotionCommand PassReceiver::runFrame(const World & world, const Robot & robot) +RobotCommand PassReceiver::runFrame(const World & world, const Robot & robot) { - easy_move_to_.setMaxVelocity(2.0); // reset default max vel getPlayInfo()["Robot"] = robot.id; if (done_) { getPlayInfo()["State"] = "Done"; @@ -53,28 +52,34 @@ ateam_msgs::msg::RobotMotionCommand PassReceiver::runFrame(const World & world, const auto ball_was_kicked = hasBallBeenKicked(world); const auto ball_stalled_and_reachable = isBallStalledAndReachable(world, robot); + RobotCommand command; + if (bot_close_to_target && ball_vel_matching_bot_vel && ball_close) { done_ = true; getPlayInfo()["State"] = "Done"; - return runPostPass(); + command = runPostPass(); } else if (ball_was_kicked && ball_close && !ball_fast) { done_ = true; getPlayInfo()["State"] = "Done"; - return runPostPass(); + command = runPostPass(); } else if (ball_was_kicked && ball_fast && !ball_vel_matching_bot_vel) { getPlayInfo()["State"] = "Pass"; - return runPass(world, robot); + command = runPass(world, robot); } else if (ball_was_kicked && ball_close && ball_vel_matching_bot_vel) { done_ = true; getPlayInfo()["State"] = "Post"; - return runPostPass(); + command = runPostPass(); } else if (ball_stalled_and_reachable) { getPlayInfo()["State"] = "Capture"; - return runApproachBall(world, robot); + command = runApproachBall(world, robot); } else { getPlayInfo()["State"] = "Pre"; - return runPrePass(world, robot); + command = runPrePass(world, robot); } + + // TODO(barulicm): Set max velocity to 2.0 + + return command; } bool PassReceiver::isBallFast(const World & world) @@ -111,7 +116,7 @@ bool PassReceiver::hasBallBeenKicked(const World & world) return kick_happened_; } -ateam_msgs::msg::RobotMotionCommand PassReceiver::runPrePass( +RobotCommand PassReceiver::runPrePass( const World & world, const Robot & robot) { @@ -136,29 +141,24 @@ ateam_msgs::msg::RobotMotionCommand PassReceiver::runPrePass( } } - easy_move_to_.setTargetPosition(destination); - easy_move_to_.face_point(world.ball.pos); - path_planning::PlannerOptions planner_options; - planner_options.avoid_ball = false; - easy_move_to_.setPlannerOptions(planner_options); - return easy_move_to_.runFrame(robot, world); + RobotCommand command; + command.motion_intent.linear = motion::intents::linear::PositionIntent{destination}; + command.motion_intent.angular = motion::intents::angular::FacingIntent{world.ball.pos}; + command.motion_intent.planner_options.avoid_ball = false; + return command; } -ateam_msgs::msg::RobotMotionCommand PassReceiver::runPass(const World & world, const Robot & robot) +RobotCommand PassReceiver::runPass(const World & world, const Robot & robot) { + RobotCommand command; const ateam_geometry::Ray ball_ray(world.ball.pos, world.ball.vel); const auto destination = ball_ray.supporting_line().projection(robot.pos); - // const auto vel_bump_vec = 0.07 * ateam_geometry::normalize(destination - robot.pos); - easy_move_to_.setTargetPosition( - destination/*, ateam_geometry::Vector(-0.001, 0.0) + vel_bump_vec*/); - easy_move_to_.face_point(world.ball.pos); - easy_move_to_.setMaxDecel(4.0); - path_planning::PlannerOptions planner_options; - planner_options.avoid_ball = false; - easy_move_to_.setPlannerOptions(planner_options); - ateam_msgs::msg::RobotMotionCommand motion_command; - motion_command = easy_move_to_.runFrame(robot, world); - motion_command.dribbler_speed = kDefaultDribblerSpeed * 1.2; + command.motion_intent.linear = motion::intents::linear::PositionIntent{destination}; + command.motion_intent.angular = motion::intents::angular::FacingIntent{world.ball.pos}; + // TODO(barulicm): Set max decel to 4.0 + command.motion_intent.planner_options.avoid_ball = false; + + command.dribbler_speed = kDefaultDribblerSpeed * 1.2; const auto dist_to_ball = ateam_geometry::norm(robot.pos - world.ball.pos); const auto time_to_ball = dist_to_ball / ateam_geometry::norm(world.ball.vel); if (time_to_ball < 0.5) { @@ -167,37 +167,35 @@ ateam_msgs::msg::RobotMotionCommand PassReceiver::runPass(const World & world, c const auto angle_between_vecs = ateam_geometry::ShortestAngleBetween(world.ball.vel, robot_backwards_vec); if(std::abs(angle_between_vecs) < M_PI_2) { - ateam_geometry::Vector robot_vel(motion_command.twist.linear.x, - motion_command.twist.linear.y); - const auto multiplier = robot.breakbeam_ball_detected_filtered ? 0.2 : 0.6; - robot_vel += ateam_geometry::normalize(world.ball.vel) * multiplier; - motion_command.twist.linear.x = robot_vel.x(); - motion_command.twist.linear.y = robot_vel.y(); + command.motion_intent.callback = [](motion::BodyVelocity plan_velocity, + const path_planning::Path &, const Robot & robot, const World & world) -> motion::BodyVelocity { + const auto multiplier = robot.breakbeam_ball_detected_filtered ? 0.2 : 0.6; + plan_velocity.linear += motion::WorldToLocalFrame(ateam_geometry::normalize(world.ball.vel) * multiplier, robot); + return plan_velocity; + }; } } - return motion_command; + return command; } -ateam_msgs::msg::RobotMotionCommand PassReceiver::runPostPass() +RobotCommand PassReceiver::runPostPass() { - ateam_msgs::msg::RobotMotionCommand command; + RobotCommand command; command.dribbler_speed = kDefaultDribblerSpeed; - command.twist.linear.x = 0; - command.twist.linear.y = 0; - command.twist.angular.z = 0; return command; } -ateam_msgs::msg::RobotMotionCommand PassReceiver::runApproachBall( +RobotCommand PassReceiver::runApproachBall( const World & world, const Robot & robot) { const auto ball_to_bot_vector = robot.pos - world.ball.pos; const auto target = world.ball.pos + ateam_geometry::normalize(ball_to_bot_vector) * kRobotDiameter * 1.05; - easy_move_to_.setTargetPosition(target); - easy_move_to_.setMaxVelocity(1.0); - return easy_move_to_.runFrame(robot, world); + RobotCommand command; + command.motion_intent.linear = motion::intents::linear::PositionIntent{target}; + // TODO(barulicm): Set max velocity to 1.0 + return command; } } // namespace ateam_kenobi::skills diff --git a/ateam_kenobi/src/skills/pass_receiver.hpp b/ateam_kenobi/src/skills/pass_receiver.hpp index 118540e99..d20782241 100644 --- a/ateam_kenobi/src/skills/pass_receiver.hpp +++ b/ateam_kenobi/src/skills/pass_receiver.hpp @@ -21,9 +21,8 @@ #ifndef SKILLS__PASS_RECEIVER_HPP_ #define SKILLS__PASS_RECEIVER_HPP_ -#include -#include "core/play_helpers/easy_move_to.hpp" #include "core/stp/skill.hpp" +#include "core/types/robot_command.hpp" #include "core/types/world.hpp" namespace ateam_kenobi::skills @@ -36,7 +35,7 @@ class PassReceiver : public stp::Skill void reset(); - ateam_msgs::msg::RobotMotionCommand runFrame(const World & world, const Robot & robot); + RobotCommand runFrame(const World & world, const Robot & robot); ateam_geometry::Point getAssignmentPoint() { @@ -60,7 +59,6 @@ class PassReceiver : public stp::Skill private: ateam_geometry::Point target_; - play_helpers::EasyMoveTo easy_move_to_; bool done_ = false; bool kick_happened_ = false; double expected_kick_speed_ = 6.5; @@ -72,10 +70,10 @@ class PassReceiver : public stp::Skill bool isBotCloseToTarget(const Robot & robot); bool hasBallBeenKicked(const World & world); - ateam_msgs::msg::RobotMotionCommand runPrePass(const World & world, const Robot & robot); - ateam_msgs::msg::RobotMotionCommand runPass(const World & world, const Robot & robot); - ateam_msgs::msg::RobotMotionCommand runPostPass(); - ateam_msgs::msg::RobotMotionCommand runApproachBall(const World & world, const Robot & robot); + RobotCommand runPrePass(const World & world, const Robot & robot); + RobotCommand runPass(const World & world, const Robot & robot); + RobotCommand runPostPass(); + RobotCommand runApproachBall(const World & world, const Robot & robot); }; } // namespace ateam_kenobi::skills From 7ee450a507111cb62a9039d7e277d55a8476f706 Mon Sep 17 00:00:00 2001 From: Matthew Barulic Date: Tue, 16 Sep 2025 21:37:56 -0400 Subject: [PATCH 13/35] Migrates MultiMoveTo to motion intents --- ateam_kenobi/CMakeLists.txt | 2 +- ateam_kenobi/src/tactics/multi_move_to.cpp | 22 +++++--------------- ateam_kenobi/src/tactics/multi_move_to.hpp | 24 +++++++--------------- 3 files changed, 13 insertions(+), 35 deletions(-) diff --git a/ateam_kenobi/CMakeLists.txt b/ateam_kenobi/CMakeLists.txt index 267865e49..768246d72 100644 --- a/ateam_kenobi/CMakeLists.txt +++ b/ateam_kenobi/CMakeLists.txt @@ -90,7 +90,7 @@ target_sources(kenobi_node_component PRIVATE target_sources(kenobi_node_component PRIVATE # src/tactics/blockers.cpp # src/tactics/defenders.cpp - # src/tactics/multi_move_to.cpp + src/tactics/multi_move_to.cpp # src/tactics/pass_to_segment.cpp # src/tactics/pass.cpp # src/tactics/standard_defense.cpp diff --git a/ateam_kenobi/src/tactics/multi_move_to.cpp b/ateam_kenobi/src/tactics/multi_move_to.cpp index 6a7d1e18d..431fe414d 100644 --- a/ateam_kenobi/src/tactics/multi_move_to.cpp +++ b/ateam_kenobi/src/tactics/multi_move_to.cpp @@ -28,14 +28,6 @@ namespace ateam_kenobi::tactics MultiMoveTo::MultiMoveTo(stp::Options stp_options) : stp::Tactic(stp_options) { - createIndexedChildren(easy_move_tos_, "easy_move_to"); -} - -void MultiMoveTo::Reset() -{ - for (auto & emt : easy_move_tos_) { - emt.reset(); - } } std::vector MultiMoveTo::GetAssignmentPoints() @@ -44,9 +36,8 @@ std::vector MultiMoveTo::GetAssignmentPoints() } void MultiMoveTo::RunFrame( - const World & world, const std::vector> & robots, - std::array, 16> & motion_commands) + std::array, 16> & motion_commands) { for (auto ind = 0ul; ind < robots.size(); ++ind) { const auto & maybe_robot = robots[ind]; @@ -55,18 +46,15 @@ void MultiMoveTo::RunFrame( } const auto & robot = *maybe_robot; const auto & target_position = target_points_[ind]; - - auto & easy_move_to = easy_move_tos_.at(robot.id); + RobotCommand command; + command.motion_intent.linear = motion::intents::linear::PositionIntent{target_position}; + command.motion_intent.angular = angular_intent_; + motion_commands.at(robot.id) = command; auto viz_circle = ateam_geometry::makeCircle(target_position, kRobotRadius); getOverlays().drawCircle( "destination_" + std::to_string( robot.id), viz_circle, "blue", "transparent"); - - easy_move_to.setTargetPosition(target_position); - easy_move_to.face_point(world.ball.pos); - - motion_commands.at(robot.id) = easy_move_to.runFrame(robot, world); } } diff --git a/ateam_kenobi/src/tactics/multi_move_to.hpp b/ateam_kenobi/src/tactics/multi_move_to.hpp index 29248d625..176d9d00d 100644 --- a/ateam_kenobi/src/tactics/multi_move_to.hpp +++ b/ateam_kenobi/src/tactics/multi_move_to.hpp @@ -23,8 +23,7 @@ #include #include "core/stp/tactic.hpp" -#include "core/play_helpers/easy_move_to.hpp" -#include "core/motion/motion_controller.hpp" +#include "core/types/robot_command.hpp" namespace ateam_kenobi::tactics { @@ -34,14 +33,11 @@ class MultiMoveTo : public stp::Tactic public: explicit MultiMoveTo(stp::Options stp_options); - void Reset(); - std::vector GetAssignmentPoints(); void RunFrame( - const World & world, const std::vector> & robots, - std::array, 16> & motion_commands); + std::array, 16> & motion_commands); void SetTargetPoints(const std::vector & targets) { @@ -50,24 +46,22 @@ class MultiMoveTo : public stp::Tactic void SetFaceNone() { - angle_mode_ = motion::AngleMode::no_face; + angular_intent_ = motion::intents::None{}; } void SetFaceTravel() { - angle_mode_ = motion::AngleMode::face_travel; + angular_intent_ = motion::intents::angular::FaceTravelIntent{}; } void SetFaceAbsolue(double angle) { - angle_mode_ = motion::AngleMode::face_absolute; - absolute_angle_reference_ = angle; + angular_intent_ = motion::intents::angular::HeadingIntent{angle}; } void SetFacePoint(const ateam_geometry::Point & point) { - angle_mode_ = motion::AngleMode::face_point; - point_angle_reference_ = point; + angular_intent_ = motion::intents::angular::FacingIntent{point}; } void SetObstacles(const std::vector & obstacles) @@ -81,15 +75,11 @@ class MultiMoveTo : public stp::Tactic } private: - motion::AngleMode angle_mode_ = motion::AngleMode::no_face; - double absolute_angle_reference_ = 0.0; - ateam_geometry::Point point_angle_reference_; + motion::MotionIntent::AngularIntent angular_intent_; std::vector obstacles_; std::vector target_points_; - - std::array easy_move_tos_; }; } // namespace ateam_kenobi::tactics From b8e09557470ef712e39f82e7b0043e4d148f87bc Mon Sep 17 00:00:00 2001 From: Matthew Barulic Date: Tue, 16 Sep 2025 21:41:00 -0400 Subject: [PATCH 14/35] Migrates Defenders to motion intents --- ateam_kenobi/CMakeLists.txt | 2 +- ateam_kenobi/src/tactics/defenders.cpp | 23 +++++++---------------- ateam_kenobi/src/tactics/defenders.hpp | 8 ++------ 3 files changed, 10 insertions(+), 23 deletions(-) diff --git a/ateam_kenobi/CMakeLists.txt b/ateam_kenobi/CMakeLists.txt index 768246d72..6d193944a 100644 --- a/ateam_kenobi/CMakeLists.txt +++ b/ateam_kenobi/CMakeLists.txt @@ -89,7 +89,7 @@ target_sources(kenobi_node_component PRIVATE # target_sources(kenobi_node_component PRIVATE # src/tactics/blockers.cpp - # src/tactics/defenders.cpp + src/tactics/defenders.cpp src/tactics/multi_move_to.cpp # src/tactics/pass_to_segment.cpp # src/tactics/pass.cpp diff --git a/ateam_kenobi/src/tactics/defenders.cpp b/ateam_kenobi/src/tactics/defenders.cpp index f9803ab88..5e0138d10 100644 --- a/ateam_kenobi/src/tactics/defenders.cpp +++ b/ateam_kenobi/src/tactics/defenders.cpp @@ -30,12 +30,6 @@ namespace ateam_kenobi::tactics Defenders::Defenders(stp::Options stp_options) : stp::Tactic(stp_options) { - createIndexedChildren(easy_move_tos_, "EasyMoveTo"); -} - -void Defenders::reset() -{ - std::ranges::for_each(easy_move_tos_, std::mem_fn(&play_helpers::EasyMoveTo::reset)); } std::vector Defenders::getAssignmentPoints(const World & world) @@ -46,7 +40,7 @@ std::vector Defenders::getAssignmentPoints(const World & void Defenders::runFrame( const World & world, const std::vector & robots, - std::array, 16> & motion_commands) + std::array, 16> & motion_commands) { const auto defender_points = getDefenderPoints(world); const auto num_defenders = std::min(defender_points.size(), robots.size()); @@ -54,15 +48,12 @@ void Defenders::runFrame( for (auto i = 0ul; i < num_defenders; ++i) { const auto & robot = robots[i]; const auto & defender_point = defender_points[i]; - auto & emt = easy_move_tos_[robot.id]; - emt.setTargetPosition(defender_point); - // emt.face_point(world.ball.pos); - emt.face_absolute(0.0); - path_planning::PlannerOptions planner_options; - planner_options.avoid_ball = false; - planner_options.footprint_inflation = 0.03; - emt.setPlannerOptions(planner_options); - motion_commands[robot.id] = emt.runFrame(robot, world); + RobotCommand command; + command.motion_intent.linear = motion::intents::linear::PositionIntent{defender_point}; + command.motion_intent.angular = motion::intents::angular::HeadingIntent{0.0}; + command.motion_intent.planner_options.avoid_ball = false; + command.motion_intent.planner_options.footprint_inflation = 0.03; + motion_commands[robot.id] = command; } drawDefenseSegments(world); diff --git a/ateam_kenobi/src/tactics/defenders.hpp b/ateam_kenobi/src/tactics/defenders.hpp index 335adeef6..88d84167b 100644 --- a/ateam_kenobi/src/tactics/defenders.hpp +++ b/ateam_kenobi/src/tactics/defenders.hpp @@ -24,11 +24,10 @@ #include #include #include -#include #include "core/types/world.hpp" #include "core/types/robot.hpp" +#include "core/types/robot_command.hpp" #include "core/stp/tactic.hpp" -#include "core/play_helpers/easy_move_to.hpp" namespace ateam_kenobi::tactics { @@ -38,19 +37,16 @@ class Defenders : public stp::Tactic public: explicit Defenders(stp::Options stp_options); - void reset(); - std::vector getAssignmentPoints(const World & world); void runFrame( const World & world, const std::vector & robots, - std::array, 16> & motion_commands); + std::array, 16> & motion_commands); private: static constexpr double kMargin = 0.05; static constexpr double kDefenseSegmentOffset = kRobotRadius + kMargin; - std::array easy_move_tos_; std::vector getDefenderPoints(const World & world); From 7fe923f9abe72077d72706f882d3022e1d451d6c Mon Sep 17 00:00:00 2001 From: Matthew Barulic Date: Tue, 16 Sep 2025 21:44:23 -0400 Subject: [PATCH 15/35] Migrates Blockers to motion intents --- ateam_kenobi/CMakeLists.txt | 2 +- ateam_kenobi/src/tactics/blockers.cpp | 21 ++++++--------------- ateam_kenobi/src/tactics/blockers.hpp | 9 ++------- 3 files changed, 9 insertions(+), 23 deletions(-) diff --git a/ateam_kenobi/CMakeLists.txt b/ateam_kenobi/CMakeLists.txt index 6d193944a..f9a0525c5 100644 --- a/ateam_kenobi/CMakeLists.txt +++ b/ateam_kenobi/CMakeLists.txt @@ -88,7 +88,7 @@ target_sources(kenobi_node_component PRIVATE # Tactic Sources # target_sources(kenobi_node_component PRIVATE - # src/tactics/blockers.cpp + src/tactics/blockers.cpp src/tactics/defenders.cpp src/tactics/multi_move_to.cpp # src/tactics/pass_to_segment.cpp diff --git a/ateam_kenobi/src/tactics/blockers.cpp b/ateam_kenobi/src/tactics/blockers.cpp index d554c4546..227d07a37 100644 --- a/ateam_kenobi/src/tactics/blockers.cpp +++ b/ateam_kenobi/src/tactics/blockers.cpp @@ -32,14 +32,6 @@ namespace ateam_kenobi::tactics Blockers::Blockers(stp::Options stp_options) : stp::Tactic(stp_options) { - createIndexedChildren(easy_move_tos_, "EasyMoveTo"); -} - -void Blockers::reset() -{ - for (auto & move_to : easy_move_tos_) { - move_to.reset(); - } } std::vector Blockers::getAssignmentPoints(const World & world) @@ -56,7 +48,7 @@ std::vector Blockers::getAssignmentPoints(const World & w return positions; } -std::vector Blockers::runFrame( +std::vector Blockers::runFrame( const World & world, const std::vector & robots, nlohmann::json * play_info) { @@ -71,16 +63,15 @@ std::vector Blockers::runFrame( const auto num_blockers = std::min(max_blocker_count_, robots.size()); positions.erase(positions.begin() + num_blockers, positions.end()); - std::vector motion_commands; + std::vector motion_commands; for (auto robot_index = 0ul; robot_index < robots.size(); ++robot_index) { const auto & robot = robots[robot_index]; const auto & position = positions[robot_index]; - const auto robot_id = robot.id; - auto & move_to = easy_move_tos_[robot_id]; - move_to.setTargetPosition(position); - move_to.face_point(world.ball.pos); - motion_commands.push_back(move_to.runFrame(robot, world)); + RobotCommand command; + command.motion_intent.linear = motion::intents::linear::PositionIntent{position}; + command.motion_intent.angular = motion::intents::angular::FacingIntent{world.ball.pos}; + motion_commands.push_back(command); if (play_info) { (*play_info)["Blockers"][std::to_string(robot.id)]["Blocking"] = blockable_robots[robot_index].id; diff --git a/ateam_kenobi/src/tactics/blockers.hpp b/ateam_kenobi/src/tactics/blockers.hpp index 6ad3c99a2..1781ad5e7 100644 --- a/ateam_kenobi/src/tactics/blockers.hpp +++ b/ateam_kenobi/src/tactics/blockers.hpp @@ -23,11 +23,10 @@ #define TACTICS__BLOCKERS_HPP_ #include -#include #include #include "core/stp/tactic.hpp" +#include "core/types/robot_command.hpp" #include "core/types/world.hpp" -#include "core/play_helpers/easy_move_to.hpp" namespace ateam_kenobi::tactics { @@ -37,11 +36,9 @@ class Blockers : public stp::Tactic public: explicit Blockers(stp::Options stp_options); - void reset(); - std::vector getAssignmentPoints(const World & world); - std::vector runFrame( + std::vector runFrame( const World & world, const std::vector & robots, nlohmann::json * play_info = nullptr); @@ -58,8 +55,6 @@ class Blockers : public stp::Tactic private: std::size_t max_blocker_count_ = 2; - std::array easy_move_tos_; - std::vector getRankedBlockableRobots(const World & world); ateam_geometry::Point getBlockingPosition(const World & world, const Robot & blockee); From c2042667b7a9d5ba85b911866dd3d8cc5db2ba7c Mon Sep 17 00:00:00 2001 From: Matthew Barulic Date: Tue, 16 Sep 2025 21:45:57 -0400 Subject: [PATCH 16/35] Migrates StandardDefense to motion intents --- ateam_kenobi/CMakeLists.txt | 2 +- ateam_kenobi/src/tactics/standard_defense.cpp | 3 +-- ateam_kenobi/src/tactics/standard_defense.hpp | 4 ++-- 3 files changed, 4 insertions(+), 5 deletions(-) diff --git a/ateam_kenobi/CMakeLists.txt b/ateam_kenobi/CMakeLists.txt index f9a0525c5..56ecba03a 100644 --- a/ateam_kenobi/CMakeLists.txt +++ b/ateam_kenobi/CMakeLists.txt @@ -93,7 +93,7 @@ target_sources(kenobi_node_component PRIVATE src/tactics/multi_move_to.cpp # src/tactics/pass_to_segment.cpp # src/tactics/pass.cpp - # src/tactics/standard_defense.cpp + src/tactics/standard_defense.cpp ) # diff --git a/ateam_kenobi/src/tactics/standard_defense.cpp b/ateam_kenobi/src/tactics/standard_defense.cpp index fc1ab0cb1..5f158dd0b 100644 --- a/ateam_kenobi/src/tactics/standard_defense.cpp +++ b/ateam_kenobi/src/tactics/standard_defense.cpp @@ -33,7 +33,6 @@ StandardDefense::StandardDefense(stp::Options stp_options) void StandardDefense::reset() { goalie_.reset(); - defenders_.reset(); } std::vector StandardDefense::getAssignmentPoints(const World & world) @@ -44,7 +43,7 @@ std::vector StandardDefense::getAssignmentPoints(const Wo void StandardDefense::runFrame( const World & world, const std::vector & defender_bots, - std::array, 16> & motion_commands) + std::array, 16> & motion_commands) { goalie_.runFrame(world, motion_commands); ForwardPlayInfo(goalie_); diff --git a/ateam_kenobi/src/tactics/standard_defense.hpp b/ateam_kenobi/src/tactics/standard_defense.hpp index 6e549d734..0e614387e 100644 --- a/ateam_kenobi/src/tactics/standard_defense.hpp +++ b/ateam_kenobi/src/tactics/standard_defense.hpp @@ -22,9 +22,9 @@ #define TACTICS__STANDARD_DEFENSE_HPP_ #include -#include #include "core/types/world.hpp" #include "core/types/robot.hpp" +#include "core/types/robot_command.hpp" #include "core/stp/tactic.hpp" #include "defenders.hpp" #include "skills/goalie.hpp" @@ -50,7 +50,7 @@ class StandardDefense : public stp::Tactic void runFrame( const World & world, const std::vector & defender_bots, - std::array, 16> & motion_commands); + std::array, 16> & motion_commands); private: skills::Goalie goalie_; From 2a51dda68a0329bb5435dedb46f5148f6fa93db1 Mon Sep 17 00:00:00 2001 From: Matthew Barulic Date: Tue, 16 Sep 2025 21:47:22 -0400 Subject: [PATCH 17/35] Migrates Pass to motion intents --- ateam_kenobi/CMakeLists.txt | 2 +- ateam_kenobi/src/tactics/pass.cpp | 4 ++-- ateam_kenobi/src/tactics/pass.hpp | 5 +++-- 3 files changed, 6 insertions(+), 5 deletions(-) diff --git a/ateam_kenobi/CMakeLists.txt b/ateam_kenobi/CMakeLists.txt index 56ecba03a..0ccd35484 100644 --- a/ateam_kenobi/CMakeLists.txt +++ b/ateam_kenobi/CMakeLists.txt @@ -92,7 +92,7 @@ target_sources(kenobi_node_component PRIVATE src/tactics/defenders.cpp src/tactics/multi_move_to.cpp # src/tactics/pass_to_segment.cpp - # src/tactics/pass.cpp + src/tactics/pass.cpp src/tactics/standard_defense.cpp ) diff --git a/ateam_kenobi/src/tactics/pass.cpp b/ateam_kenobi/src/tactics/pass.cpp index 966983a2b..462559fb5 100644 --- a/ateam_kenobi/src/tactics/pass.cpp +++ b/ateam_kenobi/src/tactics/pass.cpp @@ -56,8 +56,8 @@ ateam_geometry::Point Pass::getReceiverAssignmentPoint() void Pass::runFrame( const World & world, const Robot & kicker_bot, const Robot & receiver_bot, - ateam_msgs::msg::RobotMotionCommand & kicker_command, - ateam_msgs::msg::RobotMotionCommand & receiver_command) + RobotCommand & kicker_command, + RobotCommand & receiver_command) { kicker_id_ = kicker_bot.id; diff --git a/ateam_kenobi/src/tactics/pass.hpp b/ateam_kenobi/src/tactics/pass.hpp index 83d474679..57a99a070 100644 --- a/ateam_kenobi/src/tactics/pass.hpp +++ b/ateam_kenobi/src/tactics/pass.hpp @@ -24,6 +24,7 @@ #include #include "core/stp/tactic.hpp" +#include "core/types/robot_command.hpp" #include "core/types/world.hpp" #include "skills/universal_kick.hpp" #include "skills/pass_receiver.hpp" @@ -44,8 +45,8 @@ class Pass : public stp::Tactic void runFrame( const World & world, const Robot & kicker_bot, const Robot & receiver_bot, - ateam_msgs::msg::RobotMotionCommand & kicker_command, - ateam_msgs::msg::RobotMotionCommand & receiver_command); + RobotCommand & kicker_command, + RobotCommand & receiver_command); bool isDone(); From af6aaf189484c4492d724d37e5e91b444aa7187f Mon Sep 17 00:00:00 2001 From: Matthew Barulic Date: Tue, 16 Sep 2025 21:48:43 -0400 Subject: [PATCH 18/35] Migrates PassToSegment to motion intents --- ateam_kenobi/CMakeLists.txt | 2 +- ateam_kenobi/src/tactics/pass_to_segment.cpp | 4 ++-- ateam_kenobi/src/tactics/pass_to_segment.hpp | 5 +++-- 3 files changed, 6 insertions(+), 5 deletions(-) diff --git a/ateam_kenobi/CMakeLists.txt b/ateam_kenobi/CMakeLists.txt index 0ccd35484..d3d810cff 100644 --- a/ateam_kenobi/CMakeLists.txt +++ b/ateam_kenobi/CMakeLists.txt @@ -91,7 +91,7 @@ target_sources(kenobi_node_component PRIVATE src/tactics/blockers.cpp src/tactics/defenders.cpp src/tactics/multi_move_to.cpp - # src/tactics/pass_to_segment.cpp + src/tactics/pass_to_segment.cpp src/tactics/pass.cpp src/tactics/standard_defense.cpp ) diff --git a/ateam_kenobi/src/tactics/pass_to_segment.cpp b/ateam_kenobi/src/tactics/pass_to_segment.cpp index eb0345838..158da1f18 100644 --- a/ateam_kenobi/src/tactics/pass_to_segment.cpp +++ b/ateam_kenobi/src/tactics/pass_to_segment.cpp @@ -47,8 +47,8 @@ ateam_geometry::Point PassToSegment::getReceiverAssignmentPoint(const World & wo void PassToSegment::runFrame( const World & world, const Robot & kicker_bot, const Robot & receiver_bot, - ateam_msgs::msg::RobotMotionCommand & kicker_command, - ateam_msgs::msg::RobotMotionCommand & receiver_command) + RobotCommand & kicker_command, + RobotCommand & receiver_command) { if (ateam_geometry::norm(world.ball.vel) < 0.01) { pass_tactic_.setTarget(getTargetPointOnSegment(world)); diff --git a/ateam_kenobi/src/tactics/pass_to_segment.hpp b/ateam_kenobi/src/tactics/pass_to_segment.hpp index b4808bd65..8e4cdd9ad 100644 --- a/ateam_kenobi/src/tactics/pass_to_segment.hpp +++ b/ateam_kenobi/src/tactics/pass_to_segment.hpp @@ -23,6 +23,7 @@ #include #include "core/stp/tactic.hpp" +#include "core/types/robot_command.hpp" #include "pass.hpp" namespace ateam_kenobi::tactics @@ -41,8 +42,8 @@ class PassToSegment : public stp::Tactic void runFrame( const World & world, const Robot & kicker_bot, const Robot & receiver_bot, - ateam_msgs::msg::RobotMotionCommand & kicker_command, - ateam_msgs::msg::RobotMotionCommand & receiver_command); + RobotCommand & kicker_command, + RobotCommand & receiver_command); bool isDone() { From b05d947593367b3c20b394118b31113f676672ad Mon Sep 17 00:00:00 2001 From: Matthew Barulic Date: Tue, 16 Sep 2025 22:53:55 -0400 Subject: [PATCH 19/35] Migrates root plays to motion intents --- ateam_kenobi/CMakeLists.txt | 20 +- ateam_kenobi/src/plays/basic_122.cpp | 12 +- ateam_kenobi/src/plays/basic_122.hpp | 6 +- .../src/plays/defenders_only_play.cpp | 4 +- .../src/plays/defenders_only_play.hpp | 2 +- ateam_kenobi/src/plays/defense_play.cpp | 5 +- ateam_kenobi/src/plays/defense_play.hpp | 2 +- ateam_kenobi/src/plays/extract_play.cpp | 8 +- ateam_kenobi/src/plays/extract_play.hpp | 2 +- ateam_kenobi/src/plays/kick_on_goal_play.cpp | 8 +- ateam_kenobi/src/plays/kick_on_goal_play.hpp | 2 +- .../src/plays/our_ball_placement_play.cpp | 242 +++++++++--------- .../src/plays/our_ball_placement_play.hpp | 18 +- ateam_kenobi/src/plays/our_penalty_play.cpp | 49 ++-- ateam_kenobi/src/plays/our_penalty_play.hpp | 8 +- .../src/plays/their_ball_placement_play.cpp | 125 +++++---- .../src/plays/their_ball_placement_play.hpp | 12 +- ateam_kenobi/src/plays/their_penalty_play.cpp | 41 +-- ateam_kenobi/src/plays/their_penalty_play.hpp | 7 +- ateam_kenobi/src/plays/wall_play.cpp | 31 +-- ateam_kenobi/src/plays/wall_play.hpp | 2 + ateam_kenobi/src/tactics/multi_move_to.cpp | 20 ++ ateam_kenobi/src/tactics/multi_move_to.hpp | 4 + 23 files changed, 334 insertions(+), 296 deletions(-) diff --git a/ateam_kenobi/CMakeLists.txt b/ateam_kenobi/CMakeLists.txt index d3d810cff..c2a520c34 100644 --- a/ateam_kenobi/CMakeLists.txt +++ b/ateam_kenobi/CMakeLists.txt @@ -70,18 +70,18 @@ target_sources(kenobi_node_component PRIVATE # src/plays/test_plays/test_play.cpp # src/plays/test_plays/triangle_pass_play.cpp # src/plays/test_plays/waypoints_play.cpp - # src/plays/util_plays/corner_lineup_play.cpp + src/plays/util_plays/corner_lineup_play.cpp src/plays/halt_play.cpp - # src/plays/kick_on_goal_play.cpp - # src/plays/our_ball_placement_play.cpp - # src/plays/their_ball_placement_play.cpp + src/plays/kick_on_goal_play.cpp + src/plays/our_ball_placement_play.cpp + src/plays/their_ball_placement_play.cpp src/plays/wall_play.cpp - # src/plays/basic_122.cpp - # src/plays/defenders_only_play.cpp - # src/plays/our_penalty_play.cpp - # src/plays/their_penalty_play.cpp - # src/plays/defense_play.cpp - # src/plays/extract_play.cpp + src/plays/basic_122.cpp + src/plays/defenders_only_play.cpp + src/plays/our_penalty_play.cpp + src/plays/their_penalty_play.cpp + src/plays/defense_play.cpp + src/plays/extract_play.cpp ) # diff --git a/ateam_kenobi/src/plays/basic_122.cpp b/ateam_kenobi/src/plays/basic_122.cpp index 93fae679a..443f205aa 100644 --- a/ateam_kenobi/src/plays/basic_122.cpp +++ b/ateam_kenobi/src/plays/basic_122.cpp @@ -56,14 +56,13 @@ stp::PlayScore Basic122::getScore(const World & world) void Basic122::reset() { - blockers_skill_.reset(); defense_.reset(); } -std::array, 16> Basic122::runFrame( +std::array, 16> Basic122::runFrame( const World & world) { - std::array, 16> motion_commands; + std::array, 16> motion_commands; auto available_robots = play_helpers::getAvailableRobots(world); play_helpers::removeGoalie(available_robots, world); @@ -112,7 +111,7 @@ std::array, 16> Basic122::run void Basic122::runStriker( const Robot & striker_bot, const World & world, - ateam_msgs::msg::RobotMotionCommand & motion_command) + RobotCommand & motion_command) { if (striker_skill_.IsDone()) { striker_skill_.Reset(); @@ -126,8 +125,7 @@ void Basic122::runStriker( const auto ball_to_bot_vec = striker_bot.pos - world.ball.pos; const auto vel = ateam_geometry::normalize(ball_to_bot_vec) * 0.25; if (ateam_geometry::norm(ball_to_bot_vec) < kRobotDiameter) { - motion_command.twist.linear.x = vel.x(); - motion_command.twist.linear.y = vel.y(); + motion_command.motion_intent.linear = motion::intents::linear::VelocityIntent{vel}; } return; } @@ -161,7 +159,7 @@ void Basic122::runStriker( void Basic122::runBlockers( const std::vector & blocker_bots, const World & world, - std::array, + std::array, 16> & motion_commands) { const auto skill_commands = blockers_skill_.runFrame(world, blocker_bots); diff --git a/ateam_kenobi/src/plays/basic_122.hpp b/ateam_kenobi/src/plays/basic_122.hpp index b0bd38aaf..10ad1df9c 100644 --- a/ateam_kenobi/src/plays/basic_122.hpp +++ b/ateam_kenobi/src/plays/basic_122.hpp @@ -42,7 +42,7 @@ class Basic122 : public stp::Play void reset() override; - std::array, + std::array, 16> runFrame(const World & world) override; private: @@ -52,11 +52,11 @@ class Basic122 : public stp::Play void runStriker( const Robot & striker_bot, const World & world, - ateam_msgs::msg::RobotMotionCommand & motion_command); + RobotCommand & motion_command); void runBlockers( const std::vector & blocker_bots, const World & world, - std::array, + std::array, 16> & motion_commands); bool doTheyHavePossession(const World & world); diff --git a/ateam_kenobi/src/plays/defenders_only_play.cpp b/ateam_kenobi/src/plays/defenders_only_play.cpp index 9bdf8ec03..d135b433d 100644 --- a/ateam_kenobi/src/plays/defenders_only_play.cpp +++ b/ateam_kenobi/src/plays/defenders_only_play.cpp @@ -52,10 +52,10 @@ void DefendersOnlyPlay::reset() defense_tactic_.reset(); } -std::array, 16> DefendersOnlyPlay::runFrame( +std::array, 16> DefendersOnlyPlay::runFrame( const World & world) { - std::array, 16> motion_commands; + std::array, 16> motion_commands; auto available_robots = play_helpers::getAvailableRobots(world); play_helpers::removeGoalie(available_robots, world); diff --git a/ateam_kenobi/src/plays/defenders_only_play.hpp b/ateam_kenobi/src/plays/defenders_only_play.hpp index 14281f4f9..d36bdf26c 100644 --- a/ateam_kenobi/src/plays/defenders_only_play.hpp +++ b/ateam_kenobi/src/plays/defenders_only_play.hpp @@ -38,7 +38,7 @@ class DefendersOnlyPlay : public stp::Play void reset() override; - std::array, 16> runFrame( + std::array, 16> runFrame( const World & world) override; private: diff --git a/ateam_kenobi/src/plays/defense_play.cpp b/ateam_kenobi/src/plays/defense_play.cpp index 1197c6c26..d2a4b88cd 100644 --- a/ateam_kenobi/src/plays/defense_play.cpp +++ b/ateam_kenobi/src/plays/defense_play.cpp @@ -56,13 +56,12 @@ stp::PlayScore DefensePlay::getScore(const World & world) void DefensePlay::reset() { defense_tactic_.reset(); - blockers_.reset(); } -std::array, 16> DefensePlay::runFrame( +std::array, 16> DefensePlay::runFrame( const World & world) { - std::array, 16> motion_commands; + std::array, 16> motion_commands; auto available_robots = play_helpers::getAvailableRobots(world); play_helpers::removeGoalie(available_robots, world); diff --git a/ateam_kenobi/src/plays/defense_play.hpp b/ateam_kenobi/src/plays/defense_play.hpp index 02e4304a0..17226b118 100644 --- a/ateam_kenobi/src/plays/defense_play.hpp +++ b/ateam_kenobi/src/plays/defense_play.hpp @@ -39,7 +39,7 @@ class DefensePlay : public stp::Play void reset() override; - std::array, 16> runFrame( + std::array, 16> runFrame( const World & world) override; private: diff --git a/ateam_kenobi/src/plays/extract_play.cpp b/ateam_kenobi/src/plays/extract_play.cpp index 7590a1120..334a55d1f 100644 --- a/ateam_kenobi/src/plays/extract_play.cpp +++ b/ateam_kenobi/src/plays/extract_play.cpp @@ -59,10 +59,10 @@ void ExtractPlay::reset() extract_.Reset(); } -std::array, 16> ExtractPlay::runFrame( +std::array, 16> ExtractPlay::runFrame( const World & world) { - std::array, 16> motion_commands; + std::array, 16> motion_commands; auto available_robots = play_helpers::getAvailableRobots(world); play_helpers::removeGoalie(available_robots, world); @@ -103,12 +103,12 @@ std::array, 16> ExtractPlay:: if (enough_bots_for_idlers) { assignments.RunPositionIfAssigned( "lane_idler_a", [this, &world, &motion_commands](const Robot & robot) { - motion_commands[robot.id] = lane_idler_a_.RunFrame(world, robot); + motion_commands[robot.id] = lane_idler_a_.RunFrame(world); }); assignments.RunPositionIfAssigned( "lane_idler_b", [this, &world, &motion_commands](const Robot & robot) { - motion_commands[robot.id] = lane_idler_b_.RunFrame(world, robot); + motion_commands[robot.id] = lane_idler_b_.RunFrame(world); }); } diff --git a/ateam_kenobi/src/plays/extract_play.hpp b/ateam_kenobi/src/plays/extract_play.hpp index 7ab59edb8..e6bee033b 100644 --- a/ateam_kenobi/src/plays/extract_play.hpp +++ b/ateam_kenobi/src/plays/extract_play.hpp @@ -40,7 +40,7 @@ class ExtractPlay : public stp::Play void reset() override; - std::array, 16> runFrame( + std::array, 16> runFrame( const World & world) override; private: diff --git a/ateam_kenobi/src/plays/kick_on_goal_play.cpp b/ateam_kenobi/src/plays/kick_on_goal_play.cpp index 141759cc6..0b70cd993 100644 --- a/ateam_kenobi/src/plays/kick_on_goal_play.cpp +++ b/ateam_kenobi/src/plays/kick_on_goal_play.cpp @@ -70,10 +70,10 @@ void KickOnGoalPlay::reset() striker_.Reset(); } -std::array, 16> KickOnGoalPlay::runFrame( +std::array, 16> KickOnGoalPlay::runFrame( const World & world) { - std::array, 16> motion_commands; + std::array, 16> motion_commands; auto available_robots = play_helpers::getAvailableRobots(world); play_helpers::removeGoalie(available_robots, world); @@ -134,12 +134,12 @@ std::array, 16> KickOnGoalPla if (enough_bots_for_idlers) { assignments.RunPositionIfAssigned( "lane_idler_a", [this, &world, &motion_commands](const Robot & robot) { - motion_commands[robot.id] = lane_idler_a_.RunFrame(world, robot); + motion_commands[robot.id] = lane_idler_a_.RunFrame(world); }); assignments.RunPositionIfAssigned( "lane_idler_b", [this, &world, &motion_commands](const Robot & robot) { - motion_commands[robot.id] = lane_idler_b_.RunFrame(world, robot); + motion_commands[robot.id] = lane_idler_b_.RunFrame(world); }); } diff --git a/ateam_kenobi/src/plays/kick_on_goal_play.hpp b/ateam_kenobi/src/plays/kick_on_goal_play.hpp index f3a8337a4..5be9e4855 100644 --- a/ateam_kenobi/src/plays/kick_on_goal_play.hpp +++ b/ateam_kenobi/src/plays/kick_on_goal_play.hpp @@ -40,7 +40,7 @@ class KickOnGoalPlay : public stp::Play void reset() override; - std::array, 16> runFrame( + std::array, 16> runFrame( const World & world) override; private: diff --git a/ateam_kenobi/src/plays/our_ball_placement_play.cpp b/ateam_kenobi/src/plays/our_ball_placement_play.cpp index db0630159..b6d731e6d 100644 --- a/ateam_kenobi/src/plays/our_ball_placement_play.cpp +++ b/ateam_kenobi/src/plays/our_ball_placement_play.cpp @@ -31,9 +31,9 @@ namespace ateam_kenobi::plays OurBallPlacementPlay::OurBallPlacementPlay(stp::Options stp_options) : stp::Play(kPlayName, stp_options), pass_tactic_(createChild("pass")), - dribble_(createChild("dribble")) + dribble_(createChild("dribble")), + multi_move_to_(createChild("multi_move_to")) { - createIndexedChildren(easy_move_tos_, "EasyMoveTo"); } stp::PlayScore OurBallPlacementPlay::getScore(const World & world) @@ -50,15 +50,12 @@ void OurBallPlacementPlay::reset() state_ = State::Placing; pass_tactic_.reset(); dribble_.reset(); - for (auto & emt : easy_move_tos_) { - emt.reset(); - } } -std::array, 16> OurBallPlacementPlay::runFrame( +std::array, 16> OurBallPlacementPlay::runFrame( const World & world) { - std::array, 16> maybe_motion_commands; + std::array, 16> maybe_motion_commands; auto available_robots = play_helpers::getAvailableRobots(world); @@ -76,7 +73,6 @@ std::array, 16> OurBallPlacem DrawKeepoutArea(world.ball.pos, placement_point_); const auto point_to_ball = world.ball.pos - placement_point_; - const auto angle = std::atan2(point_to_ball.y(), point_to_ball.x()); const auto ball_dist = ateam_geometry::norm(point_to_ball); const auto ball_speed = ateam_geometry::norm(world.ball.vel); @@ -156,61 +152,46 @@ std::array, 16> OurBallPlacem getPlayInfo()["State"] = "Done"; } - for (auto ind = 0ul; ind < available_robots.size(); ++ind) { - const auto & robot = available_robots[ind]; - auto & motion_command = maybe_motion_commands[robot.id]; - - if (!motion_command) { - auto & emt = easy_move_tos_[robot.id]; + available_robots.erase( + std::remove_if( + available_robots.begin(), available_robots.end(), + [&maybe_motion_commands](const Robot & robot) { + return maybe_motion_commands[robot.id].has_value(); + }), + available_robots.end()); - const auto placement_segment = ateam_geometry::Segment(placement_point_, world.ball.pos); - const auto nearest_point = - ateam_geometry::nearestPointOnSegment(placement_segment, robot.pos); + const auto partition_iter = std::partition(available_robots.begin(), available_robots.end(), + [this, &world](const Robot & robot) { + return shouldRobotMove(world, placement_point_, robot); + }); - ateam_geometry::Point target_position = robot.pos; - if (ateam_geometry::norm(robot.pos - nearest_point) < 0.6 + kRobotRadius) { - target_position = nearest_point + - 0.7 * ateam_geometry::Vector(std::cos(angle + M_PI / 2), std::sin(angle + M_PI / 2)); + const std::vector robots_to_move(available_robots.begin(), partition_iter); + const std::vector robots_to_stay(partition_iter, available_robots.end()); - const auto alternate_position = nearest_point + - 0.7 * ateam_geometry::Vector(std::cos(angle - M_PI / 2), std::sin(angle - M_PI / 2)); + for(const auto & robot : robots_to_move) { + getPlayInfo()["Robots"][std::to_string(robot.id)] = "MOVING"; + } + for(const auto & robot : robots_to_stay) { + getPlayInfo()["Robots"][std::to_string(robot.id)] = "-"; + } - const auto offset = kRobotRadius * 0.95; - const auto x_bound = (world.field.field_length / 2.0) + world.field.boundary_width - offset; - const auto y_bound = (world.field.field_width / 2.0) + world.field.boundary_width - offset; - ateam_geometry::Rectangle pathable_region(ateam_geometry::Point(-x_bound, -y_bound), - ateam_geometry::Point(x_bound, y_bound)); + std::vector target_points; + std::transform(robots_to_move.begin(), robots_to_move.end(), + std::back_inserter(target_points), + [this, &world](const Robot & robot) { + return getTargetPoint(world, placement_point_, robot); + }); - if (!CGAL::do_intersect(target_position, pathable_region)) { - target_position = alternate_position; - } else if (!CGAL::do_intersect(alternate_position, pathable_region)) { - // Stick with target_position - } else { - // Use the shorter path - if (ateam_geometry::norm(target_position - robot.pos) > - ateam_geometry::norm(alternate_position - robot.pos)) - { - target_position = alternate_position; - } - } - - emt.setTargetPosition(target_position); - emt.face_point(world.ball.pos); - maybe_motion_commands[robot.id] = emt.runFrame(robot, world); - - getPlayInfo()["Robots"][std::to_string(robot.id)] = "MOVING"; - } else { - getPlayInfo()["Robots"][std::to_string(robot.id)] = "-"; - } - } - } + multi_move_to_.SetTargetPoints(target_points); + multi_move_to_.SetFacePoint(world.ball.pos); + multi_move_to_.RunFrame(robots_to_move, maybe_motion_commands); return maybe_motion_commands; } void OurBallPlacementPlay::runPassing( const std::vector & available_robots, const World & world, - std::array, + std::array, 16> & motion_commands) { if(available_robots.size() < 2) { @@ -236,9 +217,9 @@ void OurBallPlacementPlay::runPassing( (kRobotRadius * ateam_geometry::normalize(ball_to_placement))); auto & kicker_command = - *(motion_commands[kicker_robot.id] = ateam_msgs::msg::RobotMotionCommand{}); + *(motion_commands[kicker_robot.id] = RobotCommand{}); auto & receiver_command = - *(motion_commands[receiver_robot.id] = ateam_msgs::msg::RobotMotionCommand{}); + *(motion_commands[receiver_robot.id] = RobotCommand{}); pass_tactic_.runFrame(world, kicker_robot, receiver_robot, kicker_command, receiver_command); @@ -247,7 +228,7 @@ void OurBallPlacementPlay::runPassing( void OurBallPlacementPlay::runExtracting( const std::vector & available_robots, const World & world, - std::array, + std::array, 16> & motion_commands) { const auto approach_point = approach_point_; @@ -261,8 +242,6 @@ void OurBallPlacementPlay::runExtracting( getPlayInfo()["Assignments"]["Extractor"] = extract_robot.id; - auto & emt = easy_move_tos_[extract_robot.id]; - const auto robot_to_ball = (world.ball.pos - extract_robot.pos); const auto ball_to_approach = approach_point_ - world.ball.pos; @@ -280,7 +259,7 @@ void OurBallPlacementPlay::runExtracting( const bool robot_already_in_position = robot_near_ball && robot_facing_ball && abs(robot_approach_angle_offset) < 0.3; - auto motion_command = ateam_msgs::msg::RobotMotionCommand(); + auto motion_command = RobotCommand(); if (extract_robot.breakbeam_ball_detected_filtered) { getPlayInfo()["ExtractState"] = "extracting ball"; @@ -297,71 +276,60 @@ void OurBallPlacementPlay::runExtracting( extract_robot.theta ); - motion_command.twist.angular.z = std::copysign(0.8, direction); + motion_command.motion_intent.angular = + motion::intents::angular::VelocityIntent{std::copysign(0.8, direction)}; } else { state_ = State::Placing; } } else { - MotionOptions motion_options; - motion_options.completion_threshold = 0; - emt.setMotionOptions(motion_options); - path_planning::PlannerOptions planner_options = emt.getPlannerOptions(); - planner_options.avoid_ball = false; - planner_options.footprint_inflation = -0.9 * kRobotRadius; - emt.setPlannerOptions(planner_options); - - emt.setTargetPosition(approach_point_); - emt.setMaxVelocity(0.2); - emt.setMaxAccel(1.0); - emt.setMaxDecel(1.0); - if (world.ball.visible) { - emt.face_point(world.ball.pos); + motion_command.motion_intent.motion_options.completion_threshold = 0; + motion_command.motion_intent.planner_options.avoid_ball = false; + motion_command.motion_intent.planner_options.footprint_inflation = -0.9 * kRobotRadius; + motion_command.motion_intent.linear = + motion::intents::linear::PositionIntent{approach_point_}; + // TODO(barulicm): set max velocity to 0.2 + // TODO(barulicm): set max accel to 1.0 + // TODO(barulicm): set max decel to 1.0 - // If the ball is occluded we sometimes drive past its previous position and try to turn - // around so its better to just keep facing the same direction if we lose track of it + if (world.ball.visible) { + motion_command.motion_intent.angular = + motion::intents::angular::FacingIntent{world.ball.pos}; } else { - emt.face_absolute(extract_robot.theta); + // If the ball is occluded we sometimes drive past its previous position and try to turn + // around so its better to just keep facing the same direction if we lose track of it + motion_command.motion_intent.angular = + motion::intents::angular::HeadingIntent{extract_robot.theta}; } - - motion_command = emt.runFrame(extract_robot, world); - // motion_command.twist_frame = ateam_msgs::msg::RobotMotionCommand::FRAME_BODY; - // motion_command.twist.linear.x = -0.1; - // motion_command.twist.linear.y = 0.0; } } else if (robot_already_in_position || robot_near_approach_point) { getPlayInfo()["ExtractState"] = "capturing ball"; - MotionOptions motion_options; - motion_options.completion_threshold = 0; - emt.setMotionOptions(motion_options); - path_planning::PlannerOptions planner_options = emt.getPlannerOptions(); - planner_options.avoid_ball = false; - planner_options.footprint_inflation = -0.9 * kRobotRadius; - emt.setPlannerOptions(planner_options); - - emt.setMaxVelocity(0.35); - emt.setMaxAccel(2.0); - emt.setMaxDecel(2.0); - if (world.ball.visible) { - emt.face_point(world.ball.pos); + motion_command.motion_intent.motion_options.completion_threshold = 0; + motion_command.motion_intent.planner_options.avoid_ball = false; + motion_command.motion_intent.planner_options.footprint_inflation = -0.9 * kRobotRadius; - // If the ball is occluded we sometimes drive past its previous position and try to turn around - // so its better to just keep facing the same direction if we lose track of it + // TODO(barulicm): set max velocity to 0.35 + // TODO(barulicm): set max accel to 2.0 + // TODO(barulicm): set max decel to 2.0 + + if (world.ball.visible) { + motion_command.motion_intent.angular = motion::intents::angular::FacingIntent{world.ball.pos}; } else { - emt.face_absolute(extract_robot.theta); + // If the ball is occluded we sometimes drive past its previous position and try to turn around + // so its better to just keep facing the same direction if we lose track of it + motion_command.motion_intent.angular = + motion::intents::angular::HeadingIntent{extract_robot.theta}; } - emt.setTargetPosition(world.ball.pos); - motion_command = emt.runFrame(extract_robot, world); - motion_command.twist_frame = ateam_msgs::msg::RobotMotionCommand::FRAME_BODY; - motion_command.twist.linear.x = 0.35; - motion_command.twist.linear.y = 0.0; + motion_command.motion_intent.linear = + motion::intents::linear::VelocityIntent{ateam_geometry::Vector(0.35, 0), + motion::intents::linear::Frame::Local}; } else { getPlayInfo()["ExtractState"] = "moving to approach point"; - emt.setMaxVelocity(1.5); - emt.setMaxAccel(2.0); - emt.setMaxDecel(2.0); - emt.face_absolute(ateam_geometry::ToHeading(world.ball.pos - approach_point_)); - emt.setTargetPosition(approach_point_); - motion_command = emt.runFrame(extract_robot, world); + // TODO(barulicm): Set max velocity to 1.5 + // TODO(barulicm): Set max accel to 2.0 + // TODO(barulicm): Set max decel to 2.0 + + motion_command.motion_intent.linear = motion::intents::linear::PositionIntent{approach_point_}; + motion_command.motion_intent.angular = motion::intents::angular::HeadingIntent{ateam_geometry::ToHeading(world.ball.pos - approach_point_)}; } const bool should_dribble = extract_robot.breakbeam_ball_detected_filtered || @@ -375,7 +343,7 @@ void OurBallPlacementPlay::runExtracting( void OurBallPlacementPlay::runPlacing( const std::vector & available_robots, const World & world, - std::array, + std::array, 16> & motion_commands) { auto byDistToBall = [&world](const Robot & lhs, const Robot & rhs) { @@ -395,7 +363,7 @@ void OurBallPlacementPlay::runPlacing( void OurBallPlacementPlay::runDone( const std::vector & available_robots, const World & world, - std::array, + std::array, 16> & motion_commands) { // TODO(chachmu): Might need to add a delay for this so the dribbler slows down @@ -412,13 +380,12 @@ void OurBallPlacementPlay::runDone( const auto ball_to_robot = place_robot.pos - world.ball.pos; - auto & emt = easy_move_tos_[place_robot.id]; - + RobotCommand command; // TODO(chachmu): check next ref command to know if we need 0.5 for force start or // 0.05 for our free kick - emt.setTargetPosition(world.ball.pos + (0.5 * ateam_geometry::normalize(ball_to_robot))); - emt.face_point(world.ball.pos); - auto command = emt.runFrame(place_robot, world); + command.motion_intent.linear = motion::intents::linear::PositionIntent{ + world.ball.pos + (0.5 * ateam_geometry::normalize(ball_to_robot))}; + command.motion_intent.angular = motion::intents::angular::FacingIntent{world.ball.pos}; motion_commands[place_robot.id] = command; } @@ -526,5 +493,50 @@ std::optional OurBallPlacementPlay::calculateObstacleOff return std::nullopt; } +bool OurBallPlacementPlay::shouldRobotMove( + const World & world, + const ateam_geometry::Point & placement_point, const Robot & robot) +{ + const auto placement_segment = ateam_geometry::Segment(placement_point, world.ball.pos); + const auto nearest_point = ateam_geometry::nearestPointOnSegment(placement_segment, robot.pos); + return ateam_geometry::norm(robot.pos - nearest_point) < 0.6 + kRobotRadius; +} + +ateam_geometry::Point OurBallPlacementPlay::getTargetPoint( + const World & world, + const ateam_geometry::Point & placement_point, const Robot & robot) +{ + const auto placement_segment = ateam_geometry::Segment(placement_point, world.ball.pos); + const auto nearest_point = ateam_geometry::nearestPointOnSegment(placement_segment, robot.pos); + + const auto point_to_ball = world.ball.pos - placement_point; + const auto angle = std::atan2(point_to_ball.y(), point_to_ball.x()); + + auto target_position = nearest_point + + 0.7 * ateam_geometry::Vector(std::cos(angle + M_PI / 2), std::sin(angle + M_PI / 2)); + + const auto alternate_position = nearest_point + + 0.7 * ateam_geometry::Vector(std::cos(angle - M_PI / 2), std::sin(angle - M_PI / 2)); + + const auto offset = kRobotRadius * 0.95; + const auto x_bound = (world.field.field_length / 2.0) + world.field.boundary_width - offset; + const auto y_bound = (world.field.field_width / 2.0) + world.field.boundary_width - offset; + ateam_geometry::Rectangle pathable_region(ateam_geometry::Point(-x_bound, -y_bound), + ateam_geometry::Point(x_bound, y_bound)); + + if (!CGAL::do_intersect(target_position, pathable_region)) { + target_position = alternate_position; + } else if (!CGAL::do_intersect(alternate_position, pathable_region)) { + // Stick with target_position + } else { + // Use the shorter path + if (ateam_geometry::norm(target_position - robot.pos) > + ateam_geometry::norm(alternate_position - robot.pos)) + { + target_position = alternate_position; + } + } + return target_position; +} } // namespace ateam_kenobi::plays diff --git a/ateam_kenobi/src/plays/our_ball_placement_play.hpp b/ateam_kenobi/src/plays/our_ball_placement_play.hpp index 5e0d113e5..002acd220 100644 --- a/ateam_kenobi/src/plays/our_ball_placement_play.hpp +++ b/ateam_kenobi/src/plays/our_ball_placement_play.hpp @@ -25,7 +25,7 @@ #include "core/stp/play.hpp" #include "tactics/pass.hpp" #include "skills/dribble.hpp" -#include "core/play_helpers/easy_move_to.hpp" +#include "tactics/multi_move_to.hpp" namespace ateam_kenobi::plays { @@ -42,13 +42,13 @@ class OurBallPlacementPlay : public stp::Play void reset() override; - std::array, + std::array, 16> runFrame(const World & world) override; private: tactics::Pass pass_tactic_; skills::Dribble dribble_; - std::array easy_move_tos_; + tactics::MultiMoveTo multi_move_to_; ateam_geometry::Point placement_point_; double approach_radius_ = kRobotRadius + kBallRadius + 0.3; // m @@ -71,27 +71,31 @@ class OurBallPlacementPlay : public stp::Play void runExtracting( const std::vector & available_robots, const World & world, - std::array, + std::array, 16> & motion_commands); void runPassing( const std::vector & available_robots, const World & world, - std::array, + std::array, 16> & motion_commands); void runPlacing( const std::vector & available_robots, const World & world, - std::array, + std::array, 16> & motion_commands); void runDone( const std::vector & available_robots, const World & world, - std::array, + std::array, 16> & motion_commands); void DrawKeepoutArea( const ateam_geometry::Point & ball_pos, const ateam_geometry::Point & placement_point); + + bool shouldRobotMove(const World & world, const ateam_geometry::Point & placement_point, const Robot & robot); + + ateam_geometry::Point getTargetPoint(const World & world, const ateam_geometry::Point & placement_point, const Robot & robot); }; } // namespace ateam_kenobi::plays diff --git a/ateam_kenobi/src/plays/our_penalty_play.cpp b/ateam_kenobi/src/plays/our_penalty_play.cpp index 4d986cbbb..871d18aa8 100644 --- a/ateam_kenobi/src/plays/our_penalty_play.cpp +++ b/ateam_kenobi/src/plays/our_penalty_play.cpp @@ -30,9 +30,9 @@ namespace ateam_kenobi::plays OurPenaltyPlay::OurPenaltyPlay(stp::Options stp_options) : stp::Play(kPlayName, stp_options), goalie_skill_(createChild("goalie")), - kick_skill_(createChild("kick")) + kick_skill_(createChild("kick")), + multi_move_to_(createChild("multi_move_to")) { - createIndexedChildren(move_tos_, "EasyMoveTo"); } stp::PlayScore OurPenaltyPlay::getScore(const World & world) @@ -51,17 +51,10 @@ stp::PlayScore OurPenaltyPlay::getScore(const World & world) return stp::PlayScore::NaN(); } -void OurPenaltyPlay::reset() -{ - for (auto & move_to : move_tos_) { - move_to.reset(); - } -} - -std::array, 16> OurPenaltyPlay::runFrame( +std::array, 16> OurPenaltyPlay::runFrame( const World & world) { - std::array, 16> motion_commands; + std::array, 16> motion_commands; goalie_skill_.runFrame(world, motion_commands); @@ -92,11 +85,11 @@ std::array, 16> OurPenaltyPla getPlayInfo()["State"] = "Preparing"; kick_skill_.SetTargetPoint(chooseKickTarget(world)); const auto destination = kick_skill_.GetAssignmentPoint(world); - auto & move_to = move_tos_[kicking_robot.id]; - move_to.setTargetPosition(destination); - move_to.face_point(world.ball.pos); - move_to.setMaxVelocity(1.5); - motion_commands[kicking_robot.id] = move_to.runFrame(kicking_robot, world); + RobotCommand command; + command.motion_intent.linear = motion::intents::linear::PositionIntent{destination}; + command.motion_intent.angular = motion::intents::angular::FacingIntent{world.ball.pos}; + // TODO(barulicm): Set max velocity to 1.5 + motion_commands[kicking_robot.id] = command; } else { // Kick ball getPlayInfo()["State"] = "Kicking"; @@ -109,14 +102,22 @@ std::array, 16> OurPenaltyPla ateam_geometry::Point pattern_point(kRobotDiameter - (world.field.field_length / 2.0), kRobotDiameter - (world.field.field_width / 2.0)); ateam_geometry::Vector pattern_step(kRobotDiameter + 0.2, 0.0); - for (const auto & robot : available_robots) { - auto & move_to = move_tos_[robot.id]; - move_to.setTargetPosition(pattern_point); - move_to.setMaxVelocity(1.5); - move_to.face_travel(); - motion_commands[robot.id] = move_to.runFrame(robot, world); - pattern_point += pattern_step; - } + std::vector target_points; + std::generate_n(std::back_inserter(target_points), available_robots.size(), + [pos = pattern_point, step = pattern_step,&world]() mutable { + auto current = pos; + pos = pos + step; + if (pos.x() > (world.field.field_length / 2.0) - kRobotDiameter) { + pos = ateam_geometry::Point( + kRobotDiameter - (world.field.field_length / 2.0), + pos.y() + step.y()); + } + return current; + }); + multi_move_to_.SetTargetPoints(target_points); + multi_move_to_.SetFaceTravel(); + // TODO(barulicm): Set max velocity to 1.5 + multi_move_to_.RunFrame(available_robots, motion_commands); return motion_commands; } diff --git a/ateam_kenobi/src/plays/our_penalty_play.hpp b/ateam_kenobi/src/plays/our_penalty_play.hpp index 2bc5348f6..83f9ec9f0 100644 --- a/ateam_kenobi/src/plays/our_penalty_play.hpp +++ b/ateam_kenobi/src/plays/our_penalty_play.hpp @@ -25,7 +25,7 @@ #include "core/stp/play.hpp" #include "skills/goalie.hpp" #include "skills/universal_kick.hpp" -#include "core/play_helpers/easy_move_to.hpp" +#include "tactics/multi_move_to.hpp" namespace ateam_kenobi::plays { @@ -39,15 +39,13 @@ class OurPenaltyPlay : public stp::Play stp::PlayScore getScore(const World & world) override; - void reset() override; - - std::array, + std::array, 16> runFrame(const World & world) override; private: skills::Goalie goalie_skill_; skills::UniversalKick kick_skill_; - std::array move_tos_; + tactics::MultiMoveTo multi_move_to_; std::chrono::steady_clock::time_point kick_time_ = std::chrono::steady_clock::time_point::max(); ateam_geometry::Point chooseKickTarget(const World & world); diff --git a/ateam_kenobi/src/plays/their_ball_placement_play.cpp b/ateam_kenobi/src/plays/their_ball_placement_play.cpp index fafa42758..1211863e7 100644 --- a/ateam_kenobi/src/plays/their_ball_placement_play.cpp +++ b/ateam_kenobi/src/plays/their_ball_placement_play.cpp @@ -29,9 +29,9 @@ namespace ateam_kenobi::plays { TheirBallPlacementPlay::TheirBallPlacementPlay(stp::Options stp_options) -: stp::Play(kPlayName, stp_options) +: stp::Play(kPlayName, stp_options), + multi_move_to_(createChild("multi_move_to")) { - createIndexedChildren(easy_move_tos_, "EasyMoveTo"); } stp::PlayScore TheirBallPlacementPlay::getScore(const World & world) @@ -43,17 +43,10 @@ stp::PlayScore TheirBallPlacementPlay::getScore(const World & world) return stp::PlayScore::NaN(); } -void TheirBallPlacementPlay::reset() -{ - for (auto & emt : easy_move_tos_) { - emt.reset(); - } -} - -std::array, 16> TheirBallPlacementPlay::runFrame( +std::array, 16> TheirBallPlacementPlay::runFrame( const World & world) { - std::array, 16> maybe_motion_commands; + std::array, 16> maybe_motion_commands; auto available_robots = play_helpers::getAvailableRobots(world); @@ -68,54 +61,34 @@ std::array, 16> TheirBallPlac } }(); - const auto point_to_ball = world.ball.pos - placement_point; - const auto angle = std::atan2(point_to_ball.y(), point_to_ball.x()); DrawKeepoutArea(world.ball.pos, placement_point); - for (auto ind = 0ul; ind < available_robots.size(); ++ind) { - const auto & robot = available_robots[ind]; - auto & emt = easy_move_tos_[robot.id]; - - const auto placement_segment = ateam_geometry::Segment(placement_point, world.ball.pos); - const auto nearest_point = ateam_geometry::nearestPointOnSegment(placement_segment, robot.pos); - - ateam_geometry::Point target_position = robot.pos; - if (ateam_geometry::norm(robot.pos - nearest_point) < 0.6 + kRobotRadius) { - target_position = nearest_point + - 0.7 * ateam_geometry::Vector(std::cos(angle + M_PI / 2), std::sin(angle + M_PI / 2)); - - const auto alternate_position = nearest_point + - 0.7 * ateam_geometry::Vector(std::cos(angle - M_PI / 2), std::sin(angle - M_PI / 2)); + const auto partition_iter = std::partition(available_robots.begin(), available_robots.end(), + [this, &world, &placement_point](const Robot & robot) { + return shouldRobotMove(world, placement_point, robot); + }); - const auto offset = kRobotRadius * 0.95; - const auto x_bound = (world.field.field_length / 2.0) + world.field.boundary_width - offset; - const auto y_bound = (world.field.field_width / 2.0) + world.field.boundary_width - offset; - ateam_geometry::Rectangle pathable_region(ateam_geometry::Point(-x_bound, -y_bound), - ateam_geometry::Point(x_bound, y_bound)); + const std::vector robots_to_move(available_robots.begin(), partition_iter); + const std::vector robots_to_stay(partition_iter, available_robots.end()); - if (!CGAL::do_intersect(target_position, pathable_region)) { - target_position = alternate_position; - } else if (!CGAL::do_intersect(alternate_position, pathable_region)) { - // Stick with target_position - } else { - // Use the shorter path - if (ateam_geometry::norm(target_position - robot.pos) > - ateam_geometry::norm(alternate_position - robot.pos)) - { - target_position = alternate_position; - } - } - - getPlayInfo()["Robots"][std::to_string(robot.id)] = "MOVING"; - } else { - getPlayInfo()["Robots"][std::to_string(robot.id)] = "-"; - } - - emt.setTargetPosition(target_position); - emt.face_point(world.ball.pos); - maybe_motion_commands[robot.id] = emt.runFrame(robot, world); + for(const auto & robot : robots_to_move) { + getPlayInfo()["Robots"][std::to_string(robot.id)] = "MOVING"; } + for(const auto & robot : robots_to_stay) { + getPlayInfo()["Robots"][std::to_string(robot.id)] = "-"; + } + + std::vector target_points; + std::transform(robots_to_move.begin(), robots_to_move.end(), + std::back_inserter(target_points), + [this, &world, &placement_point](const Robot & robot) { + return getTargetPoint(world, placement_point, robot); + }); + + multi_move_to_.SetTargetPoints(target_points); + multi_move_to_.SetFacePoint(world.ball.pos); + multi_move_to_.RunFrame(robots_to_move, maybe_motion_commands); return maybe_motion_commands; } @@ -151,4 +124,50 @@ void TheirBallPlacementPlay::DrawKeepoutArea( "Red"); } +bool TheirBallPlacementPlay::shouldRobotMove( + const World & world, + const ateam_geometry::Point & placement_point, const Robot & robot) +{ + const auto placement_segment = ateam_geometry::Segment(placement_point, world.ball.pos); + const auto nearest_point = ateam_geometry::nearestPointOnSegment(placement_segment, robot.pos); + return ateam_geometry::norm(robot.pos - nearest_point) < 0.6 + kRobotRadius; +} + +ateam_geometry::Point TheirBallPlacementPlay::getTargetPoint( + const World & world, + const ateam_geometry::Point & placement_point, const Robot & robot) +{ + const auto placement_segment = ateam_geometry::Segment(placement_point, world.ball.pos); + const auto nearest_point = ateam_geometry::nearestPointOnSegment(placement_segment, robot.pos); + + const auto point_to_ball = world.ball.pos - placement_point; + const auto angle = std::atan2(point_to_ball.y(), point_to_ball.x()); + + auto target_position = nearest_point + + 0.7 * ateam_geometry::Vector(std::cos(angle + M_PI / 2), std::sin(angle + M_PI / 2)); + + const auto alternate_position = nearest_point + + 0.7 * ateam_geometry::Vector(std::cos(angle - M_PI / 2), std::sin(angle - M_PI / 2)); + + const auto offset = kRobotRadius * 0.95; + const auto x_bound = (world.field.field_length / 2.0) + world.field.boundary_width - offset; + const auto y_bound = (world.field.field_width / 2.0) + world.field.boundary_width - offset; + ateam_geometry::Rectangle pathable_region(ateam_geometry::Point(-x_bound, -y_bound), + ateam_geometry::Point(x_bound, y_bound)); + + if (!CGAL::do_intersect(target_position, pathable_region)) { + target_position = alternate_position; + } else if (!CGAL::do_intersect(alternate_position, pathable_region)) { + // Stick with target_position + } else { + // Use the shorter path + if (ateam_geometry::norm(target_position - robot.pos) > + ateam_geometry::norm(alternate_position - robot.pos)) + { + target_position = alternate_position; + } + } + return target_position; +} + } // namespace ateam_kenobi::plays diff --git a/ateam_kenobi/src/plays/their_ball_placement_play.hpp b/ateam_kenobi/src/plays/their_ball_placement_play.hpp index cc0f75a70..1c6da6934 100644 --- a/ateam_kenobi/src/plays/their_ball_placement_play.hpp +++ b/ateam_kenobi/src/plays/their_ball_placement_play.hpp @@ -23,6 +23,7 @@ #include "core/stp/play.hpp" #include "core/play_helpers/easy_move_to.hpp" +#include "tactics/multi_move_to.hpp" namespace ateam_kenobi::plays { @@ -36,18 +37,19 @@ class TheirBallPlacementPlay : public stp::Play stp::PlayScore getScore(const World & world) override; - - void reset() override; - - std::array, + std::array, 16> runFrame(const World & world) override; private: - std::array easy_move_tos_; + tactics::MultiMoveTo multi_move_to_; void DrawKeepoutArea( const ateam_geometry::Point & ball_pos, const ateam_geometry::Point & placement_point); + + bool shouldRobotMove(const World & world, const ateam_geometry::Point & placement_point, const Robot & robot); + + ateam_geometry::Point getTargetPoint(const World & world, const ateam_geometry::Point & placement_point, const Robot & robot); }; } // namespace ateam_kenobi::plays diff --git a/ateam_kenobi/src/plays/their_penalty_play.cpp b/ateam_kenobi/src/plays/their_penalty_play.cpp index f189246e8..118e181bf 100644 --- a/ateam_kenobi/src/plays/their_penalty_play.cpp +++ b/ateam_kenobi/src/plays/their_penalty_play.cpp @@ -29,9 +29,9 @@ namespace ateam_kenobi::plays TheirPenaltyPlay::TheirPenaltyPlay(stp::Options stp_options) : stp::Play(kPlayName, stp_options), - goalie_skill_(createChild("goalie")) + goalie_skill_(createChild("goalie")), + multi_move_to_(createChild("multi_move_to")) { - createIndexedChildren(move_tos_, "EasyMoveTo"); } stp::PlayScore TheirPenaltyPlay::getScore(const World & world) @@ -50,35 +50,36 @@ stp::PlayScore TheirPenaltyPlay::getScore(const World & world) return stp::PlayScore::NaN(); } -void TheirPenaltyPlay::reset() -{ - for (auto & move_to : move_tos_) { - move_to.reset(); - } -} - -std::array, 16> TheirPenaltyPlay::runFrame( +std::array, 16> TheirPenaltyPlay::runFrame( const World & world) { - std::array, 16> motion_commands; + std::array, 16> motion_commands; auto available_robots = play_helpers::getAvailableRobots(world); play_helpers::removeGoalie(available_robots, world); goalie_skill_.runFrame(world, motion_commands); - auto i = 0; ateam_geometry::Point pattern_start(kRobotDiameter - (world.field.field_length / 2.0), kRobotDiameter - (world.field.field_width / 2.0)); ateam_geometry::Vector pattern_step(kRobotDiameter + 0.2, 0.0); - for (const auto & robot : available_robots) { - auto & move_to = move_tos_[robot.id]; - move_to.setTargetPosition(pattern_start + (i * pattern_step)); - move_to.setMaxVelocity(1.5); - move_to.face_travel(); - motion_commands[robot.id] = move_to.runFrame(robot, world); - i++; - } + + std::vector target_points; + std::generate_n(std::back_inserter(target_points), available_robots.size(), + [pos = pattern_start, step = pattern_step,&world]() mutable { + auto current = pos; + pos = pos + step; + if (pos.x() > (world.field.field_length / 2.0) - kRobotDiameter) { + pos = ateam_geometry::Point( + kRobotDiameter - (world.field.field_length / 2.0), + pos.y() + step.y()); + } + return current; + }); + multi_move_to_.SetTargetPoints(target_points); + // multi_move_to_.SetMaxVelocity(1.5); // TODO(barulicm) + multi_move_to_.SetFaceTravel(); + multi_move_to_.RunFrame(available_robots, motion_commands); return motion_commands; } diff --git a/ateam_kenobi/src/plays/their_penalty_play.hpp b/ateam_kenobi/src/plays/their_penalty_play.hpp index ad59a3231..2b46c0f6e 100644 --- a/ateam_kenobi/src/plays/their_penalty_play.hpp +++ b/ateam_kenobi/src/plays/their_penalty_play.hpp @@ -25,6 +25,7 @@ #include "core/stp/play.hpp" #include "core/play_helpers/easy_move_to.hpp" #include "skills/goalie.hpp" +#include "tactics/multi_move_to.hpp" namespace ateam_kenobi::plays { @@ -38,14 +39,12 @@ class TheirPenaltyPlay : public stp::Play stp::PlayScore getScore(const World & world) override; - void reset() override; - - std::array, + std::array, 16> runFrame(const World & world) override; private: - std::array move_tos_; skills::Goalie goalie_skill_; + tactics::MultiMoveTo multi_move_to_; }; } // namespace ateam_kenobi::plays diff --git a/ateam_kenobi/src/plays/wall_play.cpp b/ateam_kenobi/src/plays/wall_play.cpp index fd1ec5199..40933c239 100644 --- a/ateam_kenobi/src/plays/wall_play.cpp +++ b/ateam_kenobi/src/plays/wall_play.cpp @@ -53,7 +53,8 @@ std::vector get_equally_spaced_points_on_segment( WallPlay::WallPlay(stp::Options stp_options) : stp::Play(kPlayName, stp_options), - goalie_skill_(createChild("goalie")) + goalie_skill_(createChild("goalie")), + multi_move_to_(createChild("multi_move_to")) { } @@ -98,31 +99,9 @@ std::array, 16> WallPlay::runFrame( current_available_robots, positions_to_assign); - for (auto ind = 0ul; ind < robot_assignments.size(); ++ind) { - const auto & maybe_robot = robot_assignments[ind]; - if (!maybe_robot) { - continue; - } - - const auto & robot = *maybe_robot; - - if (!robot.IsAvailable()) { - continue; - } - - const auto & target_position = positions_to_assign.at(ind); - - RobotCommand command; - command.motion_intent.linear = motion::intents::linear::PositionIntent{target_position}; - command.motion_intent.angular = motion::intents::angular::FacingIntent{world.ball.pos}; - - auto viz_circle = ateam_geometry::makeCircle(target_position, kRobotRadius); - getOverlays().drawCircle( - "destination_" + std::to_string( - robot.id), viz_circle, "blue", "transparent"); - - commands.at(robot.id) = command; - } + multi_move_to_.SetTargetPoints(positions_to_assign); + multi_move_to_.SetFacePoint(world.ball.pos); + multi_move_to_.RunFrame(robot_assignments, commands); return commands; } diff --git a/ateam_kenobi/src/plays/wall_play.hpp b/ateam_kenobi/src/plays/wall_play.hpp index 88f7e464b..e93b83308 100644 --- a/ateam_kenobi/src/plays/wall_play.hpp +++ b/ateam_kenobi/src/plays/wall_play.hpp @@ -30,6 +30,7 @@ #include "core/stp/play.hpp" #include "core/play_helpers/easy_move_to.hpp" #include "skills/goalie.hpp" +#include "tactics/multi_move_to.hpp" namespace ateam_kenobi::plays { @@ -49,6 +50,7 @@ class WallPlay : public stp::Play private: skills::Goalie goalie_skill_; + tactics::MultiMoveTo multi_move_to_; }; } // namespace ateam_kenobi::plays diff --git a/ateam_kenobi/src/tactics/multi_move_to.cpp b/ateam_kenobi/src/tactics/multi_move_to.cpp index 431fe414d..62194cdf4 100644 --- a/ateam_kenobi/src/tactics/multi_move_to.cpp +++ b/ateam_kenobi/src/tactics/multi_move_to.cpp @@ -58,4 +58,24 @@ void MultiMoveTo::RunFrame( } } + +void MultiMoveTo::RunFrame( + const std::vector & robots, + std::array, 16> & motion_commands) +{ + for (auto ind = 0ul; ind < robots.size(); ++ind) { + const auto & robot = robots[ind]; + const auto & target_position = target_points_[ind]; + RobotCommand command; + command.motion_intent.linear = motion::intents::linear::PositionIntent{target_position}; + command.motion_intent.angular = angular_intent_; + motion_commands.at(robot.id) = command; + + auto viz_circle = ateam_geometry::makeCircle(target_position, kRobotRadius); + getOverlays().drawCircle( + "destination_" + std::to_string( + robot.id), viz_circle, "blue", "transparent"); + } +} + } // namespace ateam_kenobi::tactics diff --git a/ateam_kenobi/src/tactics/multi_move_to.hpp b/ateam_kenobi/src/tactics/multi_move_to.hpp index 176d9d00d..94ce64ba9 100644 --- a/ateam_kenobi/src/tactics/multi_move_to.hpp +++ b/ateam_kenobi/src/tactics/multi_move_to.hpp @@ -39,6 +39,10 @@ class MultiMoveTo : public stp::Tactic const std::vector> & robots, std::array, 16> & motion_commands); + void RunFrame( + const std::vector & robots, + std::array, 16> & motion_commands); + void SetTargetPoints(const std::vector & targets) { target_points_ = targets; From 2483a43a2a2c6234d350e20b5880d945819d1a1f Mon Sep 17 00:00:00 2001 From: Matthew Barulic Date: Tue, 16 Sep 2025 23:11:22 -0400 Subject: [PATCH 20/35] Migrates stop plays to motion intents --- ateam_kenobi/CMakeLists.txt | 8 ++-- .../core/path_planning/escape_velocity.hpp | 2 + .../plays/stop_plays/default_stop_play.cpp | 26 ++++-------- .../plays/stop_plays/default_stop_play.hpp | 8 +--- .../plays/stop_plays/defensive_stop_play.cpp | 40 +++++++------------ .../plays/stop_plays/defensive_stop_play.hpp | 9 +---- .../plays/stop_plays/offensive_stop_play.cpp | 40 +++++++------------ .../plays/stop_plays/offensive_stop_play.hpp | 9 +---- .../src/plays/stop_plays/stop_helpers.cpp | 33 +++++++-------- .../src/plays/stop_plays/stop_helpers.hpp | 10 ++--- 10 files changed, 67 insertions(+), 118 deletions(-) diff --git a/ateam_kenobi/CMakeLists.txt b/ateam_kenobi/CMakeLists.txt index c2a520c34..2cf0ebc69 100644 --- a/ateam_kenobi/CMakeLists.txt +++ b/ateam_kenobi/CMakeLists.txt @@ -60,10 +60,10 @@ target_sources(kenobi_node_component PRIVATE # src/plays/kickoff_plays/offense/our_kickoff_prep_play.cpp # src/plays/passing_plays/pass_to_lane_play.cpp # src/plays/passing_plays/pass_to_segment_play.cpp - # src/plays/stop_plays/default_stop_play.cpp - # src/plays/stop_plays/defensive_stop_play.cpp - # src/plays/stop_plays/offensive_stop_play.cpp - # src/plays/stop_plays/stop_helpers.cpp + src/plays/stop_plays/default_stop_play.cpp + src/plays/stop_plays/defensive_stop_play.cpp + src/plays/stop_plays/offensive_stop_play.cpp + src/plays/stop_plays/stop_helpers.cpp # src/plays/test_plays/controls_test_play.cpp # src/plays/test_plays/spinning_a_play.cpp # src/plays/test_plays/test_pass_play.cpp diff --git a/ateam_kenobi/src/core/path_planning/escape_velocity.hpp b/ateam_kenobi/src/core/path_planning/escape_velocity.hpp index 6045002fd..c25fb1280 100644 --- a/ateam_kenobi/src/core/path_planning/escape_velocity.hpp +++ b/ateam_kenobi/src/core/path_planning/escape_velocity.hpp @@ -32,6 +32,8 @@ namespace ateam_kenobi::path_planning constexpr double kSafeEscapeVelocity = 0.3; // m/s +// TODO(barulicm): Convert this file to using BodyVelocity return types + /** * @brief Creates a low velocity in the quickest direction to escape the given obstacle. * diff --git a/ateam_kenobi/src/plays/stop_plays/default_stop_play.cpp b/ateam_kenobi/src/plays/stop_plays/default_stop_play.cpp index bbafc9d8c..e65089e9a 100644 --- a/ateam_kenobi/src/plays/stop_plays/default_stop_play.cpp +++ b/ateam_kenobi/src/plays/stop_plays/default_stop_play.cpp @@ -40,12 +40,6 @@ namespace ateam_kenobi::plays DefaultStopPlay::DefaultStopPlay(stp::Options stp_options) : stp::Play(kPlayName, stp_options) { - createIndexedChildren(easy_move_tos_, "EasyMoveTo"); - for (auto & move_to : easy_move_tos_) { - // Rules say <1.5m/s. We'll use 1m/s to give some room for error. - move_to.setMaxVelocity(1.0); - } - DefaultStopPlay::reset(); } stp::PlayScore DefaultStopPlay::getScore(const World & world) @@ -58,31 +52,27 @@ stp::PlayScore DefaultStopPlay::getScore(const World & world) } } -void DefaultStopPlay::reset() -{ - for (auto & move_to : easy_move_tos_) { - move_to.reset(); - } -} - -std::array, 16> DefaultStopPlay::runFrame( +std::array, 16> DefaultStopPlay::runFrame( const World & world) { const auto added_obstacles = helpers::getAddedObstacles(world); helpers::drawObstacles(world, added_obstacles, getOverlays(), getLogger()); - std::array, 16> motion_commands; + std::array, 16> motion_commands; - helpers::moveBotsTooCloseToBall(world, added_obstacles, motion_commands, easy_move_tos_, - getOverlays(), getPlayInfo()); + helpers::moveBotsTooCloseToBall(world, added_obstacles, motion_commands, getOverlays(), + getPlayInfo()); helpers::moveBotsInObstacles(world, added_obstacles, motion_commands, getPlayInfo()); // Halt all robots that weren't already assigned a motion command std::ranges::replace_if( motion_commands, - [](const auto & o) {return !o;}, std::make_optional(ateam_msgs::msg::RobotMotionCommand{})); + [](const auto & o) {return !o;}, std::make_optional(RobotCommand{})); + + // Rules say <1.5m/s. We'll use 1m/s to give some room for error. + // TODO(barulicm): Set max velocity to 1.0 return motion_commands; } diff --git a/ateam_kenobi/src/plays/stop_plays/default_stop_play.hpp b/ateam_kenobi/src/plays/stop_plays/default_stop_play.hpp index 7fa3c7740..905a599b7 100644 --- a/ateam_kenobi/src/plays/stop_plays/default_stop_play.hpp +++ b/ateam_kenobi/src/plays/stop_plays/default_stop_play.hpp @@ -26,7 +26,6 @@ #include "core/path_planning/path_planner.hpp" #include "core/motion/motion_controller.hpp" #include "core/stp/play.hpp" -#include "core/play_helpers/easy_move_to.hpp" namespace ateam_kenobi::plays { @@ -39,13 +38,8 @@ class DefaultStopPlay : public stp::Play stp::PlayScore getScore(const World & world) override; - void reset() override; - - std::array, + std::array, 16> runFrame(const World & world) override; - -private: - std::array easy_move_tos_; }; } // namespace ateam_kenobi::plays diff --git a/ateam_kenobi/src/plays/stop_plays/defensive_stop_play.cpp b/ateam_kenobi/src/plays/stop_plays/defensive_stop_play.cpp index 2f1d4939f..0311a2dae 100644 --- a/ateam_kenobi/src/plays/stop_plays/defensive_stop_play.cpp +++ b/ateam_kenobi/src/plays/stop_plays/defensive_stop_play.cpp @@ -41,12 +41,6 @@ namespace ateam_kenobi::plays DefensiveStopPlay::DefensiveStopPlay(stp::Options stp_options) : stp::Play(kPlayName, stp_options) { - createIndexedChildren(easy_move_tos_, "EasyMoveTo"); - for (auto & move_to : easy_move_tos_) { - // Rules say <1.5m/s. We'll use 1m/s to give some room for error. - move_to.setMaxVelocity(1.0); - } - DefensiveStopPlay::reset(); } stp::PlayScore DefensiveStopPlay::getScore(const World & world) @@ -67,40 +61,36 @@ stp::PlayScore DefensiveStopPlay::getScore(const World & world) } } -void DefensiveStopPlay::reset() -{ - for (auto & move_to : easy_move_tos_) { - move_to.reset(); - } -} - -std::array, 16> DefensiveStopPlay::runFrame( +std::array, 16> DefensiveStopPlay::runFrame( const World & world) { const auto added_obstacles = helpers::getAddedObstacles(world); helpers::drawObstacles(world, added_obstacles, getOverlays(), getLogger()); - std::array, 16> motion_commands; + std::array, 16> motion_commands; runPrepBot(world, motion_commands); - helpers::moveBotsTooCloseToBall(world, added_obstacles, motion_commands, easy_move_tos_, - getOverlays(), getPlayInfo()); + helpers::moveBotsTooCloseToBall(world, added_obstacles, motion_commands, getOverlays(), + getPlayInfo()); helpers::moveBotsInObstacles(world, added_obstacles, motion_commands, getPlayInfo()); // Halt all robots that weren't already assigned a motion command std::ranges::replace_if( motion_commands, - [](const auto & o) {return !o;}, std::make_optional(ateam_msgs::msg::RobotMotionCommand{})); + [](const auto & o) {return !o;}, std::make_optional(RobotCommand{})); + + // Rules say <1.5m/s. We'll use 1m/s to give some room for error. + // TODO(barulicm): set max velocity to 1.0 return motion_commands; } void DefensiveStopPlay::runPrepBot( const World & world, - std::array, 16> & maybe_motion_commands) + std::array, 16> & maybe_motion_commands) { const auto our_goal_center = ateam_geometry::Point(-world.field.field_length / 2.0, 0.0); const auto target_position = world.ball.pos + @@ -123,15 +113,13 @@ void DefensiveStopPlay::runPrepBot( return; } - auto & emt = easy_move_tos_[closest_bot.id]; - - std::vector obstacles { + RobotCommand command; + command.motion_intent.linear = motion::intents::linear::PositionIntent{target_position}; + command.motion_intent.angular = motion::intents::angular::FacingIntent{world.ball.pos}; + command.motion_intent.obstacles = { ateam_geometry::makeDisk(world.ball.pos, stop_plays::stop_helpers::kKeepoutRadiusRules) }; - - emt.setTargetPosition(target_position); - emt.face_point(world.ball.pos); - maybe_motion_commands[closest_bot.id] = emt.runFrame(closest_bot, world, obstacles); + maybe_motion_commands[closest_bot.id] = command; } } // namespace ateam_kenobi::plays diff --git a/ateam_kenobi/src/plays/stop_plays/defensive_stop_play.hpp b/ateam_kenobi/src/plays/stop_plays/defensive_stop_play.hpp index 505b60d1c..959530a22 100644 --- a/ateam_kenobi/src/plays/stop_plays/defensive_stop_play.hpp +++ b/ateam_kenobi/src/plays/stop_plays/defensive_stop_play.hpp @@ -22,7 +22,6 @@ #define PLAYS__STOP_PLAYS__DEFENSIVE_STOP_PLAY_HPP_ #include "core/stp/play.hpp" -#include "core/play_helpers/easy_move_to.hpp" namespace ateam_kenobi::plays { @@ -36,19 +35,15 @@ class DefensiveStopPlay : public stp::Play stp::PlayScore getScore(const World & world) override; - void reset() override; - - std::array, + std::array, 16> runFrame(const World & world) override; private: constexpr static double kPrepBotDistFromBall = 0.9; - std::array easy_move_tos_; - void runPrepBot( const World & world, - std::array, 16> & maybe_motion_commands); + std::array, 16> & maybe_motion_commands); }; } // namespace ateam_kenobi::plays diff --git a/ateam_kenobi/src/plays/stop_plays/offensive_stop_play.cpp b/ateam_kenobi/src/plays/stop_plays/offensive_stop_play.cpp index f6f84ac4a..3fe77daac 100644 --- a/ateam_kenobi/src/plays/stop_plays/offensive_stop_play.cpp +++ b/ateam_kenobi/src/plays/stop_plays/offensive_stop_play.cpp @@ -41,12 +41,6 @@ namespace ateam_kenobi::plays OffensiveStopPlay::OffensiveStopPlay(stp::Options stp_options) : stp::Play(kPlayName, stp_options) { - createIndexedChildren(easy_move_tos_, "EasyMoveTo"); - for (auto & move_to : easy_move_tos_) { - // Rules say <1.5m/s. We'll use 1m/s to give some room for error. - move_to.setMaxVelocity(1.0); - } - OffensiveStopPlay::reset(); } stp::PlayScore OffensiveStopPlay::getScore(const World & world) @@ -68,40 +62,36 @@ stp::PlayScore OffensiveStopPlay::getScore(const World & world) } } -void OffensiveStopPlay::reset() -{ - for (auto & move_to : easy_move_tos_) { - move_to.reset(); - } -} - -std::array, 16> OffensiveStopPlay::runFrame( +std::array, 16> OffensiveStopPlay::runFrame( const World & world) { const auto added_obstacles = helpers::getAddedObstacles(world); helpers::drawObstacles(world, added_obstacles, getOverlays(), getLogger()); - std::array, 16> motion_commands; + std::array, 16> motion_commands; runPrepBot(world, motion_commands); - helpers::moveBotsTooCloseToBall(world, added_obstacles, motion_commands, easy_move_tos_, - getOverlays(), getPlayInfo()); + helpers::moveBotsTooCloseToBall(world, added_obstacles, motion_commands, getOverlays(), + getPlayInfo()); helpers::moveBotsInObstacles(world, added_obstacles, motion_commands, getPlayInfo()); // Halt all robots that weren't already assigned a motion command std::ranges::replace_if( motion_commands, - [](const auto & o) {return !o;}, std::make_optional(ateam_msgs::msg::RobotMotionCommand{})); + [](const auto & o) {return !o;}, std::make_optional(RobotCommand{})); + + // Rules say <1.5m/s. We'll use 1m/s to give some room for error. + // TODO(barulicm): Set max velocity to 1.0 return motion_commands; } void OffensiveStopPlay::runPrepBot( const World & world, - std::array, 16> & maybe_motion_commands) + std::array, 16> & maybe_motion_commands) { const auto their_goal_center = ateam_geometry::Point(world.field.field_length / 2.0, 0.0); // TODO(barulicm): May need to handle balls close to field edge smarter @@ -125,15 +115,13 @@ void OffensiveStopPlay::runPrepBot( return; } - auto & emt = easy_move_tos_[closest_bot.id]; - - std::vector obstacles { + RobotCommand command; + command.motion_intent.linear = motion::intents::linear::PositionIntent{target_position}; + command.motion_intent.angular = motion::intents::angular::FacingIntent{world.ball.pos}; + command.motion_intent.obstacles = { ateam_geometry::makeDisk(world.ball.pos, stop_plays::stop_helpers::kKeepoutRadiusRules) }; - - emt.setTargetPosition(target_position); - emt.face_point(world.ball.pos); - maybe_motion_commands[closest_bot.id] = emt.runFrame(closest_bot, world, obstacles); + maybe_motion_commands[closest_bot.id] = command; } } // namespace ateam_kenobi::plays diff --git a/ateam_kenobi/src/plays/stop_plays/offensive_stop_play.hpp b/ateam_kenobi/src/plays/stop_plays/offensive_stop_play.hpp index 92f350c7b..5b12cf756 100644 --- a/ateam_kenobi/src/plays/stop_plays/offensive_stop_play.hpp +++ b/ateam_kenobi/src/plays/stop_plays/offensive_stop_play.hpp @@ -22,7 +22,6 @@ #define PLAYS__STOP_PLAYS__OFFENSIVE_STOP_PLAY_HPP_ #include "core/stp/play.hpp" -#include "core/play_helpers/easy_move_to.hpp" namespace ateam_kenobi::plays { @@ -36,19 +35,15 @@ class OffensiveStopPlay : public stp::Play stp::PlayScore getScore(const World & world) override; - void reset() override; - - std::array, + std::array, 16> runFrame(const World & world) override; private: constexpr static double kPrepBotDistFromBall = 0.9; - std::array easy_move_tos_; - void runPrepBot( const World & world, - std::array, 16> & maybe_motion_commands); + std::array, 16> & maybe_motion_commands); }; } // namespace ateam_kenobi::plays diff --git a/ateam_kenobi/src/plays/stop_plays/stop_helpers.cpp b/ateam_kenobi/src/plays/stop_plays/stop_helpers.cpp index 34e0227a0..78100e403 100644 --- a/ateam_kenobi/src/plays/stop_plays/stop_helpers.cpp +++ b/ateam_kenobi/src/plays/stop_plays/stop_helpers.cpp @@ -224,8 +224,8 @@ void drawObstacles( void moveBotsTooCloseToBall( const World & world, const std::vector & added_obstacles, - std::array, 16> & motion_commands, - std::array & easy_move_tos, visualization::Overlays & overlays, + std::array, 16> & motion_commands, + visualization::Overlays & overlays, nlohmann::json & play_info) { const auto spots = getOpenSpots(world, overlays); @@ -256,19 +256,12 @@ void moveBotsTooCloseToBall( if(motion_commands[bot.id]) { continue; } - auto & emt = easy_move_tos.at(bot.id); - emt.setTargetPosition(spot); - emt.face_point(world.ball.pos); - if (bot.id == world.referee_info.our_goalie_id) { - auto planner_options = emt.getPlannerOptions(); - planner_options.use_default_obstacles = false; - emt.setPlannerOptions(planner_options); - } else { - auto planner_options = emt.getPlannerOptions(); - planner_options.use_default_obstacles = true; - emt.setPlannerOptions(planner_options); - } - motion_commands.at(bot.id) = emt.runFrame(bot, world, added_obstacles); + RobotCommand command; + command.motion_intent.linear = motion::intents::linear::PositionIntent{spot}; + command.motion_intent.angular = motion::intents::angular::FacingIntent{world.ball.pos}; + command.motion_intent.planner_options.use_default_obstacles = bot.id != world.referee_info.our_goalie_id; + command.motion_intent.obstacles = added_obstacles; + motion_commands[bot.id] = command; overlays.drawCircle( "spot" + std::to_string(spot_ind), ateam_geometry::makeCircle(spot, kRobotRadius), "blue", "transparent"); @@ -278,7 +271,7 @@ void moveBotsTooCloseToBall( void moveBotsInObstacles( const World & world, const std::vector & added_obstacles, - std::array, 16> & motion_commands, + std::array, 16> & motion_commands, nlohmann::json & play_info) { auto & play_info_bots = play_info["bots in obstacles"]; @@ -295,8 +288,12 @@ void moveBotsInObstacles( if(!opt_escape_vel) { continue; } - ateam_msgs::msg::RobotMotionCommand command; - command.twist = *opt_escape_vel; + RobotCommand command; + command.motion_intent.linear = motion::intents::linear::VelocityIntent{ + ateam_geometry::Vector{opt_escape_vel->linear.x, opt_escape_vel->linear.y}, + motion::intents::linear::Frame::World + }; + command.motion_intent.angular = motion::intents::angular::VelocityIntent{opt_escape_vel->angular.z}; motion_commands[i] = command; play_info_bots.push_back(robot.id); } diff --git a/ateam_kenobi/src/plays/stop_plays/stop_helpers.hpp b/ateam_kenobi/src/plays/stop_plays/stop_helpers.hpp index 736ad8d84..dcf1efa09 100644 --- a/ateam_kenobi/src/plays/stop_plays/stop_helpers.hpp +++ b/ateam_kenobi/src/plays/stop_plays/stop_helpers.hpp @@ -27,10 +27,10 @@ #include #include #include -#include #include +#include "core/types/robot_command.hpp" #include "core/types/world.hpp" -#include "core/play_helpers/easy_move_to.hpp" +#include "core/visualization/overlays.hpp" namespace ateam_kenobi::plays::stop_plays::stop_helpers { @@ -56,14 +56,14 @@ void drawObstacles( void moveBotsTooCloseToBall( const World & world, const std::vector & added_obstacles, - std::array, 16> & motion_commands, - std::array & easy_move_tos, visualization::Overlays & overlays, + std::array, 16> & motion_commands, + visualization::Overlays & overlays, nlohmann::json & play_info); void moveBotsInObstacles( const World & world, const std::vector & added_obstacles, - std::array, 16> & motion_commands, + std::array, 16> & motion_commands, nlohmann::json & play_info); } // namespace ateam_kenobi::plays::stop_plays::stop_helpers From 5e4c17d0bece19ff918ab73834fcf106831e380b Mon Sep 17 00:00:00 2001 From: Matthew Barulic Date: Tue, 16 Sep 2025 23:12:14 -0400 Subject: [PATCH 21/35] Migrates CornerLineupPlay to motion intents --- .../plays/util_plays/corner_lineup_play.cpp | 38 +++++++------------ .../plays/util_plays/corner_lineup_play.hpp | 8 ++-- 2 files changed, 16 insertions(+), 30 deletions(-) diff --git a/ateam_kenobi/src/plays/util_plays/corner_lineup_play.cpp b/ateam_kenobi/src/plays/util_plays/corner_lineup_play.cpp index 1867dcb06..4fcd7e88e 100644 --- a/ateam_kenobi/src/plays/util_plays/corner_lineup_play.cpp +++ b/ateam_kenobi/src/plays/util_plays/corner_lineup_play.cpp @@ -30,9 +30,9 @@ namespace ateam_kenobi::plays CornerLineupPlay::CornerLineupPlay(stp::Options stp_options, double x_mult, double y_mult) : stp::Play(stp_options.name, stp_options), x_mult_(x_mult), - y_mult_(y_mult) + y_mult_(y_mult), + multi_move_to_(createChild("MultiMoveTo")) { - createIndexedChildren(easy_move_tos_, "EasyMoveTo"); setEnabled(false); } @@ -41,39 +41,27 @@ stp::PlayScore CornerLineupPlay::getScore(const World &) return stp::PlayScore::NaN(); } -void CornerLineupPlay::reset() -{ - for (auto & move_to : easy_move_tos_) { - move_to.reset(); - } -} - -std::array, 16> CornerLineupPlay::runFrame( +std::array, 16> CornerLineupPlay::runFrame( const World & world) { - std::array, 16> maybe_motion_commands; + std::array, 16> maybe_motion_commands; const ateam_geometry::Point start_point(x_mult_ * world.field.field_length / 2.0, y_mult_ * world.field.field_width / 2.0); const auto x_dir = -1.0 * std::copysign(1.0, x_mult_); const ateam_geometry::Vector direction(x_dir * 2 * kRobotDiameter, 0.0); - auto spot_counter = 0; - for (auto & robot : world.our_robots) { - if (!robot.visible && !robot.radio_connected) { - continue; - } + const std::vector available_robots = play_helpers::getAvailableRobots(world); - const auto spot = start_point + (direction * spot_counter); - spot_counter++; + std::vector target_points; + std::generate_n(std::back_inserter(target_points), available_robots.size(), + [spot = start_point, direction, n = 0]() mutable { + return spot + (direction * n++); + }); - if (robot.IsAvailable()) { - auto & easy_move_to = easy_move_tos_.at(robot.id); - easy_move_to.setTargetPosition(spot); - easy_move_to.face_absolute(0.0); - maybe_motion_commands.at(robot.id) = easy_move_to.runFrame(robot, world); - } - } + multi_move_to_.SetTargetPoints(target_points); + multi_move_to_.SetFaceAbsolue(0.0); + multi_move_to_.RunFrame(available_robots, maybe_motion_commands); return maybe_motion_commands; } diff --git a/ateam_kenobi/src/plays/util_plays/corner_lineup_play.hpp b/ateam_kenobi/src/plays/util_plays/corner_lineup_play.hpp index 17a983921..52afcc1de 100644 --- a/ateam_kenobi/src/plays/util_plays/corner_lineup_play.hpp +++ b/ateam_kenobi/src/plays/util_plays/corner_lineup_play.hpp @@ -28,7 +28,7 @@ #include "ateam_geometry/types.hpp" #include "core/types/robot.hpp" #include "core/stp/play.hpp" -#include "core/play_helpers/easy_move_to.hpp" +#include "tactics/multi_move_to.hpp" namespace ateam_kenobi::plays { @@ -40,15 +40,13 @@ class CornerLineupPlay : public stp::Play stp::PlayScore getScore(const World & world) override; - void reset() override; - - std::array, + std::array, 16> runFrame(const World & world) override; private: const double x_mult_; const double y_mult_; - std::array easy_move_tos_; + tactics::MultiMoveTo multi_move_to_; }; } // namespace ateam_kenobi::plays From 84d8fb3559e585e43d25834f22af5abf9af70d82 Mon Sep 17 00:00:00 2001 From: Matthew Barulic Date: Tue, 16 Sep 2025 23:17:09 -0400 Subject: [PATCH 22/35] Migrates free kick plays to motion intents --- ateam_kenobi/CMakeLists.txt | 4 ++-- .../defense/their_free_kick_play.cpp | 19 ++++++++----------- .../defense/their_free_kick_play.hpp | 6 ++---- .../offense/free_kick_on_goal_play.cpp | 8 ++++---- .../offense/free_kick_on_goal_play.hpp | 3 +-- 5 files changed, 17 insertions(+), 23 deletions(-) diff --git a/ateam_kenobi/CMakeLists.txt b/ateam_kenobi/CMakeLists.txt index 2cf0ebc69..d1dd97b7c 100644 --- a/ateam_kenobi/CMakeLists.txt +++ b/ateam_kenobi/CMakeLists.txt @@ -52,8 +52,8 @@ target_sources(kenobi_node_component PRIVATE # Play Sources # target_sources(kenobi_node_component PRIVATE - # src/plays/free_kick_plays/defense/their_free_kick_play.cpp - # src/plays/free_kick_plays/offense/free_kick_on_goal_play.cpp + src/plays/free_kick_plays/defense/their_free_kick_play.cpp + src/plays/free_kick_plays/offense/free_kick_on_goal_play.cpp # src/plays/kickoff_plays/defense/their_kickoff_play.cpp # src/plays/kickoff_plays/offense/kickoff_on_goal.cpp # src/plays/kickoff_plays/offense/kickoff_pass_play.cpp diff --git a/ateam_kenobi/src/plays/free_kick_plays/defense/their_free_kick_play.cpp b/ateam_kenobi/src/plays/free_kick_plays/defense/their_free_kick_play.cpp index c8a14b8b9..14a0c0403 100644 --- a/ateam_kenobi/src/plays/free_kick_plays/defense/their_free_kick_play.cpp +++ b/ateam_kenobi/src/plays/free_kick_plays/defense/their_free_kick_play.cpp @@ -32,7 +32,6 @@ TheirFreeKickPlay::TheirFreeKickPlay(stp::Options stp_options) : stp::Play(kPlayName, stp_options), defense_(createChild("defense")) { - createIndexedChildren(easy_move_tos_, "easy_move_to"); } stp::PlayScore TheirFreeKickPlay::getScore(const World & world) @@ -47,15 +46,12 @@ stp::PlayScore TheirFreeKickPlay::getScore(const World & world) void TheirFreeKickPlay::reset() { defense_.reset(); - for (auto & emt : easy_move_tos_) { - emt.reset(); - } } -std::array, +std::array, 16> TheirFreeKickPlay::runFrame(const World & world) { - std::array, 16> motion_commands; + std::array, 16> motion_commands; auto available_robots = play_helpers::getAvailableRobots(world); play_helpers::removeGoalie(available_robots, world); @@ -101,7 +97,7 @@ std::vector TheirFreeKickPlay::getBlockerPoints(const Wor void TheirFreeKickPlay::runBlockers( const World & world, const std::vector & robots, const std::vector & points, - std::array, 16> & motion_commands) + std::array, 16> & motion_commands) { // Rules section 5.3.3 require 0.5m distance between defenders and ball const auto ball_obstacle = ateam_geometry::makeDisk(world.ball.pos, 0.7); @@ -125,10 +121,11 @@ void TheirFreeKickPlay::runBlockers( for (auto i = 0ul; i < std::min(robots.size(), points.size()); ++i) { const auto & robot = robots[i]; const auto & point = points[i]; - auto & emt = easy_move_tos_[robot.id]; - emt.setTargetPosition(point); - emt.face_point(world.ball.pos); - motion_commands[robot.id] = emt.runFrame(robot, world, obstacles); + RobotCommand command; + command.motion_intent.linear = motion::intents::linear::PositionIntent{point}; + command.motion_intent.angular = motion::intents::angular::FacingIntent{world.ball.pos}; + command.motion_intent.obstacles = obstacles; + motion_commands[robot.id] = command; } } diff --git a/ateam_kenobi/src/plays/free_kick_plays/defense/their_free_kick_play.hpp b/ateam_kenobi/src/plays/free_kick_plays/defense/their_free_kick_play.hpp index 0eeba79ed..b85020a3e 100644 --- a/ateam_kenobi/src/plays/free_kick_plays/defense/their_free_kick_play.hpp +++ b/ateam_kenobi/src/plays/free_kick_plays/defense/their_free_kick_play.hpp @@ -23,7 +23,6 @@ #include #include "core/stp/play.hpp" -#include "core/play_helpers/easy_move_to.hpp" #include "tactics/standard_defense.hpp" namespace ateam_kenobi::plays @@ -40,19 +39,18 @@ class TheirFreeKickPlay : public stp::Play void reset() override; - std::array, + std::array, 16> runFrame(const World & world) override; private: tactics::StandardDefense defense_; - std::array easy_move_tos_; std::vector getBlockerPoints(const World & world); void runBlockers( const World & world, const std::vector & robots, const std::vector & points, - std::array, + std::array, 16> & motion_commands); }; diff --git a/ateam_kenobi/src/plays/free_kick_plays/offense/free_kick_on_goal_play.cpp b/ateam_kenobi/src/plays/free_kick_plays/offense/free_kick_on_goal_play.cpp index a5b828c78..c074bd02b 100644 --- a/ateam_kenobi/src/plays/free_kick_plays/offense/free_kick_on_goal_play.cpp +++ b/ateam_kenobi/src/plays/free_kick_plays/offense/free_kick_on_goal_play.cpp @@ -68,10 +68,10 @@ void FreeKickOnGoalPlay::enter() striker_.Reset(); } -std::array, +std::array, 16> FreeKickOnGoalPlay::runFrame(const World & world) { - std::array, 16> motion_commands; + std::array, 16> motion_commands; const auto window = GetLargestWindowOnGoal(world); @@ -123,7 +123,7 @@ std::array, if(available_robots.size() > 3) { assignments.RunPositionIfAssigned("idler1", [this, &world, &motion_commands](const auto & robot){ - motion_commands[robot.id] = idler_1_.RunFrame(world, robot); + motion_commands[robot.id] = idler_1_.RunFrame(world); getPlayInfo()["Idlers"].push_back(robot.id); }); } @@ -131,7 +131,7 @@ std::array, if(available_robots.size() > 4) { assignments.RunPositionIfAssigned("idler2", [this, &world, &motion_commands](const auto & robot){ - motion_commands[robot.id] = idler_2_.RunFrame(world, robot); + motion_commands[robot.id] = idler_2_.RunFrame(world); getPlayInfo()["Idlers"].push_back(robot.id); }); } diff --git a/ateam_kenobi/src/plays/free_kick_plays/offense/free_kick_on_goal_play.hpp b/ateam_kenobi/src/plays/free_kick_plays/offense/free_kick_on_goal_play.hpp index 4941e0736..5edfbca24 100644 --- a/ateam_kenobi/src/plays/free_kick_plays/offense/free_kick_on_goal_play.hpp +++ b/ateam_kenobi/src/plays/free_kick_plays/offense/free_kick_on_goal_play.hpp @@ -23,7 +23,6 @@ #include #include "core/stp/play.hpp" -#include "core/play_helpers/easy_move_to.hpp" #include "tactics/standard_defense.hpp" #include "skills/universal_kick.hpp" #include "skills/lane_idler.hpp" @@ -42,7 +41,7 @@ class FreeKickOnGoalPlay : public stp::Play void enter() override; - std::array, + std::array, 16> runFrame(const World & world) override; private: From 666cd28d524dede38efe01820d4bac00b88ff390 Mon Sep 17 00:00:00 2001 From: Matthew Barulic Date: Tue, 16 Sep 2025 23:25:20 -0400 Subject: [PATCH 23/35] Migrates kickoff plays to motion intents --- ateam_kenobi/CMakeLists.txt | 8 ++--- .../defense/their_kickoff_play.cpp | 33 +++++-------------- .../defense/their_kickoff_play.hpp | 9 +++-- .../kickoff_plays/offense/kickoff_on_goal.cpp | 7 ++-- .../kickoff_plays/offense/kickoff_on_goal.hpp | 2 +- .../offense/kickoff_pass_play.cpp | 11 +++---- .../offense/kickoff_pass_play.hpp | 2 +- .../offense/our_kickoff_prep_play.cpp | 7 ++-- .../offense/our_kickoff_prep_play.hpp | 2 +- 9 files changed, 31 insertions(+), 50 deletions(-) diff --git a/ateam_kenobi/CMakeLists.txt b/ateam_kenobi/CMakeLists.txt index d1dd97b7c..54847588c 100644 --- a/ateam_kenobi/CMakeLists.txt +++ b/ateam_kenobi/CMakeLists.txt @@ -54,10 +54,10 @@ target_sources(kenobi_node_component PRIVATE target_sources(kenobi_node_component PRIVATE src/plays/free_kick_plays/defense/their_free_kick_play.cpp src/plays/free_kick_plays/offense/free_kick_on_goal_play.cpp - # src/plays/kickoff_plays/defense/their_kickoff_play.cpp - # src/plays/kickoff_plays/offense/kickoff_on_goal.cpp - # src/plays/kickoff_plays/offense/kickoff_pass_play.cpp - # src/plays/kickoff_plays/offense/our_kickoff_prep_play.cpp + src/plays/kickoff_plays/defense/their_kickoff_play.cpp + src/plays/kickoff_plays/offense/kickoff_on_goal.cpp + src/plays/kickoff_plays/offense/kickoff_pass_play.cpp + src/plays/kickoff_plays/offense/our_kickoff_prep_play.cpp # src/plays/passing_plays/pass_to_lane_play.cpp # src/plays/passing_plays/pass_to_segment_play.cpp src/plays/stop_plays/default_stop_play.cpp diff --git a/ateam_kenobi/src/plays/kickoff_plays/defense/their_kickoff_play.cpp b/ateam_kenobi/src/plays/kickoff_plays/defense/their_kickoff_play.cpp index 04dd794c9..a0fe73d24 100644 --- a/ateam_kenobi/src/plays/kickoff_plays/defense/their_kickoff_play.cpp +++ b/ateam_kenobi/src/plays/kickoff_plays/defense/their_kickoff_play.cpp @@ -32,9 +32,9 @@ namespace ateam_kenobi::plays TheirKickoffPlay::TheirKickoffPlay(stp::Options stp_options) : stp::Play(kPlayName, stp_options), - defense_(createChild("defense")) + defense_(createChild("defense")), + multi_move_to_(createChild("multi_move_to")) { - createIndexedChildren(easy_move_tos_, "EasyMoveTo"); } stp::PlayScore TheirKickoffPlay::getScore(const World & world) @@ -56,15 +56,12 @@ stp::PlayScore TheirKickoffPlay::getScore(const World & world) void TheirKickoffPlay::reset() { defense_.reset(); - for (auto & emt : easy_move_tos_) { - emt.reset(); - } } -std::array, +std::array, 16> TheirKickoffPlay::runFrame(const World & world) { - std::array, 16> motion_commands; + std::array, 16> motion_commands; auto available_robots = play_helpers::getAvailableRobots(world); play_helpers::removeGoalie(available_robots, world); @@ -79,7 +76,7 @@ std::array, defense_.runFrame(world, assignments.GetGroupFilledAssignments("defense"), motion_commands); - runOffense(world, offense_points, assignments.GetGroupAssignments("offense"), motion_commands); + runOffense(offense_points, assignments.GetGroupAssignments("offense"), motion_commands); const auto offense_assignments = assignments.GetGroupAssignments("offense"); @@ -160,25 +157,13 @@ ateam_geometry::Point TheirKickoffPlay::getOffensePointToBlockTarget( } void TheirKickoffPlay::runOffense( - const World & world, const std::vector & points, const std::vector> & robots, - std::array, 16> & motion_commands) + std::array, 16> & motion_commands) { - const auto num_runable = std::min(points.size(), robots.size()); - - for (auto i = 0ul; i < num_runable; ++i) { - const auto & maybe_robot = robots[i]; - if (!maybe_robot) { - continue; - } - const auto & robot = *maybe_robot; - const auto & point = points[i]; - auto & emt = easy_move_tos_[robot.id]; - emt.setTargetPosition(point); - emt.face_absolute(0.0); - motion_commands[robot.id] = emt.runFrame(robot, world); - } + multi_move_to_.SetTargetPoints(points); + multi_move_to_.SetFaceAbsolue(0.0); + multi_move_to_.RunFrame(robots, motion_commands); } } // namespace ateam_kenobi::plays diff --git a/ateam_kenobi/src/plays/kickoff_plays/defense/their_kickoff_play.hpp b/ateam_kenobi/src/plays/kickoff_plays/defense/their_kickoff_play.hpp index 9c8fcd7ff..5847e4833 100644 --- a/ateam_kenobi/src/plays/kickoff_plays/defense/their_kickoff_play.hpp +++ b/ateam_kenobi/src/plays/kickoff_plays/defense/their_kickoff_play.hpp @@ -24,8 +24,8 @@ #include #include #include "core/stp/play.hpp" -#include "core/play_helpers/easy_move_to.hpp" #include "tactics/standard_defense.hpp" +#include "tactics/multi_move_to.hpp" namespace ateam_kenobi::plays { @@ -41,12 +41,12 @@ class TheirKickoffPlay : public stp::Play void reset() override; - std::array, + std::array, 16> runFrame(const World & world) override; private: - std::array easy_move_tos_; tactics::StandardDefense defense_; + tactics::MultiMoveTo multi_move_to_; std::vector getOffensePoints(const World & world); @@ -57,10 +57,9 @@ class TheirKickoffPlay : public stp::Play const ateam_geometry::Point & fallback); void runOffense( - const World & world, const std::vector & points, const std::vector> & robots, - std::array, 16> & motion_commands); + std::array, 16> & motion_commands); }; } // namespace ateam_kenobi::plays diff --git a/ateam_kenobi/src/plays/kickoff_plays/offense/kickoff_on_goal.cpp b/ateam_kenobi/src/plays/kickoff_plays/offense/kickoff_on_goal.cpp index 7f8cce097..59668e61a 100644 --- a/ateam_kenobi/src/plays/kickoff_plays/offense/kickoff_on_goal.cpp +++ b/ateam_kenobi/src/plays/kickoff_plays/offense/kickoff_on_goal.cpp @@ -59,13 +59,12 @@ void KickoffOnGoalPlay::reset() { defense_.reset(); kick_.Reset(); - multi_move_to_.Reset(); } -std::array, 16> KickoffOnGoalPlay::runFrame( +std::array, 16> KickoffOnGoalPlay::runFrame( const World & world) { - std::array, 16> motion_commands; + std::array, 16> motion_commands; auto available_robots = play_helpers::getAvailableRobots(world); play_helpers::removeGoalie(available_robots, world); @@ -108,7 +107,7 @@ std::array, 16> KickoffOnGoal motion_commands); if (enough_bots_for_supports) { - multi_move_to_.RunFrame(world, assignments.GetGroupAssignments("supports"), motion_commands); + multi_move_to_.RunFrame(assignments.GetGroupAssignments("supports"), motion_commands); } return motion_commands; diff --git a/ateam_kenobi/src/plays/kickoff_plays/offense/kickoff_on_goal.hpp b/ateam_kenobi/src/plays/kickoff_plays/offense/kickoff_on_goal.hpp index fa0744022..334cddb86 100644 --- a/ateam_kenobi/src/plays/kickoff_plays/offense/kickoff_on_goal.hpp +++ b/ateam_kenobi/src/plays/kickoff_plays/offense/kickoff_on_goal.hpp @@ -40,7 +40,7 @@ class KickoffOnGoalPlay : public stp::Play void reset() override; - std::array, 16> runFrame( + std::array, 16> runFrame( const World & world) override; private: diff --git a/ateam_kenobi/src/plays/kickoff_plays/offense/kickoff_pass_play.cpp b/ateam_kenobi/src/plays/kickoff_plays/offense/kickoff_pass_play.cpp index 10399fd35..f812f13e3 100644 --- a/ateam_kenobi/src/plays/kickoff_plays/offense/kickoff_pass_play.cpp +++ b/ateam_kenobi/src/plays/kickoff_plays/offense/kickoff_pass_play.cpp @@ -103,7 +103,6 @@ stp::PlayCompletionState KickoffPassPlay::getCompletionState() void KickoffPassPlay::enter() { defense_.reset(); - multi_move_to_.Reset(); pass_.reset(); pass_direction_chosen_ = false; } @@ -114,10 +113,10 @@ void KickoffPassPlay::exit() pass_.reset(); } -std::array, 16> KickoffPassPlay::runFrame( +std::array, 16> KickoffPassPlay::runFrame( const World & world) { - std::array, 16> maybe_motion_commands; + std::array, 16> maybe_motion_commands; std::vector current_available_robots = play_helpers::getAvailableRobots(world); play_helpers::removeGoalie(current_available_robots, world); @@ -176,9 +175,9 @@ std::array, 16> KickoffPassPl const auto & kicker = *maybe_kicker; const auto & receiver = *maybe_receiver; auto & kicker_command = - *(maybe_motion_commands[kicker.id] = ateam_msgs::msg::RobotMotionCommand{}); + *(maybe_motion_commands[kicker.id] = RobotCommand{}); auto & receiver_command = - *(maybe_motion_commands[receiver.id] = ateam_msgs::msg::RobotMotionCommand{}); + *(maybe_motion_commands[receiver.id] = RobotCommand{}); pass_.runFrame(world, kicker, receiver, kicker_command, receiver_command); } @@ -187,7 +186,7 @@ std::array, 16> KickoffPassPl if (enough_bots_for_supports) { multi_move_to_.RunFrame( - world, assignments.GetGroupAssignments("supports"), + assignments.GetGroupAssignments("supports"), maybe_motion_commands); } diff --git a/ateam_kenobi/src/plays/kickoff_plays/offense/kickoff_pass_play.hpp b/ateam_kenobi/src/plays/kickoff_plays/offense/kickoff_pass_play.hpp index e67f0a1d1..c01e32a80 100644 --- a/ateam_kenobi/src/plays/kickoff_plays/offense/kickoff_pass_play.hpp +++ b/ateam_kenobi/src/plays/kickoff_plays/offense/kickoff_pass_play.hpp @@ -43,7 +43,7 @@ class KickoffPassPlay : public stp::Play void exit() override; - std::array, + std::array, 16> runFrame(const World & world) override; void set_kickoff_ready(); diff --git a/ateam_kenobi/src/plays/kickoff_plays/offense/our_kickoff_prep_play.cpp b/ateam_kenobi/src/plays/kickoff_plays/offense/our_kickoff_prep_play.cpp index f8db7ff6e..2e0a34507 100644 --- a/ateam_kenobi/src/plays/kickoff_plays/offense/our_kickoff_prep_play.cpp +++ b/ateam_kenobi/src/plays/kickoff_plays/offense/our_kickoff_prep_play.cpp @@ -44,13 +44,12 @@ stp::PlayScore OurKickoffPrepPlay::getScore(const World & world) void OurKickoffPrepPlay::reset() { defense_.reset(); - multi_move_to_.Reset(); } -std::array, 16> OurKickoffPrepPlay::runFrame( +std::array, 16> OurKickoffPrepPlay::runFrame( const World & world) { - std::array, 16> motion_commands; + std::array, 16> motion_commands; auto available_robots = play_helpers::getAvailableRobots(world); play_helpers::removeGoalie(available_robots, world); @@ -80,7 +79,7 @@ std::array, 16> OurKickoffPre defense_.runFrame(world, assignments.GetGroupFilledAssignmentsOrEmpty("defense"), motion_commands); - multi_move_to_.RunFrame(world, assignments.GetGroupAssignments("movers"), motion_commands); + multi_move_to_.RunFrame(assignments.GetGroupAssignments("movers"), motion_commands); return motion_commands; } diff --git a/ateam_kenobi/src/plays/kickoff_plays/offense/our_kickoff_prep_play.hpp b/ateam_kenobi/src/plays/kickoff_plays/offense/our_kickoff_prep_play.hpp index 3d481517c..7d1323c3e 100644 --- a/ateam_kenobi/src/plays/kickoff_plays/offense/our_kickoff_prep_play.hpp +++ b/ateam_kenobi/src/plays/kickoff_plays/offense/our_kickoff_prep_play.hpp @@ -39,7 +39,7 @@ class OurKickoffPrepPlay : public stp::Play void reset() override; - std::array, 16> runFrame( + std::array, 16> runFrame( const World & world) override; private: From 9ef6f078299394dfedc64acf1e0a7e3ec6677c0c Mon Sep 17 00:00:00 2001 From: Matthew Barulic Date: Tue, 16 Sep 2025 23:26:55 -0400 Subject: [PATCH 24/35] Migrates passing plays to motion intents --- ateam_kenobi/CMakeLists.txt | 4 ++-- .../src/plays/passing_plays/pass_to_segment_play.cpp | 11 +++++------ .../src/plays/passing_plays/pass_to_segment_play.hpp | 2 +- 3 files changed, 8 insertions(+), 9 deletions(-) diff --git a/ateam_kenobi/CMakeLists.txt b/ateam_kenobi/CMakeLists.txt index 54847588c..1bd1b1067 100644 --- a/ateam_kenobi/CMakeLists.txt +++ b/ateam_kenobi/CMakeLists.txt @@ -58,8 +58,8 @@ target_sources(kenobi_node_component PRIVATE src/plays/kickoff_plays/offense/kickoff_on_goal.cpp src/plays/kickoff_plays/offense/kickoff_pass_play.cpp src/plays/kickoff_plays/offense/our_kickoff_prep_play.cpp - # src/plays/passing_plays/pass_to_lane_play.cpp - # src/plays/passing_plays/pass_to_segment_play.cpp + src/plays/passing_plays/pass_to_lane_play.cpp + src/plays/passing_plays/pass_to_segment_play.cpp src/plays/stop_plays/default_stop_play.cpp src/plays/stop_plays/defensive_stop_play.cpp src/plays/stop_plays/offensive_stop_play.cpp diff --git a/ateam_kenobi/src/plays/passing_plays/pass_to_segment_play.cpp b/ateam_kenobi/src/plays/passing_plays/pass_to_segment_play.cpp index ab5f39f56..27d5be2fc 100644 --- a/ateam_kenobi/src/plays/passing_plays/pass_to_segment_play.cpp +++ b/ateam_kenobi/src/plays/passing_plays/pass_to_segment_play.cpp @@ -90,15 +90,14 @@ void PassToSegmentPlay::reset() { defense_tactic_.reset(); pass_tactic_.reset(); - idler_skill_.Reset(); started_ = false; cached_target_ = ateam_geometry::Segment{}; } -std::array, 16> PassToSegmentPlay::runFrame( +std::array, 16> PassToSegmentPlay::runFrame( const World & world) { - std::array, 16> motion_commands; + std::array, 16> motion_commands; ateam_geometry::Segment target = cached_target_; @@ -157,9 +156,9 @@ std::array, 16> PassToSegment const auto maybe_receiver = assignments.GetPositionAssignment("receiver"); if (maybe_kicker && maybe_receiver) { auto & kicker_command = - *(motion_commands[maybe_kicker->id] = ateam_msgs::msg::RobotMotionCommand{}); + *(motion_commands[maybe_kicker->id] = RobotCommand{}); auto & receiver_command = - *(motion_commands[maybe_receiver->id] = ateam_msgs::msg::RobotMotionCommand{}); + *(motion_commands[maybe_receiver->id] = RobotCommand{}); if (CGAL::squared_distance(world.ball.pos, maybe_kicker->pos) < 0.5) { started_ = true; @@ -174,7 +173,7 @@ std::array, 16> PassToSegment if (enough_bots_for_idler) { assignments.RunPositionIfAssigned( "idler", [this, &world, &motion_commands](const Robot & robot) { - motion_commands[robot.id] = idler_skill_.RunFrame(world, robot); + motion_commands[robot.id] = idler_skill_.RunFrame(world); }); } diff --git a/ateam_kenobi/src/plays/passing_plays/pass_to_segment_play.hpp b/ateam_kenobi/src/plays/passing_plays/pass_to_segment_play.hpp index 36b14fae0..37babea7f 100644 --- a/ateam_kenobi/src/plays/passing_plays/pass_to_segment_play.hpp +++ b/ateam_kenobi/src/plays/passing_plays/pass_to_segment_play.hpp @@ -55,7 +55,7 @@ class PassToSegmentPlay : public stp::Play void reset() override; - std::array, + std::array, 16> runFrame(const World & world) override; private: From 5f050d99c2b780699a117f0eceabacd0bdf85e9d Mon Sep 17 00:00:00 2001 From: Matthew Barulic Date: Tue, 16 Sep 2025 23:46:50 -0400 Subject: [PATCH 25/35] Migrates test plays to motion intents --- ateam_kenobi/CMakeLists.txt | 12 ++--- .../plays/test_plays/controls_test_play.cpp | 17 ++++-- .../plays/test_plays/controls_test_play.hpp | 3 +- .../src/plays/test_plays/spinning_a_play.cpp | 35 ++++++------ .../src/plays/test_plays/spinning_a_play.hpp | 6 +-- .../plays/test_plays/test_intercept_play.cpp | 4 +- .../plays/test_plays/test_intercept_play.hpp | 2 +- .../src/plays/test_plays/test_kick_play.hpp | 4 +- .../src/plays/test_plays/test_pass_play.cpp | 4 +- .../src/plays/test_plays/test_pass_play.hpp | 2 +- .../src/plays/test_plays/test_pivot_play.hpp | 12 ++--- .../src/plays/test_plays/test_play.cpp | 17 +++--- .../src/plays/test_plays/test_play.hpp | 5 +- .../src/plays/test_plays/test_window_eval.hpp | 2 +- .../plays/test_plays/triangle_pass_play.cpp | 54 +++++++++---------- .../plays/test_plays/triangle_pass_play.hpp | 12 ++--- .../src/plays/test_plays/waypoints_play.cpp | 21 +++----- .../src/plays/test_plays/waypoints_play.hpp | 4 +- 18 files changed, 99 insertions(+), 117 deletions(-) diff --git a/ateam_kenobi/CMakeLists.txt b/ateam_kenobi/CMakeLists.txt index 1bd1b1067..70dfdfea9 100644 --- a/ateam_kenobi/CMakeLists.txt +++ b/ateam_kenobi/CMakeLists.txt @@ -64,12 +64,12 @@ target_sources(kenobi_node_component PRIVATE src/plays/stop_plays/defensive_stop_play.cpp src/plays/stop_plays/offensive_stop_play.cpp src/plays/stop_plays/stop_helpers.cpp - # src/plays/test_plays/controls_test_play.cpp - # src/plays/test_plays/spinning_a_play.cpp - # src/plays/test_plays/test_pass_play.cpp - # src/plays/test_plays/test_play.cpp - # src/plays/test_plays/triangle_pass_play.cpp - # src/plays/test_plays/waypoints_play.cpp + src/plays/test_plays/controls_test_play.cpp + src/plays/test_plays/spinning_a_play.cpp + src/plays/test_plays/test_pass_play.cpp + src/plays/test_plays/test_play.cpp + src/plays/test_plays/triangle_pass_play.cpp + src/plays/test_plays/waypoints_play.cpp src/plays/util_plays/corner_lineup_play.cpp src/plays/halt_play.cpp src/plays/kick_on_goal_play.cpp diff --git a/ateam_kenobi/src/plays/test_plays/controls_test_play.cpp b/ateam_kenobi/src/plays/test_plays/controls_test_play.cpp index c4cea0d1d..255f675d9 100644 --- a/ateam_kenobi/src/plays/test_plays/controls_test_play.cpp +++ b/ateam_kenobi/src/plays/test_plays/controls_test_play.cpp @@ -62,10 +62,10 @@ void ControlsTestPlay::reset() goal_hit_time = std::chrono::steady_clock::now(); } -std::array, 16> ControlsTestPlay::runFrame( +std::array, 16> ControlsTestPlay::runFrame( const World & world) { - std::array, 16> maybe_motion_commands; + std::array, 16> maybe_motion_commands; auto current_available_robots = play_helpers::getAvailableRobots(world); if (current_available_robots.empty()) { // No robots available to run @@ -116,10 +116,17 @@ std::array, 16> ControlsTestP motion::MotionOptions motion_options; motion_options.completion_threshold = position_threshold; - maybe_motion_commands[robot.id] = motion_controller_.get_command( + const auto command_msg = motion_controller_.get_command( robot, current_time, motion_options); + RobotCommand command; + command.motion_intent.linear = motion::intents::linear::VelocityIntent{ + ateam_geometry::Vector(command_msg.twist.linear.x, command_msg.twist.linear.y), + motion::intents::linear::Frame::World}; + command.motion_intent.angular = motion::intents::angular::VelocityIntent{command_msg.twist.angular.z}; + maybe_motion_commands[robot.id] = command; + const std::vector viz_path = path; getOverlays().drawLine("controls_test_path", viz_path, "purple"); @@ -140,8 +147,8 @@ std::array, 16> ControlsTestP getPlayInfo()["robot"]["vel"]["y"] = robot.vel.y(); getPlayInfo()["robot"]["vel"]["t"] = robot.omega; - getPlayInfo()["robot"]["cmd_vel"]["x"] = maybe_motion_commands[robot.id].value().twist.linear.x; - getPlayInfo()["robot"]["cmd_vel"]["y"] = maybe_motion_commands[robot.id].value().twist.linear.y; + getPlayInfo()["robot"]["cmd_vel"]["x"] = command_msg.twist.linear.x; + getPlayInfo()["robot"]["cmd_vel"]["y"] = command_msg.twist.linear.y; getPlayInfo()["error"]["x"] = waypoints[index].position.x() - robot.pos.x(); getPlayInfo()["error"]["y"] = waypoints[index].position.y() - robot.pos.y(); diff --git a/ateam_kenobi/src/plays/test_plays/controls_test_play.hpp b/ateam_kenobi/src/plays/test_plays/controls_test_play.hpp index 860d84aca..d063ac0ff 100644 --- a/ateam_kenobi/src/plays/test_plays/controls_test_play.hpp +++ b/ateam_kenobi/src/plays/test_plays/controls_test_play.hpp @@ -26,7 +26,6 @@ #include "core/motion/motion_controller.hpp" #include "core/stp/play.hpp" #include "ateam_geometry/types.hpp" -#include "core/play_helpers/easy_move_to.hpp" namespace ateam_kenobi::plays { @@ -39,7 +38,7 @@ class ControlsTestPlay : public stp::Play void reset() override; - std::array, + std::array, 16> runFrame(const World & world) override; private: diff --git a/ateam_kenobi/src/plays/test_plays/spinning_a_play.cpp b/ateam_kenobi/src/plays/test_plays/spinning_a_play.cpp index 1d4d21129..83ad87ef4 100644 --- a/ateam_kenobi/src/plays/test_plays/spinning_a_play.cpp +++ b/ateam_kenobi/src/plays/test_plays/spinning_a_play.cpp @@ -28,7 +28,8 @@ namespace ateam_kenobi::plays { SpinningAPlay::SpinningAPlay(stp::Options stp_options) -: stp::Play(kPlayName, stp_options) +: stp::Play(kPlayName, stp_options), + multi_move_to_(createChild("multi_move_to")) { base_shape_ = { ateam_geometry::Point(0, 0), @@ -43,15 +44,9 @@ SpinningAPlay::SpinningAPlay(stp::Options stp_options) void SpinningAPlay::reset() { angle_ = 0.0; - for (auto & emt : easy_move_tos_) { - path_planning::PlannerOptions options; - options.footprint_inflation = 0.1; - emt.setPlannerOptions(options); - emt.reset(); - } } -std::array, +std::array, 16> SpinningAPlay::runFrame(const World & world) { const auto available_robots = play_helpers::getAvailableRobots(world); @@ -61,18 +56,20 @@ std::array, CGAL::Aff_transformation_2 rotate_transform(CGAL::ROTATION, std::sin(angle_), std::cos(angle_)); - std::array, 16> motion_commands; + std::array, 16> motion_commands; - for (auto ind = 0ul; ind < num_robots; ++ind) { - const auto & robot = available_robots[ind]; - const auto point = base_shape_[ind].transform(rotate_transform); - auto & emt = easy_move_tos_[robot.id]; - emt.setTargetPosition(point); - emt.face_absolute(-M_PI_2 + angle_); - motion_commands[robot.id] = emt.runFrame(robot, world); - getOverlays().drawCircle( - "dest" + std::to_string(ind), - ateam_geometry::makeCircle(point, kRobotRadius)); + std::vector targets; + std::transform( + base_shape_.begin(), base_shape_.begin() + num_robots, std::back_inserter(targets), + [&rotate_transform](const auto & point) { + return point.transform(rotate_transform); + }); + multi_move_to_.SetTargetPoints(targets); + multi_move_to_.SetFaceAbsolue(-M_PI_2 + angle_); + multi_move_to_.RunFrame(available_robots, motion_commands); + for(auto & maybe_motion_command : motion_commands) { + if(!maybe_motion_command) continue; + maybe_motion_command->motion_intent.planner_options.footprint_inflation = 0.1; } if (angle_ < kNumRotations * 2 * M_PI) { diff --git a/ateam_kenobi/src/plays/test_plays/spinning_a_play.hpp b/ateam_kenobi/src/plays/test_plays/spinning_a_play.hpp index 73216d0e0..5a7cf6130 100644 --- a/ateam_kenobi/src/plays/test_plays/spinning_a_play.hpp +++ b/ateam_kenobi/src/plays/test_plays/spinning_a_play.hpp @@ -24,7 +24,7 @@ #include #include "core/stp/play.hpp" -#include "core/play_helpers/easy_move_to.hpp" +#include "tactics/multi_move_to.hpp" namespace ateam_kenobi::plays { @@ -38,15 +38,15 @@ class SpinningAPlay : public stp::Play void reset() override; - std::array, + std::array, 16> runFrame(const World & world) override; private: const double kAngleSpeed = 0.01; const double kNumRotations = 5; - std::array easy_move_tos_; std::vector base_shape_; double angle_; + tactics::MultiMoveTo multi_move_to_; }; } // namespace ateam_kenobi::plays diff --git a/ateam_kenobi/src/plays/test_plays/test_intercept_play.cpp b/ateam_kenobi/src/plays/test_plays/test_intercept_play.cpp index 9afd9c2e1..d4363b1fe 100644 --- a/ateam_kenobi/src/plays/test_plays/test_intercept_play.cpp +++ b/ateam_kenobi/src/plays/test_plays/test_intercept_play.cpp @@ -40,10 +40,10 @@ void TestInterceptPlay::enter() kick_skill_.Reset(); } -std::array, 16> TestInterceptPlay::runFrame( +std::array, 16> TestInterceptPlay::runFrame( const World & world) { - std::array, 16> maybe_motion_commands; + std::array, 16> maybe_motion_commands; // pass_tactic_.setTarget(targets_[target_ind_]); diff --git a/ateam_kenobi/src/plays/test_plays/test_intercept_play.hpp b/ateam_kenobi/src/plays/test_plays/test_intercept_play.hpp index dbb3c16c9..725f3fd1d 100644 --- a/ateam_kenobi/src/plays/test_plays/test_intercept_play.hpp +++ b/ateam_kenobi/src/plays/test_plays/test_intercept_play.hpp @@ -37,7 +37,7 @@ class TestInterceptPlay : public stp::Play void enter() override; - std::array, + std::array, 16> runFrame(const World & world) override; private: diff --git a/ateam_kenobi/src/plays/test_plays/test_kick_play.hpp b/ateam_kenobi/src/plays/test_plays/test_kick_play.hpp index 097f9dbb9..c8c7a62ba 100644 --- a/ateam_kenobi/src/plays/test_plays/test_kick_play.hpp +++ b/ateam_kenobi/src/plays/test_plays/test_kick_play.hpp @@ -49,10 +49,10 @@ class TestKickPlay : public stp::Play pivot_kick_skill_.Reset(); } - std::array, + std::array, 16> runFrame(const World & world) override { - std::array, 16> motion_commands; + std::array, 16> motion_commands; const auto & robots = play_helpers::getAvailableRobots(world); if (robots.empty()) { return {}; diff --git a/ateam_kenobi/src/plays/test_plays/test_pass_play.cpp b/ateam_kenobi/src/plays/test_plays/test_pass_play.cpp index f7707eb67..805fc33a6 100644 --- a/ateam_kenobi/src/plays/test_plays/test_pass_play.cpp +++ b/ateam_kenobi/src/plays/test_plays/test_pass_play.cpp @@ -42,10 +42,10 @@ void TestPassPlay::enter() pass_tactic_.reset(); } -std::array, 16> TestPassPlay::runFrame( +std::array, 16> TestPassPlay::runFrame( const World & world) { - std::array, 16> maybe_motion_commands; + std::array, 16> maybe_motion_commands; if(first_frame_) { const auto furthest_target_iter = std::max_element(targets_.begin(), targets_.end(), diff --git a/ateam_kenobi/src/plays/test_plays/test_pass_play.hpp b/ateam_kenobi/src/plays/test_plays/test_pass_play.hpp index 5da8aa26b..fcfb5ab09 100644 --- a/ateam_kenobi/src/plays/test_plays/test_pass_play.hpp +++ b/ateam_kenobi/src/plays/test_plays/test_pass_play.hpp @@ -36,7 +36,7 @@ class TestPassPlay : public stp::Play void enter() override; - std::array, + std::array, 16> runFrame(const World & world) override; private: diff --git a/ateam_kenobi/src/plays/test_plays/test_pivot_play.hpp b/ateam_kenobi/src/plays/test_plays/test_pivot_play.hpp index a8350ac98..8ed7e5b03 100644 --- a/ateam_kenobi/src/plays/test_plays/test_pivot_play.hpp +++ b/ateam_kenobi/src/plays/test_plays/test_pivot_play.hpp @@ -43,10 +43,10 @@ class TestPivotPlay : public stp::Play target_angle_ = 0.0; } - std::array, + std::array, 16> runFrame(const World & world) override { - std::array, 16> motion_commands; + std::array, 16> motion_commands; const auto & robots = play_helpers::getAvailableRobots(world); if (robots.empty()) { return {}; @@ -78,7 +78,7 @@ class TestPivotPlay : public stp::Play target_angle_ += target_step_; target_angle_ = angles::normalize_angle(target_angle_); } - motion_commands[robot.id] = ateam_msgs::msg::RobotMotionCommand(); + motion_commands[robot.id] = RobotCommand(); } else { play_info["Time At Target"] = 0.0; motion_commands[robot.id] = getPivotCommand(robot, angle_error); @@ -102,9 +102,9 @@ class TestPivotPlay : public stp::Play std::chrono::steady_clock::time_point arrival_time_; - ateam_msgs::msg::RobotMotionCommand getPivotCommand(const Robot & robot, double angle_error) + RobotCommand getPivotCommand(const Robot & robot, double angle_error) { - ateam_msgs::msg::RobotMotionCommand command; + RobotCommand command; const double vel = robot.prev_command_omega; const double dt = 0.01; @@ -153,7 +153,7 @@ class TestPivotPlay : public stp::Play command.twist.linear.x = 0.0; command.twist.linear.y = -velocity; - command.twist_frame = ateam_msgs::msg::RobotMotionCommand::FRAME_BODY; + command.twist_frame = RobotCommand::FRAME_BODY; return command; } }; diff --git a/ateam_kenobi/src/plays/test_plays/test_play.cpp b/ateam_kenobi/src/plays/test_plays/test_play.cpp index 773f2f942..6e6b4946a 100644 --- a/ateam_kenobi/src/plays/test_plays/test_play.cpp +++ b/ateam_kenobi/src/plays/test_plays/test_play.cpp @@ -30,32 +30,27 @@ TestPlay::TestPlay(stp::Options stp_options) : stp::Play(kPlayName, stp_options), goalie_skill_(createChild("goalie")) { - createIndexedChildren(easy_move_tos_, "EasyMoveTo"); } void TestPlay::reset() { - for (auto & move_to : easy_move_tos_) { - move_to.reset(); - } goalie_skill_.reset(); } -std::array, 16> TestPlay::runFrame( +std::array, 16> TestPlay::runFrame( const World & world) { - std::array, 16> maybe_motion_commands; + std::array, 16> maybe_motion_commands; auto current_available_robots = play_helpers::getAvailableRobots(world); play_helpers::removeGoalie(current_available_robots, world); if (current_available_robots.size() > 0) { const auto & robot = current_available_robots[0]; int robot_id = robot.id; - auto & easy_move_to = easy_move_tos_.at(robot_id); - - easy_move_to.setTargetPosition(world.ball.pos + ateam_geometry::Vector(-.2, 0)); - easy_move_to.face_point(world.ball.pos); - maybe_motion_commands.at(robot_id) = easy_move_to.runFrame(robot, world); + RobotCommand command; + command.motion_intent.linear = motion::intents::linear::PositionIntent{world.ball.pos + ateam_geometry::Vector(-.2, 0)}; + command.motion_intent.angular = motion::intents::angular::FacingIntent{world.ball.pos}; + maybe_motion_commands.at(robot_id) = command; } goalie_skill_.runFrame(world, maybe_motion_commands); diff --git a/ateam_kenobi/src/plays/test_plays/test_play.hpp b/ateam_kenobi/src/plays/test_plays/test_play.hpp index e6d0f50ce..fb6851cab 100644 --- a/ateam_kenobi/src/plays/test_plays/test_play.hpp +++ b/ateam_kenobi/src/plays/test_plays/test_play.hpp @@ -21,8 +21,6 @@ #ifndef PLAYS__TEST_PLAYS__TEST_PLAY_HPP_ #define PLAYS__TEST_PLAYS__TEST_PLAY_HPP_ -#include "core/path_planning/path_planner.hpp" -#include "core/motion/motion_controller.hpp" #include "core/stp/play.hpp" #include "skills/goalie.hpp" @@ -37,11 +35,10 @@ class TestPlay : public stp::Play void reset() override; - std::array, + std::array, 16> runFrame(const World & world) override; private: - std::array easy_move_tos_; skills::Goalie goalie_skill_; }; } // namespace ateam_kenobi::plays diff --git a/ateam_kenobi/src/plays/test_plays/test_window_eval.hpp b/ateam_kenobi/src/plays/test_plays/test_window_eval.hpp index 7b8f37423..005e8298d 100644 --- a/ateam_kenobi/src/plays/test_plays/test_window_eval.hpp +++ b/ateam_kenobi/src/plays/test_plays/test_window_eval.hpp @@ -43,7 +43,7 @@ class TestWindowEvalPlay : public stp::Play { } - std::array, + std::array, 16> runFrame(const World & world) override { const auto source = world.ball.pos; diff --git a/ateam_kenobi/src/plays/test_plays/triangle_pass_play.cpp b/ateam_kenobi/src/plays/test_plays/triangle_pass_play.cpp index f0989a17b..79cb4b207 100644 --- a/ateam_kenobi/src/plays/test_plays/triangle_pass_play.cpp +++ b/ateam_kenobi/src/plays/test_plays/triangle_pass_play.cpp @@ -34,7 +34,6 @@ TrianglePassPlay::TrianglePassPlay(stp::Options stp_options) : stp::Play(kPlayName, stp_options), pass_tactic_(createChild("pass")) { - createIndexedChildren(easy_move_tos_, "EasyMoveTo"); positions.emplace_back(0.85, 0); const auto angle = angles::from_degrees(120); CGAL::Aff_transformation_2 rotate_transform(CGAL::ROTATION, std::sin( @@ -50,15 +49,12 @@ void TrianglePassPlay::reset() { state_ = State::Setup; kick_target_ind_ = 0; - for (auto & e : easy_move_tos_) { - e.reset(); - } } -std::array, 16> TrianglePassPlay::runFrame( +std::array, 16> TrianglePassPlay::runFrame( const World & world) { - std::array, 16> maybe_motion_commands; + std::array, 16> maybe_motion_commands; auto available_robots = play_helpers::getAvailableRobots(world); @@ -96,7 +92,7 @@ std::array, 16> TrianglePassP for (auto ind = 0ul; ind < maybe_motion_commands.size(); ++ind) { auto & motion_command = maybe_motion_commands[ind]; if (!motion_command) { - motion_command = ateam_msgs::msg::RobotMotionCommand{}; + motion_command = RobotCommand{}; } } @@ -119,7 +115,7 @@ bool TrianglePassPlay::isReady(const World & world) void TrianglePassPlay::runSetup( const std::vector & robots, const World & world, - std::array, + std::array, 16> & motion_commands) { if (isReady(world)) { @@ -130,17 +126,17 @@ void TrianglePassPlay::runSetup( for (auto pos_ind = 0ul; pos_ind < positions.size(); ++pos_ind) { const auto & position = positions[pos_ind]; const auto & robot = robots[pos_ind]; - auto & emt = easy_move_tos_[robot.id]; - emt.setTargetPosition(position); - emt.face_travel(); - motion_commands[robot.id] = emt.runFrame(robot, world); + RobotCommand command; + command.motion_intent.linear = motion::intents::linear::PositionIntent{position}; + command.motion_intent.angular = motion::intents::angular::FaceTravelIntent{}; + motion_commands[robot.id] = command; } } void TrianglePassPlay::runPassing( const std::vector & robots, const World & world, - std::array, + std::array, 16> & motion_commands) { if (pass_tactic_.isDone()) { @@ -159,21 +155,22 @@ void TrianglePassPlay::runPassing( getPlayInfo()["Kicker"] = std::to_string(kicker_bot.id); getPlayInfo()["Receiver"] = std::to_string(receiver_bot.id); - auto & kicker_command = *(motion_commands[kicker_bot.id] = ateam_msgs::msg::RobotMotionCommand{}); + auto & kicker_command = *(motion_commands[kicker_bot.id] = RobotCommand{}); auto & receiver_command = - *(motion_commands[receiver_bot.id] = ateam_msgs::msg::RobotMotionCommand{}); + *(motion_commands[receiver_bot.id] = RobotCommand{}); pass_tactic_.runFrame(world, kicker_bot, receiver_bot, kicker_command, receiver_command); - auto & idler_emt = easy_move_tos_[idler_bot.id]; - idler_emt.setTargetPosition(positions[idler_index]); - idler_emt.face_point(world.ball.pos); - motion_commands[idler_bot.id] = idler_emt.runFrame(idler_bot, world); + RobotCommand command; + command.motion_intent.linear = motion::intents::linear::PositionIntent{ + positions[idler_index]}; + command.motion_intent.angular = motion::intents::angular::FacingIntent{world.ball.pos}; + motion_commands[idler_bot.id] = command; } void TrianglePassPlay::runBackOff( const std::vector & available_robots, const World & world, - std::array, + std::array, 16> & motion_commands) { auto byDistToBall = [&world](const Robot & lhs, const Robot & rhs) { @@ -197,26 +194,25 @@ void TrianglePassPlay::runBackOff( pass_tactic_.reset(); } + RobotCommand command; const auto vel = ateam_geometry::normalize(ball_to_bot_vec) * 0.25; - auto & motion_command = motion_commands[receiver_robot.id] = - ateam_msgs::msg::RobotMotionCommand{}; - motion_command->twist.linear.x = vel.x(); - motion_command->twist.linear.y = vel.y(); + command.motion_intent.linear = motion::intents::linear::VelocityIntent{vel}; + motion_commands[receiver_robot.id] = command; } void TrianglePassPlay::runIdlers( const std::vector & robots, const World & world, - std::array, 16> & motion_commands) + std::array, 16> & motion_commands) { const ateam_geometry::Point first_pos(-world.field.field_length / 2.0, -world.field.field_width / 2.0); const ateam_geometry::Vector step(kRobotDiameter * 1.5, 0); for( auto i = 0ul; i < robots.size(); ++i) { auto & robot = robots[i]; - auto & emt = easy_move_tos_[robot.id]; - emt.setTargetPosition(first_pos + step * i); - emt.face_absolute(0.0); - motion_commands[robot.id] = emt.runFrame(robot, world); + RobotCommand command; + command.motion_intent.linear = motion::intents::linear::PositionIntent{first_pos + step * i}; + command.motion_intent.angular = motion::intents::angular::HeadingIntent{0.0}; + motion_commands[robot.id] = command; } } diff --git a/ateam_kenobi/src/plays/test_plays/triangle_pass_play.hpp b/ateam_kenobi/src/plays/test_plays/triangle_pass_play.hpp index 98941004b..c3ee11bde 100644 --- a/ateam_kenobi/src/plays/test_plays/triangle_pass_play.hpp +++ b/ateam_kenobi/src/plays/test_plays/triangle_pass_play.hpp @@ -24,7 +24,6 @@ #include #include "core/stp/play.hpp" -#include "core/play_helpers/easy_move_to.hpp" #include "tactics/pass.hpp" namespace ateam_kenobi::plays @@ -39,13 +38,12 @@ class TrianglePassPlay : public stp::Play void reset() override; - std::array, + std::array, 16> runFrame(const World & world) override; private: static constexpr double kKickSpeed = 3.0; tactics::Pass pass_tactic_; - std::array easy_move_tos_; std::vector positions; std::size_t kick_target_ind_ = 0; @@ -60,23 +58,23 @@ class TrianglePassPlay : public stp::Play void runSetup( const std::vector & robots, - const World & world, std::array, + const World & world, std::array, 16> & motion_commands); void runPassing( const std::vector & robots, const World & world, - std::array, + std::array, 16> & motion_commands); void runBackOff( const std::vector & available_robots, const World & world, - std::array, + std::array, 16> & motion_commands); void runIdlers( const std::vector & robots, const World & world, - std::array, 16> & motion_commands); + std::array, 16> & motion_commands); }; } // namespace ateam_kenobi::plays diff --git a/ateam_kenobi/src/plays/test_plays/waypoints_play.cpp b/ateam_kenobi/src/plays/test_plays/waypoints_play.cpp index d280be39e..ddacc59f1 100644 --- a/ateam_kenobi/src/plays/test_plays/waypoints_play.cpp +++ b/ateam_kenobi/src/plays/test_plays/waypoints_play.cpp @@ -31,7 +31,6 @@ namespace ateam_kenobi::plays WaypointsPlay::WaypointsPlay(stp::Options stp_options) : stp::Play(kPlayName, stp_options) { - createIndexedChildren(easy_move_tos_, "EasyMoveTo"); addWaypoint( 5000, { {0.00, 0, M_PI}, @@ -58,16 +57,9 @@ void WaypointsPlay::reset() waypoint_index_ = 0; next_transition_time_ = std::chrono::steady_clock::now() + std::chrono::milliseconds( waypoints_.front().duration_ms); - path_planning::PlannerOptions planner_options; - planner_options.use_default_obstacles = false; - planner_options.footprint_inflation = 0.02; - for (auto & emt : easy_move_tos_) { - emt.reset(); - emt.setPlannerOptions(planner_options); - } } -std::array, 16> WaypointsPlay::runFrame( +std::array, 16> WaypointsPlay::runFrame( const World & world) { const auto now = std::chrono::steady_clock::now(); @@ -85,14 +77,17 @@ std::array, 16> WaypointsPlay const auto num_robots = std::min(waypoint.poses.size(), available_robots.size()); - std::array, 16> motion_commands; + std::array, 16> motion_commands; for (auto robot_ind = 0ul; robot_ind < num_robots; ++robot_ind) { const auto & pose = waypoint.poses[robot_ind]; const auto & robot = available_robots[robot_ind]; - easy_move_tos_[robot.id].setTargetPosition(pose.position); - easy_move_tos_[robot.id].face_absolute(pose.heading); - motion_commands[robot.id] = easy_move_tos_[robot.id].runFrame(robot, world); + RobotCommand command; + command.motion_intent.linear = motion::intents::linear::PositionIntent{pose.position}; + command.motion_intent.angular = motion::intents::angular::HeadingIntent{pose.heading}; + command.motion_intent.planner_options.footprint_inflation = 0.02; + command.motion_intent.planner_options.use_default_obstacles = false; + motion_commands[robot.id] = command; } return motion_commands; diff --git a/ateam_kenobi/src/plays/test_plays/waypoints_play.hpp b/ateam_kenobi/src/plays/test_plays/waypoints_play.hpp index 1e7a95638..0b80dcd62 100644 --- a/ateam_kenobi/src/plays/test_plays/waypoints_play.hpp +++ b/ateam_kenobi/src/plays/test_plays/waypoints_play.hpp @@ -29,7 +29,6 @@ #include #include #include "core/stp/play.hpp" -#include "core/play_helpers/easy_move_to.hpp" namespace ateam_kenobi::plays { @@ -43,7 +42,7 @@ class WaypointsPlay : public stp::Play void reset() override; - std::array, + std::array, 16> runFrame(const World & world) override; private: @@ -59,7 +58,6 @@ class WaypointsPlay : public stp::Play int64_t duration_ms; }; - std::array easy_move_tos_; std::vector waypoints_; std::chrono::steady_clock::time_point next_transition_time_ = std::chrono::steady_clock::time_point::max(); From dafb859bd5da402b390dc71980cd5fd17c25205a Mon Sep 17 00:00:00 2001 From: Matthew Barulic Date: Wed, 17 Sep 2025 00:41:07 -0400 Subject: [PATCH 26/35] Moves motion limits to MotionOptions --- .../src/core/motion/motion_controller.cpp | 50 ++++++++++--------- .../src/core/motion/motion_controller.hpp | 15 ++---- .../src/core/motion/motion_options.hpp | 9 ++++ .../src/core/play_helpers/easy_move_to.cpp | 12 ++--- .../src/plays/our_ball_placement_play.cpp | 18 +++---- ateam_kenobi/src/plays/our_penalty_play.cpp | 7 ++- .../plays/stop_plays/default_stop_play.cpp | 5 +- .../plays/stop_plays/defensive_stop_play.cpp | 5 +- .../plays/stop_plays/offensive_stop_play.cpp | 5 +- .../plays/test_plays/controls_test_play.cpp | 9 ++-- ateam_kenobi/src/plays/their_penalty_play.cpp | 5 +- ateam_kenobi/src/skills/capture.cpp | 5 +- ateam_kenobi/src/skills/dribble.cpp | 6 +-- ateam_kenobi/src/skills/extract.cpp | 6 +-- ateam_kenobi/src/skills/line_kick.cpp | 5 +- ateam_kenobi/src/skills/pass_receiver.cpp | 6 +-- 16 files changed, 91 insertions(+), 77 deletions(-) diff --git a/ateam_kenobi/src/core/motion/motion_controller.cpp b/ateam_kenobi/src/core/motion/motion_controller.cpp index fea6e541a..ace1e3d61 100644 --- a/ateam_kenobi/src/core/motion/motion_controller.cpp +++ b/ateam_kenobi/src/core/motion/motion_controller.cpp @@ -89,7 +89,7 @@ void MotionController::reset_trajectory( } } -void MotionController::calculate_trajectory_velocity_limits() +void MotionController::calculate_trajectory_velocity_limits(const MotionOptions & options) { trajectory_velocity_limits.reserve(trajectory.size()); @@ -111,26 +111,27 @@ void MotionController::calculate_trajectory_velocity_limits() // Max velocity robot could linearly decelerate to the next velocity from double distance = ateam_geometry::norm(next_point - point); double max_decel_velocity = sqrt(std::pow(ateam_geometry::norm(next_vel), - 2) + 2 * decel_limit * distance); + 2) + 2 * options.max_deceleration * distance); // Max velocity robot can make turn (also have to account for next_vel being (0,0)) - double max_turn_velocity = v_max; + double max_turn_velocity = options.max_velocity; if (i > 0) { const ateam_geometry::Point prev_point = trajectory[i - 1]; const ateam_geometry::Vector prev_direction = point - prev_point; double angle = ateam_geometry::ShortestAngleBetween(direction, prev_direction); - max_turn_velocity = (abs(angle) > max_allowed_turn_angle) ? 0.5 : v_max; + max_turn_velocity = (abs(angle) > options.max_allowed_turn_angle) ? 0.5 : options.max_velocity; } double selected_velocity = std::clamp(std::min(max_decel_velocity, max_turn_velocity), - 0.0, v_max); + 0.0, options.max_velocity); trajectory_velocity_limits[i] = selected_velocity * direction; } } double MotionController::calculate_trapezoidal_velocity( + const MotionOptions & options, const ateam_kenobi::Robot & robot, ateam_geometry::Point target, size_t target_index, double dt) { @@ -160,21 +161,22 @@ double MotionController::calculate_trapezoidal_velocity( (2 * distance_to_next_trajectory_point); // Cruise - double trapezoidal_vel = this->v_max; + double trapezoidal_vel = options.max_velocity; // Decelerate to target velocity - if (deceleration_to_reach_target > decel_limit * 0.95) { + if (deceleration_to_reach_target > options.max_deceleration * 0.95) { trapezoidal_vel = vel - (deceleration_to_reach_target * dt); // Accelerate to speed - } else if (vel < this->v_max) { - trapezoidal_vel = vel + (accel_limit * dt); + } else if (vel < options.max_velocity) { + trapezoidal_vel = vel + (options.max_acceleration * dt); } - return std::clamp(trapezoidal_vel, -this->v_max, this->v_max); + return std::clamp(trapezoidal_vel, -options.max_velocity, options.max_velocity); } double MotionController::calculate_trapezoidal_angular_vel( + const MotionOptions & options, const ateam_kenobi::Robot & robot, double target_angle, double dt) { @@ -187,17 +189,17 @@ double MotionController::calculate_trapezoidal_angular_vel( double deceleration_to_reach_target = (vel * vel) / (2 * angle_error); // Cruise - double trapezoidal_vel = std::copysign(t_max, angle_error); + double trapezoidal_vel = std::copysign(options.max_angular_velocity, angle_error); const double error_direction = std::copysign(1, angle_error); const double decel_direction = std::copysign(1, vel * angle_error); // Decelerate to target velocity - if (decel_direction > 0 && abs(deceleration_to_reach_target) > t_accel_limit * 0.95) { + if (decel_direction > 0 && abs(deceleration_to_reach_target) > options.max_angular_acceleration * 0.95) { trapezoidal_vel = vel - (error_direction * deceleration_to_reach_target * dt); // Accelerate to speed - } else if (abs(vel) < t_max) { - trapezoidal_vel = vel + (error_direction * t_accel_limit * dt); + } else if (abs(vel) < options.max_angular_velocity) { + trapezoidal_vel = vel + (error_direction * options.max_angular_acceleration * dt); } // const auto min_angular_vel = 1.0; @@ -205,7 +207,7 @@ double MotionController::calculate_trapezoidal_angular_vel( // trapezoidal_vel = std::copysign(min_angular_vel, angle_error); // } - return std::clamp(trapezoidal_vel, -t_max, t_max); + return std::clamp(trapezoidal_vel, -options.max_angular_velocity, options.max_angular_velocity); } ateam_msgs::msg::RobotMotionCommand MotionController::get_command( @@ -235,7 +237,7 @@ ateam_msgs::msg::RobotMotionCommand MotionController::get_command( ateam_geometry::Point target = this->trajectory[target_index]; ateam_geometry::Vector target_direction = ateam_geometry::normalize(target - robot.pos); - const double lookahead_distance = this->v_max * dt; + const double lookahead_distance = options.max_velocity * dt; const auto lookahead = ateam_geometry::makeCircle(robot.pos, lookahead_distance); u_int64_t closest_index = this->trajectory.size() - 1; @@ -336,8 +338,8 @@ ateam_msgs::msg::RobotMotionCommand MotionController::get_command( const auto world_feedback = body_feedback.transform(transformation.inverse()); // Calculate trapezoidal velocity feedforward - calculate_trajectory_velocity_limits(); - double calculated_velocity = calculate_trapezoidal_velocity(robot, target, target_index, dt); + calculate_trajectory_velocity_limits(options); + double calculated_velocity = calculate_trapezoidal_velocity(options, robot, target, target_index, dt); ateam_geometry::Vector vel_vector; @@ -352,8 +354,8 @@ ateam_msgs::msg::RobotMotionCommand MotionController::get_command( } // clamp to max/min velocity - if (ateam_geometry::norm(vel_vector) > this->v_max) { - vel_vector = this->v_max * ateam_geometry::normalize(vel_vector); + if (ateam_geometry::norm(vel_vector) > options.max_velocity) { + vel_vector = options.max_velocity * ateam_geometry::normalize(vel_vector); } motion_command.twist.linear.x = vel_vector.x(); @@ -395,20 +397,20 @@ ateam_msgs::msg::RobotMotionCommand MotionController::get_command( if(std::abs(t_error) < M_PI / 12.0) { t_command = this->t_controller.compute_command(t_error, dt); } else { - t_command = this->calculate_trapezoidal_angular_vel(robot, target_angle, dt); + t_command = this->calculate_trapezoidal_angular_vel(options, robot, target_angle, dt); } if (trajectory_complete) { double theta_min = 0.0; if (abs(t_command) < theta_min) { if (t_command > 0) { - t_command = std::clamp(t_command, theta_min, this->t_max); + t_command = std::clamp(t_command, theta_min, options.max_angular_velocity); } else { - t_command = std::clamp(t_command, -theta_min, -this->t_max); + t_command = std::clamp(t_command, -theta_min, -options.max_angular_velocity); } } } - motion_command.twist.angular.z = std::clamp(t_command, -this->t_max, this->t_max); + motion_command.twist.angular.z = std::clamp(t_command, -options.max_angular_velocity, options.max_angular_velocity); } } diff --git a/ateam_kenobi/src/core/motion/motion_controller.hpp b/ateam_kenobi/src/core/motion/motion_controller.hpp index 05ee03db2..c4fd863f2 100644 --- a/ateam_kenobi/src/core/motion/motion_controller.hpp +++ b/ateam_kenobi/src/core/motion/motion_controller.hpp @@ -66,15 +66,17 @@ class MotionController void face_travel(); void no_face(); - void calculate_trajectory_velocity_limits(); + void calculate_trajectory_velocity_limits(const MotionOptions & options); double calculate_trapezoidal_velocity( + const MotionOptions & options, const ateam_kenobi::Robot & robot, ateam_geometry::Point target, size_t target_index, double dt); double calculate_trapezoidal_angular_vel( + const MotionOptions & options, const ateam_kenobi::Robot & robot, double target_angle, double dt); @@ -93,17 +95,6 @@ class MotionController void set_y_pid_gains(double p, double i, double d, double i_max, double i_min); void set_t_pid_gains(double p, double i, double d, double i_max, double i_min); - // Velocity limits - double v_max = 2.0; - double t_max = 3.0; - - // Acceleration limits - double accel_limit = 1.5; - double decel_limit = 1.5; - double t_accel_limit = 5.0; - - double max_allowed_turn_angle = M_PI / 4.0; - double face_angle = 0; std::optional face_towards; diff --git a/ateam_kenobi/src/core/motion/motion_options.hpp b/ateam_kenobi/src/core/motion/motion_options.hpp index 562adfb0a..48bb69bbe 100644 --- a/ateam_kenobi/src/core/motion/motion_options.hpp +++ b/ateam_kenobi/src/core/motion/motion_options.hpp @@ -31,6 +31,15 @@ struct MotionOptions /// @brief angle around the end point that will be considered completed double angular_completion_threshold = 0.035; // radians + + double max_velocity = 1.0; // m/s + double max_acceleration = 2.0; // m/s^2 + double max_deceleration = 2.0; // m/s^2 + + double max_angular_velocity = 3.0; // rad/s + double max_angular_acceleration = 4.0; // rad/s^2 + + double max_allowed_turn_angle = M_PI / 4.0; // radians }; } // namespace ateam_kenobi::motion diff --git a/ateam_kenobi/src/core/play_helpers/easy_move_to.cpp b/ateam_kenobi/src/core/play_helpers/easy_move_to.cpp index 7d38d8521..ca665b40f 100644 --- a/ateam_kenobi/src/core/play_helpers/easy_move_to.cpp +++ b/ateam_kenobi/src/core/play_helpers/easy_move_to.cpp @@ -128,7 +128,7 @@ void EasyMoveTo::setMaxVelocity(double velocity) RCLCPP_WARN(getLogger(), "UNREASONABLY LARGE VELOCITY GIVEN TO SET MAX VELOCITY"); return; } - motion_controller_.v_max = velocity; + motion_options_.max_velocity = velocity; } void EasyMoveTo::setMaxAngularVelocity(double velocity) @@ -137,7 +137,7 @@ void EasyMoveTo::setMaxAngularVelocity(double velocity) RCLCPP_WARN(getLogger(), "UNREASONABLY LARGE VELOCITY GIVEN TO SET MAX ANGULAR VELOCITY"); return; } - motion_controller_.t_max = velocity; + motion_options_.max_angular_velocity = velocity; } void EasyMoveTo::setMaxAccel(double accel) @@ -146,7 +146,7 @@ void EasyMoveTo::setMaxAccel(double accel) RCLCPP_WARN(getLogger(), "UNREASONABLY LARGE ACCELERATION GIVEN TO SET MAX ACCELERATION"); return; } - motion_controller_.accel_limit = accel; + motion_options_.max_acceleration = accel; } void EasyMoveTo::setMaxDecel(double decel) @@ -155,7 +155,7 @@ void EasyMoveTo::setMaxDecel(double decel) RCLCPP_WARN(getLogger(), "UNREASONABLY LARGE DECELERATION GIVEN TO SET MAX DECELERATION"); return; } - motion_controller_.decel_limit = decel; + motion_options_.max_deceleration = decel; } void EasyMoveTo::setMaxThetaAccel(double accel) @@ -164,12 +164,12 @@ void EasyMoveTo::setMaxThetaAccel(double accel) RCLCPP_WARN(getLogger(), "UNREASONABLY LARGE ACCELERATION GIVEN TO SET MAX THETA ACCELERATION"); return; } - motion_controller_.t_accel_limit = accel; + motion_options_.max_angular_acceleration = accel; } void EasyMoveTo::setMaxAllowedTurnAngle(double angle) { - motion_controller_.max_allowed_turn_angle = angle; + motion_options_.max_allowed_turn_angle = angle; } ateam_msgs::msg::RobotMotionCommand EasyMoveTo::runFrame( diff --git a/ateam_kenobi/src/plays/our_ball_placement_play.cpp b/ateam_kenobi/src/plays/our_ball_placement_play.cpp index b6d731e6d..4e32adcc0 100644 --- a/ateam_kenobi/src/plays/our_ball_placement_play.cpp +++ b/ateam_kenobi/src/plays/our_ball_placement_play.cpp @@ -287,9 +287,9 @@ void OurBallPlacementPlay::runExtracting( motion_command.motion_intent.planner_options.footprint_inflation = -0.9 * kRobotRadius; motion_command.motion_intent.linear = motion::intents::linear::PositionIntent{approach_point_}; - // TODO(barulicm): set max velocity to 0.2 - // TODO(barulicm): set max accel to 1.0 - // TODO(barulicm): set max decel to 1.0 + motion_command.motion_intent.motion_options.max_velocity = 0.2; + motion_command.motion_intent.motion_options.max_acceleration = 1.0; + motion_command.motion_intent.motion_options.max_deceleration = 1.0; if (world.ball.visible) { motion_command.motion_intent.angular = @@ -307,9 +307,9 @@ void OurBallPlacementPlay::runExtracting( motion_command.motion_intent.planner_options.avoid_ball = false; motion_command.motion_intent.planner_options.footprint_inflation = -0.9 * kRobotRadius; - // TODO(barulicm): set max velocity to 0.35 - // TODO(barulicm): set max accel to 2.0 - // TODO(barulicm): set max decel to 2.0 + motion_command.motion_intent.motion_options.max_velocity = 0.35; + motion_command.motion_intent.motion_options.max_acceleration = 2.0; + motion_command.motion_intent.motion_options.max_deceleration = 2.0; if (world.ball.visible) { motion_command.motion_intent.angular = motion::intents::angular::FacingIntent{world.ball.pos}; @@ -324,9 +324,9 @@ void OurBallPlacementPlay::runExtracting( motion::intents::linear::Frame::Local}; } else { getPlayInfo()["ExtractState"] = "moving to approach point"; - // TODO(barulicm): Set max velocity to 1.5 - // TODO(barulicm): Set max accel to 2.0 - // TODO(barulicm): Set max decel to 2.0 + motion_command.motion_intent.motion_options.max_velocity = 1.5; + motion_command.motion_intent.motion_options.max_acceleration = 2.0; + motion_command.motion_intent.motion_options.max_deceleration = 2.0; motion_command.motion_intent.linear = motion::intents::linear::PositionIntent{approach_point_}; motion_command.motion_intent.angular = motion::intents::angular::HeadingIntent{ateam_geometry::ToHeading(world.ball.pos - approach_point_)}; diff --git a/ateam_kenobi/src/plays/our_penalty_play.cpp b/ateam_kenobi/src/plays/our_penalty_play.cpp index 871d18aa8..65552b32d 100644 --- a/ateam_kenobi/src/plays/our_penalty_play.cpp +++ b/ateam_kenobi/src/plays/our_penalty_play.cpp @@ -88,7 +88,7 @@ std::array, 16> OurPenaltyPlay::runFrame( RobotCommand command; command.motion_intent.linear = motion::intents::linear::PositionIntent{destination}; command.motion_intent.angular = motion::intents::angular::FacingIntent{world.ball.pos}; - // TODO(barulicm): Set max velocity to 1.5 + command.motion_intent.motion_options.max_velocity = 1.5; motion_commands[kicking_robot.id] = command; } else { // Kick ball @@ -116,7 +116,10 @@ std::array, 16> OurPenaltyPlay::runFrame( }); multi_move_to_.SetTargetPoints(target_points); multi_move_to_.SetFaceTravel(); - // TODO(barulicm): Set max velocity to 1.5 + for(auto & maybe_cmd : motion_commands) { + if(!maybe_cmd) continue; + maybe_cmd->motion_intent.motion_options.max_velocity = 1.5; + } multi_move_to_.RunFrame(available_robots, motion_commands); return motion_commands; diff --git a/ateam_kenobi/src/plays/stop_plays/default_stop_play.cpp b/ateam_kenobi/src/plays/stop_plays/default_stop_play.cpp index e65089e9a..8f2ff7932 100644 --- a/ateam_kenobi/src/plays/stop_plays/default_stop_play.cpp +++ b/ateam_kenobi/src/plays/stop_plays/default_stop_play.cpp @@ -72,7 +72,10 @@ std::array, 16> DefaultStopPlay::runFrame( [](const auto & o) {return !o;}, std::make_optional(RobotCommand{})); // Rules say <1.5m/s. We'll use 1m/s to give some room for error. - // TODO(barulicm): Set max velocity to 1.0 + for(auto & maybe_cmd : motion_commands) { + if(!maybe_cmd) continue; + maybe_cmd->motion_intent.motion_options.max_velocity = 1.0; + } return motion_commands; } diff --git a/ateam_kenobi/src/plays/stop_plays/defensive_stop_play.cpp b/ateam_kenobi/src/plays/stop_plays/defensive_stop_play.cpp index 0311a2dae..ce3e984b5 100644 --- a/ateam_kenobi/src/plays/stop_plays/defensive_stop_play.cpp +++ b/ateam_kenobi/src/plays/stop_plays/defensive_stop_play.cpp @@ -83,7 +83,10 @@ std::array, 16> DefensiveStopPlay::runFrame( [](const auto & o) {return !o;}, std::make_optional(RobotCommand{})); // Rules say <1.5m/s. We'll use 1m/s to give some room for error. - // TODO(barulicm): set max velocity to 1.0 + for(auto & maybe_cmd : motion_commands) { + if(!maybe_cmd) continue; + maybe_cmd->motion_intent.motion_options.max_velocity = 1.0; + } return motion_commands; } diff --git a/ateam_kenobi/src/plays/stop_plays/offensive_stop_play.cpp b/ateam_kenobi/src/plays/stop_plays/offensive_stop_play.cpp index 3fe77daac..d4937674f 100644 --- a/ateam_kenobi/src/plays/stop_plays/offensive_stop_play.cpp +++ b/ateam_kenobi/src/plays/stop_plays/offensive_stop_play.cpp @@ -84,7 +84,10 @@ std::array, 16> OffensiveStopPlay::runFrame( [](const auto & o) {return !o;}, std::make_optional(RobotCommand{})); // Rules say <1.5m/s. We'll use 1m/s to give some room for error. - // TODO(barulicm): Set max velocity to 1.0 + for(auto & maybe_cmd : motion_commands) { + if(!maybe_cmd) continue; + maybe_cmd->motion_intent.motion_options.max_velocity = 1.0; + } return motion_commands; } diff --git a/ateam_kenobi/src/plays/test_plays/controls_test_play.cpp b/ateam_kenobi/src/plays/test_plays/controls_test_play.cpp index 255f675d9..e6de0cc2c 100644 --- a/ateam_kenobi/src/plays/test_plays/controls_test_play.cpp +++ b/ateam_kenobi/src/plays/test_plays/controls_test_play.cpp @@ -48,11 +48,6 @@ ControlsTestPlay::ControlsTestPlay(stp::Options stp_options) // {ateam_geometry::Point(-0.5, -kRobotRadius), AngleMode::face_absolute, M_PI / 2, 3.0}, // {ateam_geometry::Point(-0.5, kRobotRadius), AngleMode::face_absolute, M_PI / 2, 3.0} // }; - - motion_controller_.v_max = 2.0; - motion_controller_.t_max = 20.0; - motion_controller_.accel_limit = 2.0; - motion_controller_.decel_limit = 2.0; } void ControlsTestPlay::reset() @@ -115,6 +110,10 @@ std::array, 16> ControlsTestPlay::runFrame( world.current_time.time_since_epoch()).count(); motion::MotionOptions motion_options; + motion_options.max_velocity = 2.0; + motion_options.max_angular_velocity = 20.0; + motion_options.max_acceleration = 2.0; + motion_options.max_deceleration = 2.0; motion_options.completion_threshold = position_threshold; const auto command_msg = motion_controller_.get_command( robot, current_time, diff --git a/ateam_kenobi/src/plays/their_penalty_play.cpp b/ateam_kenobi/src/plays/their_penalty_play.cpp index 118e181bf..94288f683 100644 --- a/ateam_kenobi/src/plays/their_penalty_play.cpp +++ b/ateam_kenobi/src/plays/their_penalty_play.cpp @@ -77,9 +77,12 @@ std::array, 16> TheirPenaltyPlay::runFrame( return current; }); multi_move_to_.SetTargetPoints(target_points); - // multi_move_to_.SetMaxVelocity(1.5); // TODO(barulicm) multi_move_to_.SetFaceTravel(); multi_move_to_.RunFrame(available_robots, motion_commands); + for(auto & maybe_cmd : motion_commands) { + if(!maybe_cmd) continue; + maybe_cmd->motion_intent.motion_options.max_velocity = 1.5; + } return motion_commands; } diff --git a/ateam_kenobi/src/skills/capture.cpp b/ateam_kenobi/src/skills/capture.cpp index 5ea6e2fb2..325f184da 100644 --- a/ateam_kenobi/src/skills/capture.cpp +++ b/ateam_kenobi/src/skills/capture.cpp @@ -89,8 +89,7 @@ RobotCommand Capture::runMoveToBall( const auto max_decel_vel = std::sqrt((2.0 * decel_limit_ * decel_distance) + (capture_speed_ * capture_speed_)); - // TODO(barulicm): Set max velocity to std::min(max_decel_vel, max_speed_) - (void)max_decel_vel; + command.motion_intent.motion_options.max_velocity = std::min(max_decel_vel, max_speed_); return command; } @@ -116,7 +115,7 @@ RobotCommand Capture::runCapture(const World & world, const Robot & robot) command.motion_intent.planner_options.footprint_inflation = 0.0; command.motion_intent.motion_options.completion_threshold = 0.0; - // TODO(barulicm): Set max velocity to capture_speed_ + command.motion_intent.motion_options.max_velocity = capture_speed_; if(world.ball.visible) { command.motion_intent.angular = motion::intents::angular::FacingIntent{world.ball.pos}; diff --git a/ateam_kenobi/src/skills/dribble.cpp b/ateam_kenobi/src/skills/dribble.cpp index 95b493bf2..b7afbfc6a 100644 --- a/ateam_kenobi/src/skills/dribble.cpp +++ b/ateam_kenobi/src/skills/dribble.cpp @@ -192,7 +192,7 @@ RobotCommand Dribble::runMoveBehindBall( command.motion_intent.planner_options.draw_obstacles = true; command.motion_intent.planner_options.ignore_start_obstacle = false; - // TODO(barulicm): Set max velocity to 1.5 + command.motion_intent.motion_options.max_velocity = 1.5; if (ateam_geometry::norm(robot.pos - world.ball.pos) < 0.5) { command.dribbler_speed = 130; @@ -215,7 +215,7 @@ RobotCommand Dribble::runDribble(const Robot & robot) const auto position_target = target_ - (kRobotRadius * ateam_geometry::normalize(robot_to_target)); command.motion_intent.linear = motion::intents::linear::PositionIntent{position_target}; - // TODO(barulicm): Set max velocity to 0.35 + command.motion_intent.motion_options.max_velocity = 0.35; command.dribbler_speed = 130; @@ -233,7 +233,7 @@ RobotCommand Dribble::runBackAway(const World & world, const Robot & robot) command.dribbler_speed = 0; - // TODO(barulicm): Set max velocity to 0.35 + command.motion_intent.motion_options.max_velocity = 0.35; // Wait for the dribbler to wind down before moving if ((std::chrono::steady_clock::now() - back_away_duration_.value()) > back_away_start_) { diff --git a/ateam_kenobi/src/skills/extract.cpp b/ateam_kenobi/src/skills/extract.cpp index bc6a76029..8c6ac13fc 100644 --- a/ateam_kenobi/src/skills/extract.cpp +++ b/ateam_kenobi/src/skills/extract.cpp @@ -60,16 +60,16 @@ RobotCommand Extract::RunFrame(const World & world, const Robot & robot) command.motion_intent.linear = motion::intents::linear::PositionIntent{ robot.pos + ateam_geometry::normalize(world.ball.pos - robot.pos) * 0.25 }; - // TODO(barulicm): Set max angular velocity to 2.0 + command.motion_intent.motion_options.max_angular_velocity = 2.0; } else if (ball_far) { command.motion_intent.planner_options.avoid_ball = false; command.motion_intent.linear = motion::intents::linear::PositionIntent{world.ball.pos}; - // TODO(barulicm): Set max angular velocity to 2.0 + command.motion_intent.motion_options.max_angular_velocity = 2.0; } else { command.motion_intent.planner_options.avoid_ball = false; command.motion_intent.planner_options.footprint_inflation = -0.1; command.motion_intent.linear = motion::intents::linear::PositionIntent{world.ball.pos}; - // TODO(barulicm): Set max velocity to 0.35 + command.motion_intent.motion_options.max_velocity = 0.35; } command.motion_intent.angular = motion::intents::angular::FacingIntent{world.ball.pos}; diff --git a/ateam_kenobi/src/skills/line_kick.cpp b/ateam_kenobi/src/skills/line_kick.cpp index 722515c2e..66f5fcb6b 100644 --- a/ateam_kenobi/src/skills/line_kick.cpp +++ b/ateam_kenobi/src/skills/line_kick.cpp @@ -190,9 +190,8 @@ RobotCommand LineKick::RunMoveBehindBall( command.motion_intent.planner_options.draw_obstacles = true; command.motion_intent.planner_options.footprint_inflation = std::min(0.015, pre_kick_offset); - // TODO(barulicm): add accel limit support to motion intent - // easy_move_to_.setMaxAccel(1.5); - // easy_move_to_.setMaxDecel(1.5); + command.motion_intent.motion_options.max_acceleration = 1.5; + command.motion_intent.motion_options.max_deceleration = 1.5; double obstacle_radius_multiplier = 1.8; const auto robot_to_prekick = prekick_position - robot.pos; diff --git a/ateam_kenobi/src/skills/pass_receiver.cpp b/ateam_kenobi/src/skills/pass_receiver.cpp index 383d27694..4f93abf8a 100644 --- a/ateam_kenobi/src/skills/pass_receiver.cpp +++ b/ateam_kenobi/src/skills/pass_receiver.cpp @@ -77,7 +77,7 @@ RobotCommand PassReceiver::runFrame(const World & world, const Robot & robot) command = runPrePass(world, robot); } - // TODO(barulicm): Set max velocity to 2.0 + command.motion_intent.motion_options.max_velocity = 2.0; return command; } @@ -155,7 +155,7 @@ RobotCommand PassReceiver::runPass(const World & world, const Robot & robot) const auto destination = ball_ray.supporting_line().projection(robot.pos); command.motion_intent.linear = motion::intents::linear::PositionIntent{destination}; command.motion_intent.angular = motion::intents::angular::FacingIntent{world.ball.pos}; - // TODO(barulicm): Set max decel to 4.0 + command.motion_intent.motion_options.max_deceleration = 4.0; command.motion_intent.planner_options.avoid_ball = false; command.dribbler_speed = kDefaultDribblerSpeed * 1.2; @@ -194,7 +194,7 @@ RobotCommand PassReceiver::runApproachBall( kRobotDiameter * 1.05; RobotCommand command; command.motion_intent.linear = motion::intents::linear::PositionIntent{target}; - // TODO(barulicm): Set max velocity to 1.0 + command.motion_intent.motion_options.max_velocity = 1.0; return command; } From bef1b42741de18cf1210ce74364a8f632810b5e7 Mon Sep 17 00:00:00 2001 From: Matthew Barulic Date: Wed, 17 Sep 2025 00:56:59 -0400 Subject: [PATCH 27/35] Removes EasyMoveTo --- ateam_kenobi/CMakeLists.txt | 1 - .../src/core/play_helpers/easy_move_to.cpp | 296 ------------------ .../src/core/play_helpers/easy_move_to.hpp | 126 -------- .../src/plays/their_ball_placement_play.hpp | 1 - ateam_kenobi/src/plays/their_penalty_play.hpp | 1 - ateam_kenobi/src/plays/wall_play.hpp | 1 - ateam_kenobi/src/skills/goalie.cpp | 7 +- ateam_kenobi/src/skills/line_kick.cpp | 3 +- ateam_kenobi/src/skills/line_kick.hpp | 12 - ateam_kenobi/src/skills/pivot_kick.cpp | 4 - ateam_kenobi/src/skills/pivot_kick.hpp | 2 - 11 files changed, 5 insertions(+), 449 deletions(-) delete mode 100644 ateam_kenobi/src/core/play_helpers/easy_move_to.cpp delete mode 100644 ateam_kenobi/src/core/play_helpers/easy_move_to.hpp diff --git a/ateam_kenobi/CMakeLists.txt b/ateam_kenobi/CMakeLists.txt index 70dfdfea9..d87c9fe11 100644 --- a/ateam_kenobi/CMakeLists.txt +++ b/ateam_kenobi/CMakeLists.txt @@ -39,7 +39,6 @@ target_sources(kenobi_node_component PRIVATE src/core/motion/motion_controller.cpp src/core/motion/motion_executor.cpp src/core/visualization/overlays.cpp - src/core/play_helpers/easy_move_to.cpp src/core/play_helpers/lanes.cpp src/core/play_helpers/possession.cpp src/core/play_helpers/available_robots.cpp diff --git a/ateam_kenobi/src/core/play_helpers/easy_move_to.cpp b/ateam_kenobi/src/core/play_helpers/easy_move_to.cpp deleted file mode 100644 index ca665b40f..000000000 --- a/ateam_kenobi/src/core/play_helpers/easy_move_to.cpp +++ /dev/null @@ -1,296 +0,0 @@ -// Copyright 2021 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 "easy_move_to.hpp" -#include -#include -#include -#include -#include -#include "core/path_planning/escape_velocity.hpp" -#include "core/path_planning/obstacles.hpp" - -namespace ateam_kenobi::play_helpers -{ - -EasyMoveTo::EasyMoveTo(stp::Options stp_options) -: stp::Base(stp_options), - path_planner_(createChild("path_planner")) -{ -} - -EasyMoveTo::EasyMoveTo(EasyMoveTo && other) -: stp::Base(other) -{ - target_position_ = other.target_position_; - planner_options_ = other.planner_options_; - path_planner_ = other.path_planner_; - motion_controller_ = other.motion_controller_; -} - -EasyMoveTo::EasyMoveTo(const EasyMoveTo & other) -: stp::Base(other) -{ - target_position_ = other.target_position_; - planner_options_ = other.planner_options_; - path_planner_ = other.path_planner_; - motion_controller_ = other.motion_controller_; -} - -EasyMoveTo & EasyMoveTo::operator=(EasyMoveTo && other) -{ - stp::Base::operator=(other); - target_position_ = other.target_position_; - planner_options_ = other.planner_options_; - path_planner_ = other.path_planner_; - motion_controller_ = other.motion_controller_; - return *this; -} - -EasyMoveTo & EasyMoveTo::operator=(const EasyMoveTo & other) -{ - stp::Base::operator=(other); - target_position_ = other.target_position_; - planner_options_ = other.planner_options_; - path_planner_ = other.path_planner_; - motion_controller_ = other.motion_controller_; - return *this; -} - -void EasyMoveTo::reset() -{ - target_position_ = ateam_geometry::Point(); - motion_controller_.reset(); -} - -void EasyMoveTo::setTargetPosition( - ateam_geometry::Point target_position, - ateam_geometry::Vector target_velocity) -{ - target_position_ = target_position; - target_velocity_ = target_velocity; -} - -const path_planning::PlannerOptions & EasyMoveTo::getPlannerOptions() const -{ - return planner_options_; -} - -void EasyMoveTo::setPlannerOptions(path_planning::PlannerOptions options) -{ - planner_options_ = options; -} - -void EasyMoveTo::setMotionOptions(motion::MotionOptions options) -{ - motion_options_ = options; -} - - -void EasyMoveTo::face_point(std::optional point) -{ - motion_controller_.face_point(point); -} -void EasyMoveTo::face_absolute(double angle) -{ - motion_controller_.face_absolute(angle); -} -void EasyMoveTo::face_travel() -{ - motion_controller_.face_travel(); -} -void EasyMoveTo::no_face() -{ - motion_controller_.no_face(); -} - -void EasyMoveTo::setMaxVelocity(double velocity) -{ - if (velocity > 3.0) { - RCLCPP_WARN(getLogger(), "UNREASONABLY LARGE VELOCITY GIVEN TO SET MAX VELOCITY"); - return; - } - motion_options_.max_velocity = velocity; -} - -void EasyMoveTo::setMaxAngularVelocity(double velocity) -{ - if (velocity > 6.5) { - RCLCPP_WARN(getLogger(), "UNREASONABLY LARGE VELOCITY GIVEN TO SET MAX ANGULAR VELOCITY"); - return; - } - motion_options_.max_angular_velocity = velocity; -} - -void EasyMoveTo::setMaxAccel(double accel) -{ - if (accel > 8.0) { - RCLCPP_WARN(getLogger(), "UNREASONABLY LARGE ACCELERATION GIVEN TO SET MAX ACCELERATION"); - return; - } - motion_options_.max_acceleration = accel; -} - -void EasyMoveTo::setMaxDecel(double decel) -{ - if (decel > 8.0) { - RCLCPP_WARN(getLogger(), "UNREASONABLY LARGE DECELERATION GIVEN TO SET MAX DECELERATION"); - return; - } - motion_options_.max_deceleration = decel; -} - -void EasyMoveTo::setMaxThetaAccel(double accel) -{ - if (accel > 8.0) { - RCLCPP_WARN(getLogger(), "UNREASONABLY LARGE ACCELERATION GIVEN TO SET MAX THETA ACCELERATION"); - return; - } - motion_options_.max_angular_acceleration = accel; -} - -void EasyMoveTo::setMaxAllowedTurnAngle(double angle) -{ - motion_options_.max_allowed_turn_angle = angle; -} - -ateam_msgs::msg::RobotMotionCommand EasyMoveTo::runFrame( - const Robot & robot, const World & world, - const std::vector & obstacles) -{ - const auto escape_velocity = generateEscapeVelocity(world, robot, obstacles); - if (escape_velocity) { - return *escape_velocity; - } - const auto path = planPath(robot, world, obstacles); - const auto motion_command = getMotionCommand(path, robot, world); - drawTrajectoryOverlay(path, robot); - return motion_command; -} - -path_planning::Path EasyMoveTo::planPath( - const Robot & robot, const World & world, - const std::vector & obstacles) -{ - return path_planner_.getPath(robot.pos, target_position_, world, obstacles, planner_options_); -} - -ateam_msgs::msg::RobotMotionCommand EasyMoveTo::getMotionCommand( - const path_planning::Path & path, const Robot & robot, const World & world) -{ - const auto current_time = std::chrono::duration_cast>( - world.current_time.time_since_epoch()).count(); - - // Robot should stop at the end of the path - // if the plan doesn't reach the target point due to an obstacle - auto velocity = ateam_geometry::Vector(0, 0); - if (!path.empty() && (CGAL::squared_distance(path.back(), - target_position_) < kRobotRadius * kRobotRadius)) - { - velocity = target_velocity_; - } - - const bool used_cached_path = path_planner_.usedCachedPath(); - if (used_cached_path) { - motion_controller_.update_trajectory(path, velocity); - } else { - motion_controller_.reset_trajectory(path, velocity); - } - return motion_controller_.get_command(robot, current_time, motion_options_); -} - -void EasyMoveTo::drawTrajectoryOverlay( - const path_planning::Path & path, - const Robot & robot) -{ - auto & overlays = getOverlays(); - if(path.empty()) { - overlays.drawLine("path", {robot.pos, target_position_}, "Red"); - } else if(path.size() == 1) { - overlays.drawLine("path", {robot.pos, target_position_}, "Purple"); - } else { - const auto [closest_index, closest_point] = ProjectRobotOnPath(path, robot); - std::vector path_done(path.begin(), path.begin() + (closest_index)); - path_done.push_back(closest_point); - std::vector path_remaining(path.begin() + (closest_index), - path.end()); - path_remaining.insert(path_remaining.begin(), closest_point); - const auto translucent_purple = "#8000805F"; - overlays.drawLine("path_done", path_done, translucent_purple); - overlays.drawLine("path_remaining", path_remaining, "Purple"); - if (path_planner_.didTimeOut()) { - overlays.drawLine("afterpath", {path.back(), target_position_}, "LightSkyBlue"); - } else if (path_planner_.isPathTruncated()) { - overlays.drawLine("afterpath", {path.back(), target_position_}, "LightPink"); - } - } -} - -std::optional EasyMoveTo::generateEscapeVelocity( - const World & world, - const Robot & robot, - std::vector obstacles) -{ - if (planner_options_.use_default_obstacles) { - path_planning::AddDefaultObstacles(world, obstacles); - } - path_planning::AddRobotObstacles(world.our_robots, robot.id, obstacles); - path_planning::AddRobotObstacles(world.their_robots, obstacles); - - const auto twist = path_planning::GenerateEscapeVelocity(robot, obstacles, - planner_options_.footprint_inflation); - - if(!twist) { - return std::nullopt; - } - - ateam_msgs::msg::RobotMotionCommand command; - command.twist = *twist; - return command; -} - -std::pair EasyMoveTo::ProjectRobotOnPath( - const path_planning::Path & path, const Robot & robot) -{ - if (path.empty()) { - return {0, robot.pos}; - } - if (path.size() == 1) { - return {0, path[0]}; - } - auto closest_point = ateam_geometry::nearestPointOnSegment(ateam_geometry::Segment(path[0], - path[1]), robot.pos); - size_t closest_index = 1; - double min_distance = CGAL::squared_distance(closest_point, robot.pos); - for (size_t i = 1; i < path.size() - 1; ++i) { - const auto segment = ateam_geometry::Segment(path[i], path[i + 1]); - const auto point_on_segment = ateam_geometry::nearestPointOnSegment(segment, robot.pos); - const auto distance = CGAL::squared_distance(point_on_segment, robot.pos); - if (distance <= min_distance) { - min_distance = distance; - closest_point = point_on_segment; - closest_index = i + 1; - } - } - return {closest_index, closest_point}; -} - -} // namespace ateam_kenobi::play_helpers diff --git a/ateam_kenobi/src/core/play_helpers/easy_move_to.hpp b/ateam_kenobi/src/core/play_helpers/easy_move_to.hpp deleted file mode 100644 index b274020f4..000000000 --- a/ateam_kenobi/src/core/play_helpers/easy_move_to.hpp +++ /dev/null @@ -1,126 +0,0 @@ -// Copyright 2021 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 CORE__PLAY_HELPERS__EASY_MOVE_TO_HPP_ -#define CORE__PLAY_HELPERS__EASY_MOVE_TO_HPP_ - -#include -#include -#include -#include -#include -#include "core/path_planning/path_planner.hpp" -#include "core/motion/motion_controller.hpp" -#include "core/stp/base.hpp" -#include "core/types/robot.hpp" -#include "core/types/world.hpp" - -namespace ateam_kenobi::play_helpers -{ - -class EasyMoveTo : public stp::Base -{ -public: - /** - * @brief DO NOT USE THIS CONSTRUCTOR IN PLAY CODE - * This is meant for internal use only. - */ - EasyMoveTo() - : stp::Base(stp::Options{}) {} - - explicit EasyMoveTo(stp::Options stp_options); - - explicit EasyMoveTo(EasyMoveTo && other); - - explicit EasyMoveTo(const EasyMoveTo & other); - - EasyMoveTo & operator=(EasyMoveTo && other); - - EasyMoveTo & operator=(const EasyMoveTo & other); - - void reset(); - - void setTargetPosition( - ateam_geometry::Point target_position, - ateam_geometry::Vector target_velocity = ateam_geometry::Vector(0, 0)); - - const path_planning::PlannerOptions & getPlannerOptions() const; - void setPlannerOptions(path_planning::PlannerOptions options); - void setMotionOptions(motion::MotionOptions options); - - void face_point(std::optional point); - void face_absolute(double angle); - void face_travel(); - void no_face(); - - void setMaxVelocity(double velocity); - void setMaxAngularVelocity(double velocity); - - void setMaxAccel(double accel); - void setMaxDecel(double decel); - void setMaxThetaAccel(double accel); - void setMaxAllowedTurnAngle(double angle); - - ateam_msgs::msg::RobotMotionCommand runFrame( - const Robot & robot, const World & world, - const std::vector & obstacles = {}); - - ateam_geometry::Point getTargetPosition() const - { - return target_position_; - } - - void SetEnableEscapeVelocities(bool enabled) - { - enable_escape_velocities_ = enabled; - } - -private: - bool enable_escape_velocities_ = true; - ateam_geometry::Point target_position_; - ateam_geometry::Vector target_velocity_; - path_planning::PlannerOptions planner_options_; - path_planning::PathPlanner path_planner_; - motion::MotionController motion_controller_; - motion::MotionOptions motion_options_; - - path_planning::Path planPath( - const Robot & robot, const World & world, - const std::vector & obstacles); - - ateam_msgs::msg::RobotMotionCommand getMotionCommand( - const path_planning::Path & path, const Robot & robot, const World & world); - - void drawTrajectoryOverlay(const path_planning::Path & path, const Robot & robot); - - std::optional generateEscapeVelocity( - const World & world, - const Robot & robot, - std::vector obstacles); - - std::pair ProjectRobotOnPath( - const path_planning::Path & path, - const Robot & robot); -}; - -} // namespace ateam_kenobi::play_helpers - -#endif // CORE__PLAY_HELPERS__EASY_MOVE_TO_HPP_ diff --git a/ateam_kenobi/src/plays/their_ball_placement_play.hpp b/ateam_kenobi/src/plays/their_ball_placement_play.hpp index 1c6da6934..3c8c29f16 100644 --- a/ateam_kenobi/src/plays/their_ball_placement_play.hpp +++ b/ateam_kenobi/src/plays/their_ball_placement_play.hpp @@ -22,7 +22,6 @@ #define PLAYS__THEIR_BALL_PLACEMENT_PLAY_HPP_ #include "core/stp/play.hpp" -#include "core/play_helpers/easy_move_to.hpp" #include "tactics/multi_move_to.hpp" namespace ateam_kenobi::plays diff --git a/ateam_kenobi/src/plays/their_penalty_play.hpp b/ateam_kenobi/src/plays/their_penalty_play.hpp index 2b46c0f6e..2e0caebf5 100644 --- a/ateam_kenobi/src/plays/their_penalty_play.hpp +++ b/ateam_kenobi/src/plays/their_penalty_play.hpp @@ -23,7 +23,6 @@ #define PLAYS__THEIR_PENALTY_PLAY_HPP_ #include "core/stp/play.hpp" -#include "core/play_helpers/easy_move_to.hpp" #include "skills/goalie.hpp" #include "tactics/multi_move_to.hpp" diff --git a/ateam_kenobi/src/plays/wall_play.hpp b/ateam_kenobi/src/plays/wall_play.hpp index e93b83308..84746b1e7 100644 --- a/ateam_kenobi/src/plays/wall_play.hpp +++ b/ateam_kenobi/src/plays/wall_play.hpp @@ -28,7 +28,6 @@ #include "ateam_geometry/types.hpp" #include "core/types/robot.hpp" #include "core/stp/play.hpp" -#include "core/play_helpers/easy_move_to.hpp" #include "skills/goalie.hpp" #include "tactics/multi_move_to.hpp" diff --git a/ateam_kenobi/src/skills/goalie.cpp b/ateam_kenobi/src/skills/goalie.cpp index 7fd18e9fd..7a14b0e3e 100644 --- a/ateam_kenobi/src/skills/goalie.cpp +++ b/ateam_kenobi/src/skills/goalie.cpp @@ -39,7 +39,6 @@ Goalie::Goalie(stp::Options stp_options) kick_(createChild("Kick")) { reset(); - kick_.SetUseDefaultObstacles(false); default_planner_options_.avoid_ball = false; default_planner_options_.use_default_obstacles = false; // kick_.setPreKickOffset(kRobotRadius + kBallRadius + 0.04); @@ -316,7 +315,8 @@ RobotCommand Goalie::runClearBall( kick_.SetKickSpeed(1.3); kick_.SetTargetPoint(target_point); - const auto command = kick_.RunFrame(world, goalie); + auto command = kick_.RunFrame(world, goalie); + command.motion_intent.planner_options.use_default_obstacles = false; ForwardPlayInfo(kick_); return command; } @@ -342,7 +342,8 @@ RobotCommand Goalie::runSideEjectBall( kick_.SetKickSpeed(0.2); kick_.SetTargetPoint(world.ball.pos + shoot_vec); - const auto command = kick_.RunFrame(world, goalie); + auto command = kick_.RunFrame(world, goalie); + command.motion_intent.planner_options.use_default_obstacles = false; ForwardPlayInfo(kick_); return command; } diff --git a/ateam_kenobi/src/skills/line_kick.cpp b/ateam_kenobi/src/skills/line_kick.cpp index 66f5fcb6b..5c80f176b 100644 --- a/ateam_kenobi/src/skills/line_kick.cpp +++ b/ateam_kenobi/src/skills/line_kick.cpp @@ -29,8 +29,7 @@ namespace ateam_kenobi::skills { LineKick::LineKick(stp::Options stp_options, KickSkill::WaitType wait_type) -: KickSkill(stp_options, wait_type), - easy_move_to_(createChild("EasyMoveTo")) +: KickSkill(stp_options, wait_type) { } diff --git a/ateam_kenobi/src/skills/line_kick.hpp b/ateam_kenobi/src/skills/line_kick.hpp index f7e7e3664..0b884fa12 100644 --- a/ateam_kenobi/src/skills/line_kick.hpp +++ b/ateam_kenobi/src/skills/line_kick.hpp @@ -26,7 +26,6 @@ #include "kick_skill.hpp" #include "core/types/world.hpp" #include "core/types/robot_command.hpp" -#include "core/play_helpers/easy_move_to.hpp" namespace ateam_kenobi::skills { @@ -66,16 +65,6 @@ class LineKick : public KickSkill return state_ == State::Done; } - /** - * @brief Set the default obstacles planner option on the internal EasyMoveTo - */ - void SetUseDefaultObstacles(bool use_obstacles) - { - path_planning::PlannerOptions options = easy_move_to_.getPlannerOptions(); - options.use_default_obstacles = use_obstacles; - easy_move_to_.setPlannerOptions(options); - } - bool IsReady() const override { return state_ == State::FaceBall || state_ == State::KickBall; @@ -156,7 +145,6 @@ class LineKick : public KickSkill const double kDefaultPreKickOffset = kRobotRadius + kBallRadius + 0.06; double pre_kick_offset = kDefaultPreKickOffset; ateam_geometry::Point target_point_; - play_helpers::EasyMoveTo easy_move_to_; enum class State { diff --git a/ateam_kenobi/src/skills/pivot_kick.cpp b/ateam_kenobi/src/skills/pivot_kick.cpp index 2621cc92e..49a195395 100644 --- a/ateam_kenobi/src/skills/pivot_kick.cpp +++ b/ateam_kenobi/src/skills/pivot_kick.cpp @@ -30,7 +30,6 @@ namespace ateam_kenobi::skills PivotKick::PivotKick(stp::Options stp_options, KickSkill::WaitType wait_type) : KickSkill(stp_options, wait_type), - easy_move_to_(createChild("easy_move_to")), capture_(createChild("Capture")) { } @@ -57,7 +56,6 @@ RobotCommand PivotKick::RunFrame(const World & world, const Robot & robot) } if (!robot.breakbeam_ball_detected) { - easy_move_to_.reset(); prev_state_ = State::Capture; getPlayInfo()["State"] = "Capture"; return Capture(world, robot); @@ -67,7 +65,6 @@ RobotCommand PivotKick::RunFrame(const World & world, const Robot & robot) const auto robot_to_target_angle = std::atan2(robot_to_target.y(), robot_to_target.x()); if (abs(angles::shortest_angular_distance(robot.theta, robot_to_target_angle)) > 0.05) { if (prev_state_ != State::Pivot) { - easy_move_to_.reset(); prev_state_ = State::Pivot; } getPlayInfo()["State"] = "Pivot"; @@ -75,7 +72,6 @@ RobotCommand PivotKick::RunFrame(const World & world, const Robot & robot) } if (prev_state_ != State::KickBall) { - easy_move_to_.reset(); prev_state_ = State::KickBall; } diff --git a/ateam_kenobi/src/skills/pivot_kick.hpp b/ateam_kenobi/src/skills/pivot_kick.hpp index e4dc2e545..d8c5a4ea9 100644 --- a/ateam_kenobi/src/skills/pivot_kick.hpp +++ b/ateam_kenobi/src/skills/pivot_kick.hpp @@ -26,7 +26,6 @@ #include "kick_skill.hpp" #include "core/types/robot_command.hpp" #include "core/types/world.hpp" -#include "core/play_helpers/easy_move_to.hpp" #include "skills/capture.hpp" namespace ateam_kenobi::skills @@ -84,7 +83,6 @@ class PivotKick : public KickSkill private: ateam_geometry::Point target_point_; - play_helpers::EasyMoveTo easy_move_to_; skills::Capture capture_; bool done_ = false; double pivot_speed_ = 2.0; // rad/s From 55ea690d830dacd35c65f3b4c3b5266e6668ae30 Mon Sep 17 00:00:00 2001 From: Matthew Barulic Date: Wed, 17 Sep 2025 01:12:36 -0400 Subject: [PATCH 28/35] Converts MotionController return type to BodyVelocity --- .../src/core/motion/motion_controller.cpp | 10 +++++----- .../src/core/motion/motion_controller.hpp | 3 ++- .../src/core/motion/motion_executor.cpp | 20 +++++++------------ .../plays/test_plays/controls_test_play.cpp | 12 +++++------ 4 files changed, 19 insertions(+), 26 deletions(-) diff --git a/ateam_kenobi/src/core/motion/motion_controller.cpp b/ateam_kenobi/src/core/motion/motion_controller.cpp index ace1e3d61..ad0049b48 100644 --- a/ateam_kenobi/src/core/motion/motion_controller.cpp +++ b/ateam_kenobi/src/core/motion/motion_controller.cpp @@ -32,6 +32,7 @@ #include "ateam_geometry/ateam_geometry.hpp" #include "ateam_common/robot_constants.hpp" #include "pid.hpp" +#include "frame_conversions.hpp" namespace ateam_kenobi::motion { @@ -210,12 +211,12 @@ double MotionController::calculate_trapezoidal_angular_vel( return std::clamp(trapezoidal_vel, -options.max_angular_velocity, options.max_angular_velocity); } -ateam_msgs::msg::RobotMotionCommand MotionController::get_command( +BodyVelocity MotionController::get_command( ateam_kenobi::Robot robot, double current_time, const MotionOptions & options) { - ateam_msgs::msg::RobotMotionCommand motion_command; + BodyVelocity motion_command; // Skip if there isn't a trajectory if (this->trajectory.size() <= 0) { @@ -358,8 +359,7 @@ ateam_msgs::msg::RobotMotionCommand MotionController::get_command( vel_vector = options.max_velocity * ateam_geometry::normalize(vel_vector); } - motion_command.twist.linear.x = vel_vector.x(); - motion_command.twist.linear.y = vel_vector.y(); + motion_command.linear = WorldToLocalFrame(vel_vector, robot); } // calculate angle movement commands @@ -410,7 +410,7 @@ ateam_msgs::msg::RobotMotionCommand MotionController::get_command( } } } - motion_command.twist.angular.z = std::clamp(t_command, -options.max_angular_velocity, options.max_angular_velocity); + motion_command.angular = std::clamp(t_command, -options.max_angular_velocity, options.max_angular_velocity); } } diff --git a/ateam_kenobi/src/core/motion/motion_controller.hpp b/ateam_kenobi/src/core/motion/motion_controller.hpp index c4fd863f2..e023100b6 100644 --- a/ateam_kenobi/src/core/motion/motion_controller.hpp +++ b/ateam_kenobi/src/core/motion/motion_controller.hpp @@ -31,6 +31,7 @@ #include "core/types/world.hpp" #include "pid.hpp" #include "motion_options.hpp" +#include "motion_intent.hpp" namespace ateam_kenobi::motion { @@ -82,7 +83,7 @@ class MotionController double dt); // Generate a robot motion command to follow a trajectory - ateam_msgs::msg::RobotMotionCommand get_command( + BodyVelocity get_command( ateam_kenobi::Robot robot, double current_time, const MotionOptions & options = MotionOptions()); diff --git a/ateam_kenobi/src/core/motion/motion_executor.cpp b/ateam_kenobi/src/core/motion/motion_executor.cpp index 154c1aa35..bb626b32a 100644 --- a/ateam_kenobi/src/core/motion/motion_executor.cpp +++ b/ateam_kenobi/src/core/motion/motion_executor.cpp @@ -116,18 +116,12 @@ std::array, }, intent.angular); if(!path.empty()) { - // TODO(barulicm) change return type of get_command to BodyVelocity - auto controller_vel = controller.get_command(robot, current_time, - intent.motion_options); + auto controller_vel = controller.get_command(robot, current_time, intent.motion_options); if (use_controller_linvel) { - ConvertWorldVelsToBodyVels(controller_vel, robot); - body_velocity.linear = { - controller_vel.twist.linear.x, - controller_vel.twist.linear.y - }; + body_velocity.linear = controller_vel.linear; } if (use_controller_omega) { - body_velocity.angular = controller_vel.twist.angular.z; + body_velocity.angular = controller_vel.angular; } } @@ -219,14 +213,14 @@ void MotionExecutor::DrawOverlays( std::pair MotionExecutor::ProjectRobotOnPath( const path_planning::Path & path, const Robot & robot) { - if (path.empty()) - { + if (path.empty()) { return {0, robot.pos}; } if (path.size() == 1) { return {0, path[0]}; } - auto closest_point = ateam_geometry::nearestPointOnSegment(ateam_geometry::Segment(path[0], path[1]), robot.pos); + auto closest_point = ateam_geometry::nearestPointOnSegment(ateam_geometry::Segment(path[0], + path[1]), robot.pos); size_t closest_index = 1; double min_distance = CGAL::squared_distance(closest_point, robot.pos); for (size_t i = 1; i < path.size() - 1; ++i) { @@ -236,7 +230,7 @@ std::pair MotionExecutor::ProjectRobotOnPath( if (distance <= min_distance) { min_distance = distance; closest_point = point_on_segment; - closest_index = i+1; + closest_index = i + 1; } } return {closest_index, closest_point}; diff --git a/ateam_kenobi/src/plays/test_plays/controls_test_play.cpp b/ateam_kenobi/src/plays/test_plays/controls_test_play.cpp index e6de0cc2c..4e5ed1ac7 100644 --- a/ateam_kenobi/src/plays/test_plays/controls_test_play.cpp +++ b/ateam_kenobi/src/plays/test_plays/controls_test_play.cpp @@ -115,15 +115,13 @@ std::array, 16> ControlsTestPlay::runFrame( motion_options.max_acceleration = 2.0; motion_options.max_deceleration = 2.0; motion_options.completion_threshold = position_threshold; - const auto command_msg = motion_controller_.get_command( + const auto body_vel = motion_controller_.get_command( robot, current_time, motion_options); RobotCommand command; - command.motion_intent.linear = motion::intents::linear::VelocityIntent{ - ateam_geometry::Vector(command_msg.twist.linear.x, command_msg.twist.linear.y), - motion::intents::linear::Frame::World}; - command.motion_intent.angular = motion::intents::angular::VelocityIntent{command_msg.twist.angular.z}; + command.motion_intent.linear = motion::intents::linear::VelocityIntent{body_vel.linear, motion::intents::linear::Frame::World}; + command.motion_intent.angular = motion::intents::angular::VelocityIntent{body_vel.angular}; maybe_motion_commands[robot.id] = command; const std::vector viz_path = path; @@ -146,8 +144,8 @@ std::array, 16> ControlsTestPlay::runFrame( getPlayInfo()["robot"]["vel"]["y"] = robot.vel.y(); getPlayInfo()["robot"]["vel"]["t"] = robot.omega; - getPlayInfo()["robot"]["cmd_vel"]["x"] = command_msg.twist.linear.x; - getPlayInfo()["robot"]["cmd_vel"]["y"] = command_msg.twist.linear.y; + getPlayInfo()["robot"]["cmd_vel"]["x"] = body_vel.linear.x(); + getPlayInfo()["robot"]["cmd_vel"]["y"] = body_vel.linear.y(); getPlayInfo()["error"]["x"] = waypoints[index].position.x() - robot.pos.x(); getPlayInfo()["error"]["y"] = waypoints[index].position.y() - robot.pos.y(); From f8a9ee3eb1885bfe95555248acd01efeb97516b4 Mon Sep 17 00:00:00 2001 From: Matthew Barulic Date: Wed, 17 Sep 2025 01:33:18 -0400 Subject: [PATCH 29/35] Consolidates frame conversion code. --- .../src/core/defense_area_enforcement.cpp | 1 + .../src/core/motion/frame_conversions.hpp | 1 + .../src/core/motion/motion_executor.cpp | 1 - .../src/core/motion/world_to_body_vel.hpp | 93 ------------------- ateam_kenobi/src/kenobi_node.cpp | 19 ++-- 5 files changed, 11 insertions(+), 104 deletions(-) delete mode 100644 ateam_kenobi/src/core/motion/world_to_body_vel.hpp diff --git a/ateam_kenobi/src/core/defense_area_enforcement.cpp b/ateam_kenobi/src/core/defense_area_enforcement.cpp index eccb70444..e1830acf5 100644 --- a/ateam_kenobi/src/core/defense_area_enforcement.cpp +++ b/ateam_kenobi/src/core/defense_area_enforcement.cpp @@ -71,6 +71,7 @@ bool WouldVelocityCauseCollision( const double delta_t = 0.01; + // TODO(barulicm): This ignores the velocity frame const ateam_geometry::Vector velocity{motion_command.twist.linear.x, motion_command.twist.linear.y}; diff --git a/ateam_kenobi/src/core/motion/frame_conversions.hpp b/ateam_kenobi/src/core/motion/frame_conversions.hpp index 008c59d0e..9dfcd5321 100644 --- a/ateam_kenobi/src/core/motion/frame_conversions.hpp +++ b/ateam_kenobi/src/core/motion/frame_conversions.hpp @@ -24,6 +24,7 @@ #include #include "core/types/robot.hpp" +#include "core/motion/motion_intent.hpp" namespace ateam_kenobi::motion { diff --git a/ateam_kenobi/src/core/motion/motion_executor.cpp b/ateam_kenobi/src/core/motion/motion_executor.cpp index bb626b32a..13d5267a5 100644 --- a/ateam_kenobi/src/core/motion/motion_executor.cpp +++ b/ateam_kenobi/src/core/motion/motion_executor.cpp @@ -22,7 +22,6 @@ #include #include "core/path_planning/obstacles.hpp" #include "core/path_planning/escape_velocity.hpp" -#include "world_to_body_vel.hpp" namespace ateam_kenobi::motion { diff --git a/ateam_kenobi/src/core/motion/world_to_body_vel.hpp b/ateam_kenobi/src/core/motion/world_to_body_vel.hpp deleted file mode 100644 index f4ab7ea75..000000000 --- a/ateam_kenobi/src/core/motion/world_to_body_vel.hpp +++ /dev/null @@ -1,93 +0,0 @@ -// Copyright 2021 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 CORE__MOTION__WORLD_TO_BODY_VEL_HPP_ -#define CORE__MOTION__WORLD_TO_BODY_VEL_HPP_ - -#include -#include -#include -#include "core/types/robot.hpp" - -namespace ateam_kenobi::motion -{ - -inline void ConvertWorldVelsToBodyVels( - ateam_msgs::msg::RobotMotionCommand & command, - const Robot & robot) -{ - if(command.twist_frame == ateam_msgs::msg::RobotMotionCommand::FRAME_BODY) { - return; - } - ateam_geometry::Vector velocity(command.twist.linear.x, command.twist.linear.y); - CGAL::Aff_transformation_2 transformation(CGAL::ROTATION, - std::sin(-robot.theta), std::cos(-robot.theta)); - velocity = velocity.transform(transformation); - command.twist.linear.x = velocity.x(); - command.twist.linear.y = velocity.y(); - command.twist_frame = ateam_msgs::msg::RobotMotionCommand::FRAME_BODY; -} - -inline void ConvertWorldVelsToBodyVels( - std::array, 16> & commands, - const std::array & robots) -{ - for (auto i = 0u; i < 16; ++i) { - auto & maybe_command = commands.at(i); - const auto & robot = robots.at(i); - if (maybe_command && robot.visible) { - ConvertWorldVelsToBodyVels(maybe_command.value(), robot); - } - } -} - -inline void ConvertBodyVelsToWorldVels( - ateam_msgs::msg::RobotMotionCommand & command, - const Robot & robot) -{ - if(command.twist_frame == ateam_msgs::msg::RobotMotionCommand::FRAME_WORLD) { - return; - } - ateam_geometry::Vector velocity(command.twist.linear.x, command.twist.linear.y); - CGAL::Aff_transformation_2 transformation(CGAL::ROTATION, - std::sin(robot.theta), std::cos(robot.theta)); - velocity = velocity.transform(transformation); - command.twist.linear.x = velocity.x(); - command.twist.linear.y = velocity.y(); - command.twist_frame = ateam_msgs::msg::RobotMotionCommand::FRAME_WORLD; -} - -inline void ConvertBodyVelsToWorldVels( - std::array, 16> & commands, - const std::array & robots) -{ - for (auto i = 0u; i < 16; ++i) { - auto & maybe_command = commands.at(i); - const auto & robot = robots.at(i); - if (maybe_command && robot.visible) { - ConvertBodyVelsToWorldVels(maybe_command.value(), robot); - } - } -} - -} // namespace ateam_kenobi::motion - -#endif // CORE__MOTION__WORLD_TO_BODY_VEL_HPP_ diff --git a/ateam_kenobi/src/kenobi_node.cpp b/ateam_kenobi/src/kenobi_node.cpp index 1470d1bfa..7252b5b5e 100644 --- a/ateam_kenobi/src/kenobi_node.cpp +++ b/ateam_kenobi/src/kenobi_node.cpp @@ -50,7 +50,7 @@ #include "core/double_touch_eval.hpp" #include "core/ballsense_emulator.hpp" #include "core/ballsense_filter.hpp" -#include "core/motion/world_to_body_vel.hpp" +#include "core/motion/frame_conversions.hpp" #include "core/motion/motion_executor.hpp" #include "plays/halt_play.hpp" #include "core/defense_area_enforcement.hpp" @@ -440,12 +440,6 @@ class KenobiNode : public rclcpp::Node joystick_enforcer_.RemoveCommandForJoystickBot(motion_commands); - if (get_parameter("use_world_velocities").as_bool()) { - motion::ConvertBodyVelsToWorldVels(motion_commands, world_.our_robots); - } else { - motion::ConvertWorldVelsToBodyVels(motion_commands, world_.our_robots); - } - send_all_motion_commands(motion_commands); } @@ -465,7 +459,7 @@ class KenobiNode : public rclcpp::Node std::array, 16> motion_intents; std::ranges::transform( commands, motion_intents.begin(), - [](const std::optional & cmd) -> std::optional{ + [](const std::optional & cmd) -> std::optional { if (cmd.has_value()) { return cmd->motion_intent; } else { @@ -474,6 +468,8 @@ class KenobiNode : public rclcpp::Node }); const auto motion_commands = motion_executor_.RunFrame(motion_intents, overlays_, world); + const auto use_world_vels = get_parameter("use_world_velocities").as_bool(); + std::array, 16> ros_commands; for(auto id = 0ul; id < commands.size(); ++id) { auto & maybe_cmd = commands[id]; @@ -481,14 +477,17 @@ class KenobiNode : public rclcpp::Node if (!maybe_cmd || !maybe_motion_cmd) { ros_commands[id] = std::nullopt; } else { + const auto & robot = world.our_robots[id]; const auto & cmd = maybe_cmd.value(); const auto & motion_cmd = maybe_motion_cmd.value(); + const auto linear_vel = use_world_vels ? + motion::LocalToWorldFrame(motion_cmd.linear, robot) : motion_cmd.linear; auto & ros_cmd = ros_commands[id].emplace(); ros_cmd.dribbler_speed = cmd.dribbler_speed; ros_cmd.kick_request = static_cast(cmd.kick); ros_cmd.kick_speed = cmd.kick_speed; - ros_cmd.twist.linear.x = motion_cmd.linear.x(); - ros_cmd.twist.linear.y = motion_cmd.linear.y(); + ros_cmd.twist.linear.x = linear_vel.x(); + ros_cmd.twist.linear.y = linear_vel.y(); ros_cmd.twist.angular.z = motion_cmd.angular; ros_cmd.twist_frame = ateam_msgs::msg::RobotMotionCommand::FRAME_BODY; } From 6ba43088ac4e3dbffaeba7a4d04893e416c88f8d Mon Sep 17 00:00:00 2001 From: Matthew Barulic Date: Wed, 17 Sep 2025 01:43:03 -0400 Subject: [PATCH 30/35] Converts return types for escape velocity functions --- .../src/core/motion/motion_executor.cpp | 5 ++-- .../core/path_planning/escape_velocity.cpp | 30 ++++++------------- .../core/path_planning/escape_velocity.hpp | 15 ++++------ .../src/plays/stop_plays/stop_helpers.cpp | 3 +- 4 files changed, 19 insertions(+), 34 deletions(-) diff --git a/ateam_kenobi/src/core/motion/motion_executor.cpp b/ateam_kenobi/src/core/motion/motion_executor.cpp index 13d5267a5..ca1cda86b 100644 --- a/ateam_kenobi/src/core/motion/motion_executor.cpp +++ b/ateam_kenobi/src/core/motion/motion_executor.cpp @@ -22,6 +22,7 @@ #include #include "core/path_planning/obstacles.hpp" #include "core/path_planning/escape_velocity.hpp" +#include "frame_conversions.hpp" namespace ateam_kenobi::motion { @@ -162,8 +163,8 @@ std::optional MotionExecutor::GenerateEscapeVelocity( intent.planner_options.footprint_inflation); if (vel) { return BodyVelocity{ - ateam_geometry::Vector(vel->linear.x, vel->linear.y), - vel->angular.z + WorldToLocalFrame(*vel, robot), + 0.0 }; } else { return std::nullopt; diff --git a/ateam_kenobi/src/core/path_planning/escape_velocity.cpp b/ateam_kenobi/src/core/path_planning/escape_velocity.cpp index 54ec1e175..47a5b3bb1 100644 --- a/ateam_kenobi/src/core/path_planning/escape_velocity.cpp +++ b/ateam_kenobi/src/core/path_planning/escape_velocity.cpp @@ -26,7 +26,7 @@ namespace ateam_kenobi::path_planning { -geometry_msgs::msg::Twist GenerateEscapeVelocity( +ateam_geometry::Vector GenerateEscapeVelocity( const Robot & robot, const ateam_geometry::AnyShape & obstacle) { @@ -46,34 +46,26 @@ geometry_msgs::msg::Twist GenerateEscapeVelocity( } // Default case when we don't recognize the obstacle type - return geometry_msgs::msg::Twist{}; + return ateam_geometry::Vector{}; } -geometry_msgs::msg::Twist GenerateEscapeVelocity( +ateam_geometry::Vector GenerateEscapeVelocity( const Robot & robot, const ateam_geometry::Circle & obstacle) { const auto vector = robot.pos - obstacle.center(); - const auto velocity = ateam_geometry::normalize(vector) * kSafeEscapeVelocity; - geometry_msgs::msg::Twist msg; - msg.linear.x = velocity.x(); - msg.linear.y = velocity.y(); - return msg; + return ateam_geometry::normalize(vector) * kSafeEscapeVelocity; } -geometry_msgs::msg::Twist GenerateEscapeVelocity( +ateam_geometry::Vector GenerateEscapeVelocity( const Robot & robot, const ateam_geometry::Disk & obstacle) { const auto vector = robot.pos - obstacle.center(); - const auto velocity = ateam_geometry::normalize(vector) * kSafeEscapeVelocity; - geometry_msgs::msg::Twist msg; - msg.linear.x = velocity.x(); - msg.linear.y = velocity.y(); - return msg; + return ateam_geometry::normalize(vector) * kSafeEscapeVelocity; } -geometry_msgs::msg::Twist GenerateEscapeVelocity( +ateam_geometry::Vector GenerateEscapeVelocity( const Robot & robot, const ateam_geometry::Rectangle & obstacle) { @@ -97,14 +89,10 @@ geometry_msgs::msg::Twist GenerateEscapeVelocity( // Rotate direction by -90 deg. Assumes vertices are reported in counterclockwise order const ateam_geometry::Vector escape_vector{edge_direction.dy(), -edge_direction.dx()}; - const auto velocity = escape_vector * kSafeEscapeVelocity; - geometry_msgs::msg::Twist msg; - msg.linear.x = velocity.x(); - msg.linear.y = velocity.y(); - return msg; + return escape_vector * kSafeEscapeVelocity; } -std::optional GenerateEscapeVelocity( +std::optional GenerateEscapeVelocity( const Robot & robot, const std::vector & obstacles, double footprint_inflation) { diff --git a/ateam_kenobi/src/core/path_planning/escape_velocity.hpp b/ateam_kenobi/src/core/path_planning/escape_velocity.hpp index c25fb1280..21d0fd448 100644 --- a/ateam_kenobi/src/core/path_planning/escape_velocity.hpp +++ b/ateam_kenobi/src/core/path_planning/escape_velocity.hpp @@ -24,7 +24,6 @@ #include #include #include -#include #include "core/types/robot.hpp" namespace ateam_kenobi::path_planning @@ -32,26 +31,24 @@ namespace ateam_kenobi::path_planning constexpr double kSafeEscapeVelocity = 0.3; // m/s -// TODO(barulicm): Convert this file to using BodyVelocity return types - /** * @brief Creates a low velocity in the quickest direction to escape the given obstacle. * * @note This function does not actually check if the robot is currently in the given obstacle. */ -geometry_msgs::msg::Twist GenerateEscapeVelocity( +ateam_geometry::Vector GenerateEscapeVelocity( const Robot & robot, const ateam_geometry::AnyShape & obstacle); -geometry_msgs::msg::Twist GenerateEscapeVelocity( +ateam_geometry::Vector GenerateEscapeVelocity( const Robot & robot, const ateam_geometry::Circle & obstacle); -geometry_msgs::msg::Twist GenerateEscapeVelocity( +ateam_geometry::Vector GenerateEscapeVelocity( const Robot & robot, const ateam_geometry::Disk & obstacle); -geometry_msgs::msg::Twist GenerateEscapeVelocity( +ateam_geometry::Vector GenerateEscapeVelocity( const Robot & robot, const ateam_geometry::Rectangle & obstacle); @@ -59,10 +56,10 @@ geometry_msgs::msg::Twist GenerateEscapeVelocity( * @brief Creates a low velocity in the quickest direction to escape the first colliding obstacle * in @c obstacles. * - * @return std::optional The escape velocity, or @c nullopt if no + * @return std::optional The escape velocity, or @c nullopt if no * obstacles collide with the robot. */ -std::optional GenerateEscapeVelocity( +std::optional GenerateEscapeVelocity( const Robot & robot, const std::vector & obstacles, double footprint_inflation = 0.06); diff --git a/ateam_kenobi/src/plays/stop_plays/stop_helpers.cpp b/ateam_kenobi/src/plays/stop_plays/stop_helpers.cpp index 78100e403..8b14e2ea8 100644 --- a/ateam_kenobi/src/plays/stop_plays/stop_helpers.cpp +++ b/ateam_kenobi/src/plays/stop_plays/stop_helpers.cpp @@ -290,10 +290,9 @@ void moveBotsInObstacles( } RobotCommand command; command.motion_intent.linear = motion::intents::linear::VelocityIntent{ - ateam_geometry::Vector{opt_escape_vel->linear.x, opt_escape_vel->linear.y}, + *opt_escape_vel, motion::intents::linear::Frame::World }; - command.motion_intent.angular = motion::intents::angular::VelocityIntent{opt_escape_vel->angular.z}; motion_commands[i] = command; play_info_bots.push_back(robot.id); } From 7fba5a66ca2a5333a21764b8ab7e29085262646b Mon Sep 17 00:00:00 2001 From: Matthew Barulic Date: Wed, 17 Sep 2025 01:51:20 -0400 Subject: [PATCH 31/35] Migrates TestPivotPlay to motion intents --- .../src/plays/test_plays/test_pivot_play.hpp | 21 ++++++++++++------- 1 file changed, 14 insertions(+), 7 deletions(-) diff --git a/ateam_kenobi/src/plays/test_plays/test_pivot_play.hpp b/ateam_kenobi/src/plays/test_plays/test_pivot_play.hpp index 8ed7e5b03..19e74fab1 100644 --- a/ateam_kenobi/src/plays/test_plays/test_pivot_play.hpp +++ b/ateam_kenobi/src/plays/test_plays/test_pivot_play.hpp @@ -85,8 +85,13 @@ class TestPivotPlay : public stp::Play } prev_at_target_ = at_target; - play_info["Y Cmd"] = motion_commands[robot.id]->twist.linear.y; - play_info["Omega Cmd"] = motion_commands[robot.id]->twist.angular.z; + motion_commands[robot.id]->motion_intent.callback = [&play_info]( + motion::BodyVelocity vel, const path_planning::Path &, const Robot &, const World &) + { + play_info["Y Cmd"] = vel.linear.y(); + play_info["Omega Cmd"] = vel.angular; + return vel; + }; return motion_commands; } @@ -139,7 +144,8 @@ class TestPivotPlay : public stp::Play trapezoidal_vel = std::copysign(min_angular_vel, angle_error); } - command.twist.angular.z = std::clamp(trapezoidal_vel, -pivot_speed_, pivot_speed_); + const auto angular_vel = std::clamp(trapezoidal_vel, -pivot_speed_, pivot_speed_); + command.motion_intent.angular = motion::intents::angular::VelocityIntent{angular_vel}; /* rotate in a circle with diameter 0.0427 + 0.18 = 0.2227 (This might be tunable to use 8cm for * real robots) @@ -149,11 +155,12 @@ class TestPivotPlay : public stp::Play // double diameter = kBallDiameter + kRobotDiameter; double diameter = 2 * .095; double circumference = M_PI * diameter; - double velocity = circumference * (command.twist.angular.z / (2 * M_PI)); + double velocity = circumference * (angular_vel / (2 * M_PI)); - command.twist.linear.x = 0.0; - command.twist.linear.y = -velocity; - command.twist_frame = RobotCommand::FRAME_BODY; + command.motion_intent.linear = motion::intents::linear::VelocityIntent{ + ateam_geometry::Vector{0.0, velocity}, + motion::intents::linear::Frame::Local + }; return command; } }; From 766221ba56cea7ec4ec9c49b79d7bb76cd5e0403 Mon Sep 17 00:00:00 2001 From: Matthew Barulic Date: Wed, 17 Sep 2025 01:51:32 -0400 Subject: [PATCH 32/35] Re-adds all plays to PlaySelector --- ateam_kenobi/src/core/play_selector.cpp | 98 ++++++++++++------------- ateam_kenobi/src/plays/all_plays.hpp | 30 ++++---- 2 files changed, 64 insertions(+), 64 deletions(-) diff --git a/ateam_kenobi/src/core/play_selector.cpp b/ateam_kenobi/src/core/play_selector.cpp index bd988d5da..8826d4bdf 100644 --- a/ateam_kenobi/src/core/play_selector.cpp +++ b/ateam_kenobi/src/core/play_selector.cpp @@ -43,56 +43,56 @@ PlaySelector::PlaySelector(rclcpp::Node & node) "stp_parameters", node.get_node_parameters_interface()); halt_play_ = addPlay(stp_options); - // addPlay(stp_options); - // addPlay("TheirLeftLineup", stp_options, 1.0, 1.0); - // addPlay("TheirRightLineup", stp_options, 1.0, -1.0); - // addPlay("OurLeftLineup", stp_options, -1.0, 1.0); - // addPlay("OurRightLineup", stp_options, -1.0, -1.0); - // addPlay(stp_options); - // addPlay(stp_options); - // addPlay(stp_options); + addPlay(stp_options); + addPlay("TheirLeftLineup", stp_options, 1.0, 1.0); + addPlay("TheirRightLineup", stp_options, 1.0, -1.0); + addPlay("OurLeftLineup", stp_options, -1.0, 1.0); + addPlay("OurRightLineup", stp_options, -1.0, -1.0); + addPlay(stp_options); + addPlay(stp_options); + addPlay(stp_options); addPlay(stp_options); - // addPlay(stp_options); - // addPlay(stp_options); - // addPlay(stp_options); - // addPlay(stp_options); - // addPlay(stp_options); - // addPlay(stp_options); - // addPlay(stp_options); - // addPlay(stp_options); - // addPlay(stp_options); - // addPlay(stp_options); - // addPlay(stp_options); - // addPlay(stp_options); - // addPlay(stp_options); - // addPlay(stp_options); - // addPlay(stp_options); - // addPlay(stp_options); - // addPlay(stp_options); - // addPlay(stp_options); - // addPlay(stp_options); - // addPlay( - // "PassLeftForwardPlay", stp_options, play_helpers::lanes::Lane::Left, - // PassToLanePlay::PassDirection::Forward); - // addPlay( - // "PassCenterForwardPlay", stp_options, play_helpers::lanes::Lane::Center, - // PassToLanePlay::PassDirection::Forward); - // addPlay( - // "PassRightForwardPlay", stp_options, play_helpers::lanes::Lane::Right, - // PassToLanePlay::PassDirection::Forward); - // addPlay( - // "PassLeftBackwardPlay", stp_options, play_helpers::lanes::Lane::Left, - // PassToLanePlay::PassDirection::Backward); - // addPlay( - // "PassCenterBackwardPlay", stp_options, play_helpers::lanes::Lane::Center, - // PassToLanePlay::PassDirection::Backward); - // addPlay( - // "PassRightBackwardPlay", stp_options, play_helpers::lanes::Lane::Right, - // PassToLanePlay::PassDirection::Backward); - // addPlay(stp_options); - // addPlay(stp_options); - // addPlay(stp_options); - // addPlay(stp_options); + addPlay(stp_options); + addPlay(stp_options); + addPlay(stp_options); + addPlay(stp_options); + addPlay(stp_options); + addPlay(stp_options); + addPlay(stp_options); + addPlay(stp_options); + addPlay(stp_options); + addPlay(stp_options); + addPlay(stp_options); + addPlay(stp_options); + addPlay(stp_options); + addPlay(stp_options); + addPlay(stp_options); + addPlay(stp_options); + addPlay(stp_options); + addPlay(stp_options); + addPlay(stp_options); + addPlay( + "PassLeftForwardPlay", stp_options, play_helpers::lanes::Lane::Left, + PassToLanePlay::PassDirection::Forward); + addPlay( + "PassCenterForwardPlay", stp_options, play_helpers::lanes::Lane::Center, + PassToLanePlay::PassDirection::Forward); + addPlay( + "PassRightForwardPlay", stp_options, play_helpers::lanes::Lane::Right, + PassToLanePlay::PassDirection::Forward); + addPlay( + "PassLeftBackwardPlay", stp_options, play_helpers::lanes::Lane::Left, + PassToLanePlay::PassDirection::Backward); + addPlay( + "PassCenterBackwardPlay", stp_options, play_helpers::lanes::Lane::Center, + PassToLanePlay::PassDirection::Backward); + addPlay( + "PassRightBackwardPlay", stp_options, play_helpers::lanes::Lane::Right, + PassToLanePlay::PassDirection::Backward); + addPlay(stp_options); + addPlay(stp_options); + addPlay(stp_options); + addPlay(stp_options); } stp::Play * PlaySelector::getPlay(const World & world, ateam_msgs::msg::PlaybookState & state_msg) diff --git a/ateam_kenobi/src/plays/all_plays.hpp b/ateam_kenobi/src/plays/all_plays.hpp index 712e037b2..4b51dc4fc 100644 --- a/ateam_kenobi/src/plays/all_plays.hpp +++ b/ateam_kenobi/src/plays/all_plays.hpp @@ -23,21 +23,21 @@ #define PLAYS__ALL_PLAYS_HPP_ #include "halt_play.hpp" -// #include "kick_on_goal_play.hpp" -// #include "our_ball_placement_play.hpp" -// #include "their_ball_placement_play.hpp" +#include "kick_on_goal_play.hpp" +#include "our_ball_placement_play.hpp" +#include "their_ball_placement_play.hpp" #include "wall_play.hpp" -// #include "basic_122.hpp" -// #include "defenders_only_play.hpp" -// #include "our_penalty_play.hpp" -// #include "their_penalty_play.hpp" -// #include "defense_play.hpp" -// #include "extract_play.hpp" -// #include "passing_plays/all_passing_plays.hpp" -// #include "free_kick_plays/all_free_kick_plays.hpp" -// #include "kickoff_plays/all_kickoff_plays.hpp" -// #include "stop_plays/all_stop_plays.hpp" -// #include "test_plays/all_test_plays.hpp" -// #include "util_plays/all_util_plays.hpp" +#include "basic_122.hpp" +#include "defenders_only_play.hpp" +#include "our_penalty_play.hpp" +#include "their_penalty_play.hpp" +#include "defense_play.hpp" +#include "extract_play.hpp" +#include "passing_plays/all_passing_plays.hpp" +#include "free_kick_plays/all_free_kick_plays.hpp" +#include "kickoff_plays/all_kickoff_plays.hpp" +#include "stop_plays/all_stop_plays.hpp" +#include "test_plays/all_test_plays.hpp" +#include "util_plays/all_util_plays.hpp" #endif // PLAYS__ALL_PLAYS_HPP_ From b68476f53712ece04cd4b3f4f58475da59d1498c Mon Sep 17 00:00:00 2001 From: Matthew Barulic Date: Wed, 17 Sep 2025 01:57:02 -0400 Subject: [PATCH 33/35] Fixes command message published frame --- ateam_kenobi/src/kenobi_node.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/ateam_kenobi/src/kenobi_node.cpp b/ateam_kenobi/src/kenobi_node.cpp index 7252b5b5e..bed24343b 100644 --- a/ateam_kenobi/src/kenobi_node.cpp +++ b/ateam_kenobi/src/kenobi_node.cpp @@ -489,7 +489,9 @@ class KenobiNode : public rclcpp::Node ros_cmd.twist.linear.x = linear_vel.x(); ros_cmd.twist.linear.y = linear_vel.y(); ros_cmd.twist.angular.z = motion_cmd.angular; - ros_cmd.twist_frame = ateam_msgs::msg::RobotMotionCommand::FRAME_BODY; + ros_cmd.twist_frame = + use_world_vels ? ateam_msgs::msg::RobotMotionCommand::FRAME_WORLD : + ateam_msgs::msg::RobotMotionCommand::FRAME_BODY; } } From 8f0c8d08cc2c45a656cc776cde8e9fdae6fb9548 Mon Sep 17 00:00:00 2001 From: Matthew Barulic Date: Sun, 21 Sep 2025 22:36:06 -0400 Subject: [PATCH 34/35] Adds default initializers to BodyVelocity --- ateam_kenobi/src/core/motion/motion_intent.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ateam_kenobi/src/core/motion/motion_intent.hpp b/ateam_kenobi/src/core/motion/motion_intent.hpp index f064c2abe..8c6fc01ca 100644 --- a/ateam_kenobi/src/core/motion/motion_intent.hpp +++ b/ateam_kenobi/src/core/motion/motion_intent.hpp @@ -94,8 +94,8 @@ struct FaceTravelIntent {}; /// @brief Intended command velocity in local frame struct BodyVelocity { - ateam_geometry::Vector linear; - double angular; + ateam_geometry::Vector linear{0.0, 0.0}; + double angular = 0.0; }; struct MotionIntent From 4d648b1418af9bacc4eb456d953b81fdbaea9a13 Mon Sep 17 00:00:00 2001 From: Matthew Barulic Date: Sun, 21 Sep 2025 23:12:56 -0400 Subject: [PATCH 35/35] Fixes linting failures. --- .../src/core/motion/motion_controller.cpp | 16 +++++++++++----- .../src/core/motion/motion_controller.hpp | 3 ++- ateam_kenobi/src/core/motion/motion_executor.cpp | 5 +++-- ateam_kenobi/src/core/motion/motion_executor.hpp | 6 +++--- ateam_kenobi/src/core/motion/motion_intent.hpp | 9 +++++---- .../src/core/path_planning/planner_options.hpp | 4 ++-- ateam_kenobi/src/core/types/robot_command.hpp | 8 ++++---- ateam_kenobi/src/plays/halt_play.cpp | 3 ++- .../src/plays/our_ball_placement_play.cpp | 9 ++++++--- .../src/plays/our_ball_placement_play.hpp | 8 ++++++-- ateam_kenobi/src/plays/our_penalty_play.cpp | 5 +++-- .../src/plays/stop_plays/default_stop_play.cpp | 2 +- .../src/plays/stop_plays/defensive_stop_play.cpp | 2 +- .../src/plays/stop_plays/offensive_stop_play.cpp | 2 +- .../src/plays/stop_plays/stop_helpers.cpp | 3 ++- .../src/plays/test_plays/controls_test_play.cpp | 3 ++- .../src/plays/test_plays/spinning_a_play.cpp | 2 +- .../src/plays/test_plays/test_pivot_play.hpp | 10 +++++----- ateam_kenobi/src/plays/test_plays/test_play.cpp | 3 ++- .../src/plays/their_ball_placement_play.cpp | 8 +++++--- .../src/plays/their_ball_placement_play.hpp | 8 ++++++-- ateam_kenobi/src/plays/their_penalty_play.cpp | 5 +++-- ateam_kenobi/src/skills/dribble.cpp | 3 ++- ateam_kenobi/src/skills/goalie.cpp | 4 +++- ateam_kenobi/src/skills/pass_receiver.cpp | 13 ++++++++----- 25 files changed, 89 insertions(+), 55 deletions(-) diff --git a/ateam_kenobi/src/core/motion/motion_controller.cpp b/ateam_kenobi/src/core/motion/motion_controller.cpp index ad0049b48..e9232070c 100644 --- a/ateam_kenobi/src/core/motion/motion_controller.cpp +++ b/ateam_kenobi/src/core/motion/motion_controller.cpp @@ -34,7 +34,8 @@ #include "pid.hpp" #include "frame_conversions.hpp" -namespace ateam_kenobi::motion { +namespace ateam_kenobi::motion +{ MotionController::MotionController(rclcpp::Logger logger) : logger_(logger) @@ -121,7 +122,8 @@ void MotionController::calculate_trajectory_velocity_limits(const MotionOptions const ateam_geometry::Vector prev_direction = point - prev_point; double angle = ateam_geometry::ShortestAngleBetween(direction, prev_direction); - max_turn_velocity = (abs(angle) > options.max_allowed_turn_angle) ? 0.5 : options.max_velocity; + max_turn_velocity = (abs(angle) > + options.max_allowed_turn_angle) ? 0.5 : options.max_velocity; } double selected_velocity = std::clamp(std::min(max_decel_velocity, max_turn_velocity), @@ -195,7 +197,9 @@ double MotionController::calculate_trapezoidal_angular_vel( const double decel_direction = std::copysign(1, vel * angle_error); // Decelerate to target velocity - if (decel_direction > 0 && abs(deceleration_to_reach_target) > options.max_angular_acceleration * 0.95) { + if (decel_direction > 0 && + abs(deceleration_to_reach_target) > options.max_angular_acceleration * 0.95) + { trapezoidal_vel = vel - (error_direction * deceleration_to_reach_target * dt); // Accelerate to speed @@ -340,7 +344,8 @@ BodyVelocity MotionController::get_command( // Calculate trapezoidal velocity feedforward calculate_trajectory_velocity_limits(options); - double calculated_velocity = calculate_trapezoidal_velocity(options, robot, target, target_index, dt); + double calculated_velocity = calculate_trapezoidal_velocity(options, robot, target, + target_index, dt); ateam_geometry::Vector vel_vector; @@ -410,7 +415,8 @@ BodyVelocity MotionController::get_command( } } } - motion_command.angular = std::clamp(t_command, -options.max_angular_velocity, options.max_angular_velocity); + motion_command.angular = std::clamp(t_command, -options.max_angular_velocity, + options.max_angular_velocity); } } diff --git a/ateam_kenobi/src/core/motion/motion_controller.hpp b/ateam_kenobi/src/core/motion/motion_controller.hpp index e023100b6..98a5eac8a 100644 --- a/ateam_kenobi/src/core/motion/motion_controller.hpp +++ b/ateam_kenobi/src/core/motion/motion_controller.hpp @@ -33,7 +33,8 @@ #include "motion_options.hpp" #include "motion_intent.hpp" -namespace ateam_kenobi::motion { +namespace ateam_kenobi::motion +{ // cause the robot to: always face a point, face in the direction of travel, or stay facing the // same direction diff --git a/ateam_kenobi/src/core/motion/motion_executor.cpp b/ateam_kenobi/src/core/motion/motion_executor.cpp index ca1cda86b..10599a725 100644 --- a/ateam_kenobi/src/core/motion/motion_executor.cpp +++ b/ateam_kenobi/src/core/motion/motion_executor.cpp @@ -19,6 +19,8 @@ // THE SOFTWARE. #include "motion_executor.hpp" +#include +#include #include #include "core/path_planning/obstacles.hpp" #include "core/path_planning/escape_velocity.hpp" @@ -34,7 +36,6 @@ struct overloads : Ts ... { using Ts::operator() ...; }; MotionExecutor::MotionExecutor(rclcpp::Logger logger) : logger_(std::move(logger)) { - } std::array, @@ -236,4 +237,4 @@ std::pair MotionExecutor::ProjectRobotOnPath( return {closest_index, closest_point}; } -} // namespace ateam_kenobi::motion +} // namespace ateam_kenobi::motion diff --git a/ateam_kenobi/src/core/motion/motion_executor.hpp b/ateam_kenobi/src/core/motion/motion_executor.hpp index e0d63c6eb..5bd702eba 100644 --- a/ateam_kenobi/src/core/motion/motion_executor.hpp +++ b/ateam_kenobi/src/core/motion/motion_executor.hpp @@ -23,6 +23,7 @@ #include #include +#include #include #include "core/path_planning/path_planner.hpp" #include "core/visualization/overlays.hpp" @@ -35,7 +36,7 @@ namespace ateam_kenobi::motion class MotionExecutor { public: - MotionExecutor(rclcpp::Logger logger); + explicit MotionExecutor(rclcpp::Logger logger); std::array, 16> RunFrame( @@ -58,9 +59,8 @@ class MotionExecutor std::pair ProjectRobotOnPath( const path_planning::Path & path, const Robot & robot); - }; -} // namespace ateam_kenobi::motion +} // namespace ateam_kenobi::motion #endif // CORE__MOTION__MOTION_EXECUTOR_HPP_ diff --git a/ateam_kenobi/src/core/motion/motion_intent.hpp b/ateam_kenobi/src/core/motion/motion_intent.hpp index 8c6fc01ca..9f94d8161 100644 --- a/ateam_kenobi/src/core/motion/motion_intent.hpp +++ b/ateam_kenobi/src/core/motion/motion_intent.hpp @@ -18,12 +18,13 @@ // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN // THE SOFTWARE. -#ifndef CORE__MOTION__MOTION_TARGET_HPP_ -#define CORE__MOTION__MOTION_TARGET_HPP_ +#ifndef CORE__MOTION__MOTION_INTENT_HPP_ +#define CORE__MOTION__MOTION_INTENT_HPP_ #include #include #include +#include #include #include "core/types/world.hpp" #include "core/types/robot.hpp" @@ -118,6 +119,6 @@ struct MotionIntent bool enable_escape_velocities = true; }; -} // namespace ateam_kenobi::motion +} // namespace ateam_kenobi::motion -#endif // CORE__MOTION__MOTION_TARGET_HPP_ +#endif // CORE__MOTION__MOTION_INTENT_HPP_ diff --git a/ateam_kenobi/src/core/path_planning/planner_options.hpp b/ateam_kenobi/src/core/path_planning/planner_options.hpp index 34ce82938..87f5f3e18 100644 --- a/ateam_kenobi/src/core/path_planning/planner_options.hpp +++ b/ateam_kenobi/src/core/path_planning/planner_options.hpp @@ -75,8 +75,8 @@ struct PlannerOptions ReplanThresholds replan_thresholds; }; - -} // namespace ateam_kenobi::path_planning + +} // namespace ateam_kenobi::path_planning #endif // CORE__PATH_PLANNING__PLANNER_OPTIONS_HPP_ diff --git a/ateam_kenobi/src/core/types/robot_command.hpp b/ateam_kenobi/src/core/types/robot_command.hpp index 3a67ad3d3..f4be711bf 100644 --- a/ateam_kenobi/src/core/types/robot_command.hpp +++ b/ateam_kenobi/src/core/types/robot_command.hpp @@ -18,8 +18,8 @@ // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN // THE SOFTWARE. -#ifndef CORE__TYPES_ROBOT_COMMAND_HPP_ -#define CORE__TYPES_ROBOT_COMMAND_HPP_ +#ifndef CORE__TYPES__ROBOT_COMMAND_HPP_ +#define CORE__TYPES__ROBOT_COMMAND_HPP_ #include "core/motion/motion_intent.hpp" @@ -41,7 +41,7 @@ enum class KickState : uint8_t struct RobotCommand { motion::MotionIntent motion_intent; - + KickState kick = KickState::Arm; double kick_speed = 0.0; @@ -50,4 +50,4 @@ struct RobotCommand } // namespace ateam_kenobi -#endif // CORE__TYPES_ROBOT_COMMAND_HPP_ +#endif // CORE__TYPES__ROBOT_COMMAND_HPP_ diff --git a/ateam_kenobi/src/plays/halt_play.cpp b/ateam_kenobi/src/plays/halt_play.cpp index 09ccc14d8..768d0ce9b 100644 --- a/ateam_kenobi/src/plays/halt_play.cpp +++ b/ateam_kenobi/src/plays/halt_play.cpp @@ -52,7 +52,8 @@ std::array, 16> HaltPlay::runFrame( { std::array, 16> halt_motion_commands; RobotCommand command; - command.motion_intent.linear = motion::intents::linear::VelocityIntent{ateam_geometry::Vector{0.0, 0.0}}; + command.motion_intent.linear = motion::intents::linear::VelocityIntent{ateam_geometry::Vector{0.0, + 0.0}}; command.motion_intent.angular = motion::intents::angular::VelocityIntent{0.0}; for (size_t i = 0; i < 16; ++i) { halt_motion_commands[i] = command; diff --git a/ateam_kenobi/src/plays/our_ball_placement_play.cpp b/ateam_kenobi/src/plays/our_ball_placement_play.cpp index 4e32adcc0..42b001479 100644 --- a/ateam_kenobi/src/plays/our_ball_placement_play.cpp +++ b/ateam_kenobi/src/plays/our_ball_placement_play.cpp @@ -21,6 +21,7 @@ #include "our_ball_placement_play.hpp" #include +#include #include #include #include "core/play_helpers/available_robots.hpp" @@ -314,8 +315,8 @@ void OurBallPlacementPlay::runExtracting( if (world.ball.visible) { motion_command.motion_intent.angular = motion::intents::angular::FacingIntent{world.ball.pos}; } else { - // If the ball is occluded we sometimes drive past its previous position and try to turn around - // so its better to just keep facing the same direction if we lose track of it + // If the ball is occluded we sometimes drive past its previous position and try to turn + // around so its better to just keep facing the same direction if we lose track of it motion_command.motion_intent.angular = motion::intents::angular::HeadingIntent{extract_robot.theta}; } @@ -329,7 +330,9 @@ void OurBallPlacementPlay::runExtracting( motion_command.motion_intent.motion_options.max_deceleration = 2.0; motion_command.motion_intent.linear = motion::intents::linear::PositionIntent{approach_point_}; - motion_command.motion_intent.angular = motion::intents::angular::HeadingIntent{ateam_geometry::ToHeading(world.ball.pos - approach_point_)}; + motion_command.motion_intent.angular = + motion::intents::angular::HeadingIntent{ateam_geometry::ToHeading(world.ball.pos - + approach_point_)}; } const bool should_dribble = extract_robot.breakbeam_ball_detected_filtered || diff --git a/ateam_kenobi/src/plays/our_ball_placement_play.hpp b/ateam_kenobi/src/plays/our_ball_placement_play.hpp index 002acd220..7188a20a7 100644 --- a/ateam_kenobi/src/plays/our_ball_placement_play.hpp +++ b/ateam_kenobi/src/plays/our_ball_placement_play.hpp @@ -93,9 +93,13 @@ class OurBallPlacementPlay : public stp::Play const ateam_geometry::Point & ball_pos, const ateam_geometry::Point & placement_point); - bool shouldRobotMove(const World & world, const ateam_geometry::Point & placement_point, const Robot & robot); + bool shouldRobotMove( + const World & world, const ateam_geometry::Point & placement_point, + const Robot & robot); - ateam_geometry::Point getTargetPoint(const World & world, const ateam_geometry::Point & placement_point, const Robot & robot); + ateam_geometry::Point getTargetPoint( + const World & world, + const ateam_geometry::Point & placement_point, const Robot & robot); }; } // namespace ateam_kenobi::plays diff --git a/ateam_kenobi/src/plays/our_penalty_play.cpp b/ateam_kenobi/src/plays/our_penalty_play.cpp index 65552b32d..26d4edddd 100644 --- a/ateam_kenobi/src/plays/our_penalty_play.cpp +++ b/ateam_kenobi/src/plays/our_penalty_play.cpp @@ -21,6 +21,7 @@ #include "our_penalty_play.hpp" #include +#include #include "core/play_helpers/available_robots.hpp" #include "core/play_helpers/window_evaluation.hpp" @@ -104,7 +105,7 @@ std::array, 16> OurPenaltyPlay::runFrame( ateam_geometry::Vector pattern_step(kRobotDiameter + 0.2, 0.0); std::vector target_points; std::generate_n(std::back_inserter(target_points), available_robots.size(), - [pos = pattern_point, step = pattern_step,&world]() mutable { + [pos = pattern_point, step = pattern_step, &world]() mutable { auto current = pos; pos = pos + step; if (pos.x() > (world.field.field_length / 2.0) - kRobotDiameter) { @@ -117,7 +118,7 @@ std::array, 16> OurPenaltyPlay::runFrame( multi_move_to_.SetTargetPoints(target_points); multi_move_to_.SetFaceTravel(); for(auto & maybe_cmd : motion_commands) { - if(!maybe_cmd) continue; + if(!maybe_cmd) {continue;} maybe_cmd->motion_intent.motion_options.max_velocity = 1.5; } multi_move_to_.RunFrame(available_robots, motion_commands); diff --git a/ateam_kenobi/src/plays/stop_plays/default_stop_play.cpp b/ateam_kenobi/src/plays/stop_plays/default_stop_play.cpp index 8f2ff7932..f0d83fd7b 100644 --- a/ateam_kenobi/src/plays/stop_plays/default_stop_play.cpp +++ b/ateam_kenobi/src/plays/stop_plays/default_stop_play.cpp @@ -73,7 +73,7 @@ std::array, 16> DefaultStopPlay::runFrame( // Rules say <1.5m/s. We'll use 1m/s to give some room for error. for(auto & maybe_cmd : motion_commands) { - if(!maybe_cmd) continue; + if(!maybe_cmd) {continue;} maybe_cmd->motion_intent.motion_options.max_velocity = 1.0; } diff --git a/ateam_kenobi/src/plays/stop_plays/defensive_stop_play.cpp b/ateam_kenobi/src/plays/stop_plays/defensive_stop_play.cpp index ce3e984b5..37d4f19f1 100644 --- a/ateam_kenobi/src/plays/stop_plays/defensive_stop_play.cpp +++ b/ateam_kenobi/src/plays/stop_plays/defensive_stop_play.cpp @@ -84,7 +84,7 @@ std::array, 16> DefensiveStopPlay::runFrame( // Rules say <1.5m/s. We'll use 1m/s to give some room for error. for(auto & maybe_cmd : motion_commands) { - if(!maybe_cmd) continue; + if(!maybe_cmd) {continue;} maybe_cmd->motion_intent.motion_options.max_velocity = 1.0; } diff --git a/ateam_kenobi/src/plays/stop_plays/offensive_stop_play.cpp b/ateam_kenobi/src/plays/stop_plays/offensive_stop_play.cpp index d4937674f..f5f232cd0 100644 --- a/ateam_kenobi/src/plays/stop_plays/offensive_stop_play.cpp +++ b/ateam_kenobi/src/plays/stop_plays/offensive_stop_play.cpp @@ -85,7 +85,7 @@ std::array, 16> OffensiveStopPlay::runFrame( // Rules say <1.5m/s. We'll use 1m/s to give some room for error. for(auto & maybe_cmd : motion_commands) { - if(!maybe_cmd) continue; + if(!maybe_cmd) {continue;} maybe_cmd->motion_intent.motion_options.max_velocity = 1.0; } diff --git a/ateam_kenobi/src/plays/stop_plays/stop_helpers.cpp b/ateam_kenobi/src/plays/stop_plays/stop_helpers.cpp index 8b14e2ea8..afbf82da2 100644 --- a/ateam_kenobi/src/plays/stop_plays/stop_helpers.cpp +++ b/ateam_kenobi/src/plays/stop_plays/stop_helpers.cpp @@ -259,7 +259,8 @@ void moveBotsTooCloseToBall( RobotCommand command; command.motion_intent.linear = motion::intents::linear::PositionIntent{spot}; command.motion_intent.angular = motion::intents::angular::FacingIntent{world.ball.pos}; - command.motion_intent.planner_options.use_default_obstacles = bot.id != world.referee_info.our_goalie_id; + command.motion_intent.planner_options.use_default_obstacles = bot.id != + world.referee_info.our_goalie_id; command.motion_intent.obstacles = added_obstacles; motion_commands[bot.id] = command; overlays.drawCircle( diff --git a/ateam_kenobi/src/plays/test_plays/controls_test_play.cpp b/ateam_kenobi/src/plays/test_plays/controls_test_play.cpp index 4e5ed1ac7..aea0c6c33 100644 --- a/ateam_kenobi/src/plays/test_plays/controls_test_play.cpp +++ b/ateam_kenobi/src/plays/test_plays/controls_test_play.cpp @@ -120,7 +120,8 @@ std::array, 16> ControlsTestPlay::runFrame( motion_options); RobotCommand command; - command.motion_intent.linear = motion::intents::linear::VelocityIntent{body_vel.linear, motion::intents::linear::Frame::World}; + command.motion_intent.linear = motion::intents::linear::VelocityIntent{body_vel.linear, + motion::intents::linear::Frame::World}; command.motion_intent.angular = motion::intents::angular::VelocityIntent{body_vel.angular}; maybe_motion_commands[robot.id] = command; diff --git a/ateam_kenobi/src/plays/test_plays/spinning_a_play.cpp b/ateam_kenobi/src/plays/test_plays/spinning_a_play.cpp index 83ad87ef4..c05b94d5c 100644 --- a/ateam_kenobi/src/plays/test_plays/spinning_a_play.cpp +++ b/ateam_kenobi/src/plays/test_plays/spinning_a_play.cpp @@ -68,7 +68,7 @@ std::array, multi_move_to_.SetFaceAbsolue(-M_PI_2 + angle_); multi_move_to_.RunFrame(available_robots, motion_commands); for(auto & maybe_motion_command : motion_commands) { - if(!maybe_motion_command) continue; + if(!maybe_motion_command) {continue;} maybe_motion_command->motion_intent.planner_options.footprint_inflation = 0.1; } diff --git a/ateam_kenobi/src/plays/test_plays/test_pivot_play.hpp b/ateam_kenobi/src/plays/test_plays/test_pivot_play.hpp index 19e74fab1..7a252de45 100644 --- a/ateam_kenobi/src/plays/test_plays/test_pivot_play.hpp +++ b/ateam_kenobi/src/plays/test_plays/test_pivot_play.hpp @@ -87,11 +87,11 @@ class TestPivotPlay : public stp::Play motion_commands[robot.id]->motion_intent.callback = [&play_info]( motion::BodyVelocity vel, const path_planning::Path &, const Robot &, const World &) - { - play_info["Y Cmd"] = vel.linear.y(); - play_info["Omega Cmd"] = vel.angular; - return vel; - }; + { + play_info["Y Cmd"] = vel.linear.y(); + play_info["Omega Cmd"] = vel.angular; + return vel; + }; return motion_commands; } diff --git a/ateam_kenobi/src/plays/test_plays/test_play.cpp b/ateam_kenobi/src/plays/test_plays/test_play.cpp index 6e6b4946a..a04e7c64d 100644 --- a/ateam_kenobi/src/plays/test_plays/test_play.cpp +++ b/ateam_kenobi/src/plays/test_plays/test_play.cpp @@ -48,7 +48,8 @@ std::array, 16> TestPlay::runFrame( const auto & robot = current_available_robots[0]; int robot_id = robot.id; RobotCommand command; - command.motion_intent.linear = motion::intents::linear::PositionIntent{world.ball.pos + ateam_geometry::Vector(-.2, 0)}; + command.motion_intent.linear = motion::intents::linear::PositionIntent{world.ball.pos + + ateam_geometry::Vector(-.2, 0)}; command.motion_intent.angular = motion::intents::angular::FacingIntent{world.ball.pos}; maybe_motion_commands.at(robot_id) = command; } diff --git a/ateam_kenobi/src/plays/their_ball_placement_play.cpp b/ateam_kenobi/src/plays/their_ball_placement_play.cpp index 1211863e7..51040bfd2 100644 --- a/ateam_kenobi/src/plays/their_ball_placement_play.cpp +++ b/ateam_kenobi/src/plays/their_ball_placement_play.cpp @@ -21,6 +21,8 @@ #include "their_ball_placement_play.hpp" #include +#include +#include #include #include "core/play_helpers/available_robots.hpp" #include @@ -65,8 +67,8 @@ std::array, 16> TheirBallPlacementPlay::runFrame( DrawKeepoutArea(world.ball.pos, placement_point); const auto partition_iter = std::partition(available_robots.begin(), available_robots.end(), - [this, &world, &placement_point](const Robot & robot) { - return shouldRobotMove(world, placement_point, robot); + [this, &world, &placement_point](const Robot & robot) { + return shouldRobotMove(world, placement_point, robot); }); const std::vector robots_to_move(available_robots.begin(), partition_iter); @@ -85,7 +87,7 @@ std::array, 16> TheirBallPlacementPlay::runFrame( [this, &world, &placement_point](const Robot & robot) { return getTargetPoint(world, placement_point, robot); }); - + multi_move_to_.SetTargetPoints(target_points); multi_move_to_.SetFacePoint(world.ball.pos); multi_move_to_.RunFrame(robots_to_move, maybe_motion_commands); diff --git a/ateam_kenobi/src/plays/their_ball_placement_play.hpp b/ateam_kenobi/src/plays/their_ball_placement_play.hpp index 3c8c29f16..504e0af03 100644 --- a/ateam_kenobi/src/plays/their_ball_placement_play.hpp +++ b/ateam_kenobi/src/plays/their_ball_placement_play.hpp @@ -46,9 +46,13 @@ class TheirBallPlacementPlay : public stp::Play const ateam_geometry::Point & ball_pos, const ateam_geometry::Point & placement_point); - bool shouldRobotMove(const World & world, const ateam_geometry::Point & placement_point, const Robot & robot); + bool shouldRobotMove( + const World & world, const ateam_geometry::Point & placement_point, + const Robot & robot); - ateam_geometry::Point getTargetPoint(const World & world, const ateam_geometry::Point & placement_point, const Robot & robot); + ateam_geometry::Point getTargetPoint( + const World & world, + const ateam_geometry::Point & placement_point, const Robot & robot); }; } // namespace ateam_kenobi::plays diff --git a/ateam_kenobi/src/plays/their_penalty_play.cpp b/ateam_kenobi/src/plays/their_penalty_play.cpp index 94288f683..57a8badaa 100644 --- a/ateam_kenobi/src/plays/their_penalty_play.cpp +++ b/ateam_kenobi/src/plays/their_penalty_play.cpp @@ -21,6 +21,7 @@ #include "their_penalty_play.hpp" #include +#include #include "core/play_helpers/available_robots.hpp" #include @@ -66,7 +67,7 @@ std::array, 16> TheirPenaltyPlay::runFrame( std::vector target_points; std::generate_n(std::back_inserter(target_points), available_robots.size(), - [pos = pattern_start, step = pattern_step,&world]() mutable { + [pos = pattern_start, step = pattern_step, &world]() mutable { auto current = pos; pos = pos + step; if (pos.x() > (world.field.field_length / 2.0) - kRobotDiameter) { @@ -80,7 +81,7 @@ std::array, 16> TheirPenaltyPlay::runFrame( multi_move_to_.SetFaceTravel(); multi_move_to_.RunFrame(available_robots, motion_commands); for(auto & maybe_cmd : motion_commands) { - if(!maybe_cmd) continue; + if(!maybe_cmd) {continue;} maybe_cmd->motion_intent.motion_options.max_velocity = 1.5; } diff --git a/ateam_kenobi/src/skills/dribble.cpp b/ateam_kenobi/src/skills/dribble.cpp index b7afbfc6a..b0d9da8cb 100644 --- a/ateam_kenobi/src/skills/dribble.cpp +++ b/ateam_kenobi/src/skills/dribble.cpp @@ -212,7 +212,8 @@ RobotCommand Dribble::runDribble(const Robot & robot) command.motion_intent.angular = motion::intents::angular::FacingIntent{target_}; const auto robot_to_target = target_ - robot.pos; - const auto position_target = target_ - (kRobotRadius * ateam_geometry::normalize(robot_to_target)); + const auto position_target = target_ - + (kRobotRadius * ateam_geometry::normalize(robot_to_target)); command.motion_intent.linear = motion::intents::linear::PositionIntent{position_target}; command.motion_intent.motion_options.max_velocity = 0.35; diff --git a/ateam_kenobi/src/skills/goalie.cpp b/ateam_kenobi/src/skills/goalie.cpp index 7a14b0e3e..a8ab46cf5 100644 --- a/ateam_kenobi/src/skills/goalie.cpp +++ b/ateam_kenobi/src/skills/goalie.cpp @@ -171,7 +171,9 @@ RobotCommand Goalie::runDefaultBehavior( RobotCommand command; command.motion_intent.planner_options = default_planner_options_; - command.motion_intent.linear = motion::intents::linear::PositionIntent{ateam_geometry::nearestPointOnSegment(goalie_line, ball_state.pos)}; + command.motion_intent.linear = + motion::intents::linear::PositionIntent{ateam_geometry::nearestPointOnSegment(goalie_line, + ball_state.pos)}; command.motion_intent.angular = motion::intents::angular::HeadingIntent{M_PI_2}; command.motion_intent.obstacles = getCustomObstacles(world); return command; diff --git a/ateam_kenobi/src/skills/pass_receiver.cpp b/ateam_kenobi/src/skills/pass_receiver.cpp index 4f93abf8a..3fe62823b 100644 --- a/ateam_kenobi/src/skills/pass_receiver.cpp +++ b/ateam_kenobi/src/skills/pass_receiver.cpp @@ -53,7 +53,7 @@ RobotCommand PassReceiver::runFrame(const World & world, const Robot & robot) const auto ball_stalled_and_reachable = isBallStalledAndReachable(world, robot); RobotCommand command; - + if (bot_close_to_target && ball_vel_matching_bot_vel && ball_close) { done_ = true; getPlayInfo()["State"] = "Done"; @@ -157,7 +157,7 @@ RobotCommand PassReceiver::runPass(const World & world, const Robot & robot) command.motion_intent.angular = motion::intents::angular::FacingIntent{world.ball.pos}; command.motion_intent.motion_options.max_deceleration = 4.0; command.motion_intent.planner_options.avoid_ball = false; - + command.dribbler_speed = kDefaultDribblerSpeed * 1.2; const auto dist_to_ball = ateam_geometry::norm(robot.pos - world.ball.pos); const auto time_to_ball = dist_to_ball / ateam_geometry::norm(world.ball.vel); @@ -167,10 +167,13 @@ RobotCommand PassReceiver::runPass(const World & world, const Robot & robot) const auto angle_between_vecs = ateam_geometry::ShortestAngleBetween(world.ball.vel, robot_backwards_vec); if(std::abs(angle_between_vecs) < M_PI_2) { - command.motion_intent.callback = [](motion::BodyVelocity plan_velocity, - const path_planning::Path &, const Robot & robot, const World & world) -> motion::BodyVelocity { + command.motion_intent.callback = [](motion::BodyVelocity plan_velocity, + const path_planning::Path &, const Robot & robot, + const World & world) -> motion::BodyVelocity { const auto multiplier = robot.breakbeam_ball_detected_filtered ? 0.2 : 0.6; - plan_velocity.linear += motion::WorldToLocalFrame(ateam_geometry::normalize(world.ball.vel) * multiplier, robot); + plan_velocity.linear += + motion::WorldToLocalFrame(ateam_geometry::normalize(world.ball.vel) * multiplier, + robot); return plan_velocity; }; }