diff --git a/ateam_kenobi/CMakeLists.txt b/ateam_kenobi/CMakeLists.txt index f5ceebb31..d87c9fe11 100644 --- a/ateam_kenobi/CMakeLists.txt +++ b/ateam_kenobi/CMakeLists.txt @@ -35,9 +35,10 @@ 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 - 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/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.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..9dfcd5321 --- /dev/null +++ b/ateam_kenobi/src/core/motion/frame_conversions.hpp @@ -0,0 +1,42 @@ +// 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" +#include "core/motion/motion_intent.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/core/motion/motion_controller.cpp b/ateam_kenobi/src/core/motion/motion_controller.cpp index 2a70aedbe..dec961980 100644 --- a/ateam_kenobi/src/core/motion/motion_controller.cpp +++ b/ateam_kenobi/src/core/motion/motion_controller.cpp @@ -31,7 +31,10 @@ #include "ateam_geometry/ateam_geometry.hpp" #include "ateam_common/robot_constants.hpp" #include "pid.hpp" +#include "frame_conversions.hpp" +namespace ateam_kenobi::motion +{ MotionController::MotionController(rclcpp::Logger logger) : logger_(logger) @@ -87,7 +90,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()); @@ -109,26 +112,28 @@ 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) { @@ -158,21 +163,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) { @@ -185,17 +191,19 @@ 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; @@ -203,15 +211,15 @@ 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( +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) { @@ -233,7 +241,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; @@ -334,8 +342,9 @@ 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; @@ -350,12 +359,11 @@ 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(); - motion_command.twist.linear.y = vel_vector.y(); + motion_command.linear = WorldToLocalFrame(vel_vector, robot); } // calculate angle movement commands @@ -393,20 +401,21 @@ 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.angular = std::clamp(t_command, -options.max_angular_velocity, + options.max_angular_velocity); } } @@ -464,3 +473,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..98a5eac8a 100644 --- a/ateam_kenobi/src/core/motion/motion_controller.hpp +++ b/ateam_kenobi/src/core/motion/motion_controller.hpp @@ -30,6 +30,11 @@ #include "core/types/robot.hpp" #include "core/types/world.hpp" #include "pid.hpp" +#include "motion_options.hpp" +#include "motion_intent.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 +46,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 */ @@ -72,21 +68,23 @@ 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); // 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()); @@ -99,17 +97,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; @@ -132,4 +119,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..10599a725 --- /dev/null +++ b/ateam_kenobi/src/core/motion/motion_executor.cpp @@ -0,0 +1,240 @@ +// 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 +#include +#include "core/path_planning/obstacles.hpp" +#include "core/path_planning/escape_velocity.hpp" +#include "frame_conversions.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()) { + auto controller_vel = controller.get_command(robot, current_time, intent.motion_options); + if (use_controller_linvel) { + body_velocity.linear = controller_vel.linear; + } + if (use_controller_omega) { + body_velocity.angular = controller_vel.angular; + } + } + + 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{ + WorldToLocalFrame(*vel, robot), + 0.0 + }; + } 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() == 1) { + 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)); + 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(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(name_prefix + "afterpath", {path.back(), target_point}, "LightSkyBlue"); + } else if (planner.isPathTruncated()) { + overlays.drawLine(name_prefix + "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..5bd702eba --- /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 +#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: + explicit 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..9f94d8161 --- /dev/null +++ b/ateam_kenobi/src/core/motion/motion_intent.hpp @@ -0,0 +1,124 @@ +// 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_INTENT_HPP_ +#define CORE__MOTION__MOTION_INTENT_HPP_ + +#include +#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{0.0, 0.0}; + double angular = 0.0; +}; + +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_INTENT_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..48bb69bbe --- /dev/null +++ b/ateam_kenobi/src/core/motion/motion_options.hpp @@ -0,0 +1,47 @@ +// 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 + + 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 + +#endif // CORE__MOTION__MOTION_OPTIONS_HPP_ 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/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 6045002fd..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 @@ -37,19 +36,19 @@ constexpr double kSafeEscapeVelocity = 0.3; // m/s * * @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); @@ -57,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/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..87f5f3e18 --- /dev/null +++ b/ateam_kenobi/src/core/path_planning/planner_options.hpp @@ -0,0 +1,82 @@ +// 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_ + +#include +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 deleted file mode 100644 index 3a93ef594..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(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_controller_.v_max = velocity; -} - -void EasyMoveTo::setMaxAngularVelocity(double velocity) -{ - if (velocity > 6.5) { - RCLCPP_WARN(getLogger(), "UNREASONABLY LARGE VELOCITY GIVEN TO SET MAX ANGULAR VELOCITY"); - return; - } - motion_controller_.t_max = velocity; -} - -void EasyMoveTo::setMaxAccel(double accel) -{ - if (accel > 8.0) { - RCLCPP_WARN(getLogger(), "UNREASONABLY LARGE ACCELERATION GIVEN TO SET MAX ACCELERATION"); - return; - } - motion_controller_.accel_limit = accel; -} - -void EasyMoveTo::setMaxDecel(double decel) -{ - if (decel > 8.0) { - RCLCPP_WARN(getLogger(), "UNREASONABLY LARGE DECELERATION GIVEN TO SET MAX DECELERATION"); - return; - } - motion_controller_.decel_limit = decel; -} - -void EasyMoveTo::setMaxThetaAccel(double accel) -{ - if (accel > 8.0) { - RCLCPP_WARN(getLogger(), "UNREASONABLY LARGE ACCELERATION GIVEN TO SET MAX THETA ACCELERATION"); - return; - } - motion_controller_.t_accel_limit = accel; -} - -void EasyMoveTo::setMaxAllowedTurnAngle(double angle) -{ - motion_controller_.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::PathPlanner::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::PathPlanner::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::PathPlanner::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::PathPlanner::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 b2c7c68dd..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(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_; - MotionController motion_controller_; - MotionOptions motion_options_; - - path_planning::PathPlanner::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); - - void drawTrajectoryOverlay(const path_planning::PathPlanner::Path & path, const Robot & robot); - - std::optional generateEscapeVelocity( - const World & world, - const Robot & robot, - std::vector obstacles); - - std::pair ProjectRobotOnPath( - const path_planning::PathPlanner::Path & path, - const Robot & robot); -}; - -} // namespace ateam_kenobi::play_helpers - -#endif // CORE__PLAY_HELPERS__EASY_MOVE_TO_HPP_ 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 new file mode 100644 index 000000000..f4be711bf --- /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 "core/motion/motion_intent.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::MotionIntent motion_intent; + + KickState kick = KickState::Arm; + double kick_speed = 0.0; + + double dribbler_speed = 0.0; +}; + +} // namespace ateam_kenobi + +#endif // CORE__TYPES__ROBOT_COMMAND_HPP_ diff --git a/ateam_kenobi/src/kenobi_node.cpp b/ateam_kenobi/src/kenobi_node.cpp index 85db050b1..b1f7a3992 100644 --- a/ateam_kenobi/src/kenobi_node.cpp +++ b/ateam_kenobi/src/kenobi_node.cpp @@ -51,7 +51,8 @@ #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" #include "core/joystick_enforcer.hpp" @@ -72,6 +73,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(); @@ -184,6 +186,7 @@ class KenobiNode : public rclcpp::Node JoystickEnforcer joystick_enforcer_; visualization::Overlays overlays_; FpsTracker fps_tracker_; + motion::MotionExecutor motion_executor_; rclcpp::Publisher::SharedPtr overlay_publisher_; rclcpp::Publisher::SharedPtr play_info_publisher_; rclcpp::Subscription::SharedPtr ball_subscription_; @@ -444,12 +447,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); } @@ -464,10 +461,48 @@ 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); + + 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]; + auto & maybe_motion_cmd = motion_commands[id]; + 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 = linear_vel.x(); + ros_cmd.twist.linear.y = linear_vel.y(); + ros_cmd.twist.angular.z = motion_cmd.angular; + ros_cmd.twist_frame = + use_world_vels ? ateam_msgs::msg::RobotMotionCommand::FRAME_WORLD : + ateam_msgs::msg::RobotMotionCommand::FRAME_BODY; + } + } + overlays_.merge(play->getOverlays()); overlay_publisher_->publish(overlays_.getMsg()); overlays_.clear(); play->getOverlays().clear(); @@ -479,7 +514,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/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/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: diff --git a/ateam_kenobi/src/plays/halt_play.cpp b/ateam_kenobi/src/plays/halt_play.cpp index 94bcd6e63..768d0ce9b 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) { } @@ -48,14 +47,17 @@ 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 dd86d563f..fac6a40d6 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 @@ -38,12 +36,10 @@ class HaltPlay : public stp::Play void reset() override; - std::array, + std::array, 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/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/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: diff --git a/ateam_kenobi/src/plays/our_ball_placement_play.cpp b/ateam_kenobi/src/plays/our_ball_placement_play.cpp index db0630159..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" @@ -31,9 +32,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 +51,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 +74,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 +153,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 +218,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 +229,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 +243,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 +260,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 +277,62 @@ 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_}; + 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 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 + 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}; } 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); + 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_)}; } const bool should_dribble = extract_robot.breakbeam_ball_detected_filtered || @@ -375,7 +346,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 +366,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 +383,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 +496,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..7188a20a7 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,35 @@ 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..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" @@ -30,9 +31,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 +52,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 +86,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}; + command.motion_intent.motion_options.max_velocity = 1.5; + motion_commands[kicking_robot.id] = command; } else { // Kick ball getPlayInfo()["State"] = "Kicking"; @@ -109,14 +103,25 @@ 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(); + 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/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/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: 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..f0d83fd7b 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,30 @@ 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. + 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/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..37d4f19f1 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,39 @@ 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. + for(auto & maybe_cmd : motion_commands) { + if(!maybe_cmd) {continue;} + maybe_cmd->motion_intent.motion_options.max_velocity = 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 +116,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..f5f232cd0 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,39 @@ 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. + for(auto & maybe_cmd : motion_commands) { + if(!maybe_cmd) {continue;} + maybe_cmd->motion_intent.motion_options.max_velocity = 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 +118,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..afbf82da2 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,13 @@ 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 +272,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 +289,11 @@ 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{ + *opt_escape_vel, + motion::intents::linear::Frame::World + }; 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 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..aea0c6c33 100644 --- a/ateam_kenobi/src/plays/test_plays/controls_test_play.cpp +++ b/ateam_kenobi/src/plays/test_plays/controls_test_play.cpp @@ -38,21 +38,16 @@ 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 = { // {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() @@ -62,10 +57,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 @@ -98,28 +93,38 @@ 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.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; - maybe_motion_commands[robot.id] = 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{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; getOverlays().drawLine("controls_test_path", viz_path, "purple"); @@ -140,8 +145,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"] = 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(); @@ -163,7 +168,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..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,20 +38,20 @@ class ControlsTestPlay : public stp::Play void reset() override; - std::array, + std::array, 16> runFrame(const World & world) override; private: 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/plays/test_plays/spinning_a_play.cpp b/ateam_kenobi/src/plays/test_plays/spinning_a_play.cpp index 1d4d21129..c05b94d5c 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..7a252de45 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,15 +78,20 @@ 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); } 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; } @@ -102,9 +107,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; @@ -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 = ateam_msgs::msg::RobotMotionCommand::FRAME_BODY; + command.motion_intent.linear = motion::intents::linear::VelocityIntent{ + ateam_geometry::Vector{0.0, velocity}, + motion::intents::linear::Frame::Local + }; 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..a04e7c64d 100644 --- a/ateam_kenobi/src/plays/test_plays/test_play.cpp +++ b/ateam_kenobi/src/plays/test_plays/test_play.cpp @@ -30,32 +30,28 @@ 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(); diff --git a/ateam_kenobi/src/plays/their_ball_placement_play.cpp b/ateam_kenobi/src/plays/their_ball_placement_play.cpp index fafa42758..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 @@ -29,9 +31,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 +45,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 +63,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; - } - } + 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)] = "-"; + } - getPlayInfo()["Robots"][std::to_string(robot.id)] = "MOVING"; - } else { - 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); + }); - emt.setTargetPosition(target_position); - emt.face_point(world.ball.pos); - maybe_motion_commands[robot.id] = emt.runFrame(robot, world); - } + 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 +126,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..504e0af03 100644 --- a/ateam_kenobi/src/plays/their_ball_placement_play.hpp +++ b/ateam_kenobi/src/plays/their_ball_placement_play.hpp @@ -22,7 +22,7 @@ #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 { @@ -36,18 +36,23 @@ 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..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 @@ -29,9 +30,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,34 +51,38 @@ 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_.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/plays/their_penalty_play.hpp b/ateam_kenobi/src/plays/their_penalty_play.hpp index ad59a3231..2e0caebf5 100644 --- a/ateam_kenobi/src/plays/their_penalty_play.hpp +++ b/ateam_kenobi/src/plays/their_penalty_play.hpp @@ -23,8 +23,8 @@ #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" namespace ateam_kenobi::plays { @@ -38,14 +38,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/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 diff --git a/ateam_kenobi/src/plays/wall_play.cpp b/ateam_kenobi/src/plays/wall_play.cpp index 1bead908e..40933c239 100644 --- a/ateam_kenobi/src/plays/wall_play.cpp +++ b/ateam_kenobi/src/plays/wall_play.cpp @@ -51,12 +51,11 @@ 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")) + goalie_skill_(createChild("goalie")), + multi_move_to_(createChild("multi_move_to")) { - createIndexedChildren(easy_move_tos_, "EasyMoveTo"); } stp::PlayScore WallPlay::getScore(const World &) @@ -66,18 +65,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); @@ -103,34 +99,11 @@ std::array, 16> WallPlay::run 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; - } - - auto & easy_move_to = easy_move_tos_.at(robot.id); - - const auto & target_position = positions_to_assign.at(ind); - - 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); - } + multi_move_to_.SetTargetPoints(positions_to_assign); + multi_move_to_.SetFacePoint(world.ball.pos); + multi_move_to_.RunFrame(robot_assignments, commands); - 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..84746b1e7 100644 --- a/ateam_kenobi/src/plays/wall_play.hpp +++ b/ateam_kenobi/src/plays/wall_play.hpp @@ -28,8 +28,8 @@ #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" namespace ateam_kenobi::plays { @@ -45,12 +45,11 @@ 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_; + tactics::MultiMoveTo multi_move_to_; }; } // namespace ateam_kenobi::plays diff --git a/ateam_kenobi/src/skills/capture.cpp b/ateam_kenobi/src/skills/capture.cpp index d01394a29..325f184da 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; - 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,12 @@ 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_)); + command.motion_intent.motion_options.max_velocity = std::min(max_decel_vel, max_speed_); - 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 +109,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; - 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; + + command.motion_intent.motion_options.max_velocity = 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/dribble.cpp b/ateam_kenobi/src/skills/dribble.cpp index 38983cd8b..b0d9da8cb 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,74 @@ 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_); - - 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; + + command.motion_intent.motion_options.max_velocity = 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}; + + command.motion_intent.motion_options.max_velocity = 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))); + 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_) { 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 diff --git a/ateam_kenobi/src/skills/extract.cpp b/ateam_kenobi/src/skills/extract.cpp index d54b88f6c..8c6ac13fc 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 + }; + command.motion_intent.motion_options.max_angular_velocity = 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}; + command.motion_intent.motion_options.max_angular_velocity = 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}; + command.motion_intent.motion_options.max_velocity = 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_; }; diff --git a/ateam_kenobi/src/skills/goalie.cpp b/ateam_kenobi/src/skills/goalie.cpp index 7a02662be..a8ab46cf5 100644 --- a/ateam_kenobi/src/skills/goalie.cpp +++ b/ateam_kenobi/src/skills/goalie.cpp @@ -36,11 +36,9 @@ namespace ateam_kenobi::skills Goalie::Goalie(stp::Options stp_options) : stp::Skill(stp_options), - easy_move_to_(createChild("EasyMoveTo")), 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); @@ -48,16 +46,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 +75,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 +150,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 +169,17 @@ 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 +234,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) { @@ -263,7 +263,7 @@ ateam_msgs::msg::RobotMotionCommand 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; @@ -271,17 +271,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) { @@ -318,13 +317,14 @@ ateam_msgs::msg::RobotMotionCommand 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; } -ateam_msgs::msg::RobotMotionCommand Goalie::runSideEjectBall( +RobotCommand Goalie::runSideEjectBall( const World & world, const Robot & goalie) { auto left_count = 0; @@ -344,7 +344,8 @@ ateam_msgs::msg::RobotMotionCommand 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/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/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/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); diff --git a/ateam_kenobi/src/skills/line_kick.cpp b/ateam_kenobi/src/skills/line_kick.cpp index 66a70efaa..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) { } @@ -39,7 +38,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 +58,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 +167,36 @@ 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; - 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 + 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; + 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 +205,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..0b884fa12 100644 --- a/ateam_kenobi/src/skills/line_kick.hpp +++ b/ateam_kenobi/src/skills/line_kick.hpp @@ -22,11 +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/play_helpers/easy_move_to.hpp" +#include "core/types/robot_command.hpp" namespace ateam_kenobi::skills { @@ -59,23 +58,13 @@ 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 { 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 { @@ -176,9 +164,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 diff --git a/ateam_kenobi/src/skills/pass_receiver.cpp b/ateam_kenobi/src/skills/pass_receiver.cpp index ab537556b..3fe62823b 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); } + + command.motion_intent.motion_options.max_velocity = 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}; + 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); if (time_to_ball < 0.5) { @@ -167,37 +167,38 @@ 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}; + command.motion_intent.motion_options.max_velocity = 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 diff --git a/ateam_kenobi/src/skills/pivot_kick.cpp b/ateam_kenobi/src/skills/pivot_kick.cpp index 5be8ac64a..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")) { } @@ -40,13 +39,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) { @@ -57,7 +56,6 @@ ateam_msgs::msg::RobotMotionCommand PivotKick::RunFrame(const World & world, con } if (!robot.breakbeam_ball_detected) { - easy_move_to_.reset(); prev_state_ = State::Capture; getPlayInfo()["State"] = "Capture"; return Capture(world, robot); @@ -67,7 +65,6 @@ ateam_msgs::msg::RobotMotionCommand PivotKick::RunFrame(const World & world, con 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,35 +72,34 @@ ateam_msgs::msg::RobotMotionCommand PivotKick::RunFrame(const World & world, con } if (prev_state_ != State::KickBall) { - easy_move_to_.reset(); prev_state_ = State::KickBall; } 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 +125,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 +136,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..d8c5a4ea9 100644 --- a/ateam_kenobi/src/skills/pivot_kick.hpp +++ b/ateam_kenobi/src/skills/pivot_kick.hpp @@ -22,11 +22,10 @@ #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" namespace ateam_kenobi::skills @@ -60,24 +59,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); @@ -95,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 @@ -109,11 +96,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 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); 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); diff --git a/ateam_kenobi/src/tactics/multi_move_to.cpp b/ateam_kenobi/src/tactics/multi_move_to.cpp index 6a7d1e18d..62194cdf4 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,35 @@ 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); +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; - motion_commands.at(robot.id) = easy_move_to.runFrame(robot, world); + auto viz_circle = ateam_geometry::makeCircle(target_position, kRobotRadius); + getOverlays().drawCircle( + "destination_" + std::to_string( + robot.id), viz_circle, "blue", "transparent"); } } diff --git a/ateam_kenobi/src/tactics/multi_move_to.hpp b/ateam_kenobi/src/tactics/multi_move_to.hpp index 29ab56bca..94ce64ba9 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,15 @@ 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 RunFrame( + const std::vector & robots, + std::array, 16> & motion_commands); void SetTargetPoints(const std::vector & targets) { @@ -50,24 +50,22 @@ class MultiMoveTo : public stp::Tactic void SetFaceNone() { - angle_mode_ = AngleMode::no_face; + angular_intent_ = motion::intents::None{}; } void SetFaceTravel() { - angle_mode_ = AngleMode::face_travel; + angular_intent_ = motion::intents::angular::FaceTravelIntent{}; } void SetFaceAbsolue(double angle) { - angle_mode_ = AngleMode::face_absolute; - absolute_angle_reference_ = angle; + angular_intent_ = motion::intents::angular::HeadingIntent{angle}; } void SetFacePoint(const ateam_geometry::Point & point) { - angle_mode_ = AngleMode::face_point; - point_angle_reference_ = point; + angular_intent_ = motion::intents::angular::FacingIntent{point}; } void SetObstacles(const std::vector & obstacles) @@ -81,15 +79,11 @@ class MultiMoveTo : public stp::Tactic } private: - AngleMode angle_mode_ = 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 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(); 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() { 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_; 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";