diff --git a/rj_msgs/msg/MotionCommand.msg b/rj_msgs/msg/MotionCommand.msg index d06b4ec2d5c..f942d0b3207 100644 --- a/rj_msgs/msg/MotionCommand.msg +++ b/rj_msgs/msg/MotionCommand.msg @@ -6,7 +6,3 @@ bool[<=1] face_ball bool[<=1] ignore_ball rj_geometry_msgs/Point[<=1] pivot_point float64[<=1] pivot_radius -float64[<=1] waller_radius -uint8[<=1] waller_parent -uint8[<=1] waller_id -uint8[<=1] num_wallers diff --git a/soccer/src/soccer/CMakeLists.txt b/soccer/src/soccer/CMakeLists.txt index 9867a20f8bd..452c350b9e2 100644 --- a/soccer/src/soccer/CMakeLists.txt +++ b/soccer/src/soccer/CMakeLists.txt @@ -38,7 +38,6 @@ set(ROBOCUP_LIB_SRC planning/planner/intercept_path_planner.cpp planning/planner/line_kick_path_planner.cpp planning/planner/path_target_path_planner.cpp - planning/planner/waller_path_planner.cpp planning/planner/pivot_path_planner.cpp planning/planner/line_pivot_path_planner.cpp planning/planner/plan_request.cpp diff --git a/soccer/src/soccer/planning/planner/motion_command.hpp b/soccer/src/soccer/planning/planner/motion_command.hpp index e6d5f42017f..28148cc922d 100644 --- a/soccer/src/soccer/planning/planner/motion_command.hpp +++ b/soccer/src/soccer/planning/planner/motion_command.hpp @@ -58,10 +58,6 @@ struct MotionCommand { bool ignore_ball{false}; rj_geometry::Point pivot_point{}; double pivot_radius{kRobotRadius}; - double waller_radius{0}; - uint8_t waller_id{}; - uint8_t num_wallers{}; - uint8_t waller_parent{}; }; bool operator==(const MotionCommand& a, const MotionCommand& b); @@ -117,16 +113,6 @@ struct RosConverter { // convert the pivot radius result.pivot_radius.push_back(from.pivot_radius); - // convert pivot point - result.waller_parent.push_back(from.waller_parent); - - // convert the pivot radius - result.waller_radius.push_back(from.waller_radius); - - result.waller_id.push_back(from.waller_id); - - result.num_wallers.push_back(from.num_wallers); - return result; } @@ -168,24 +154,6 @@ struct RosConverter { result.pivot_radius = from.pivot_radius[0]; } - // convert waller - if (!from.waller_parent.empty()) { - result.waller_parent = from.waller_parent[0]; - } - - if (!from.waller_radius.empty()) { - result.waller_radius = from.waller_radius[0]; - } - - if (!from.waller_id.empty()) { - result.waller_id = from.waller_id[0]; - } - - - if (!from.num_wallers.empty()) { - result.num_wallers = from.num_wallers[0]; - } - return result; } }; diff --git a/soccer/src/soccer/planning/planner/waller_path_planner.cpp b/soccer/src/soccer/planning/planner/waller_path_planner.cpp deleted file mode 100644 index 67fb52059c1..00000000000 --- a/soccer/src/soccer/planning/planner/waller_path_planner.cpp +++ /dev/null @@ -1,82 +0,0 @@ -#include "waller_path_planner.hpp" - -namespace planning { -using namespace rj_geometry; - -Trajectory WallerPathPlanner::plan(const PlanRequest& request) { - auto command = request.motion_command; - - auto robot_id = request.shell_id; - auto robot_pos = request.world_state->get_robot(true, robot_id).pose.position(); - auto ball_pos = request.world_state->ball.position; - auto waller_radius = command.waller_radius; - auto goal_pos = Point{0, 0}; - auto num_wallers = command.num_wallers; - auto parent_point = request.world_state->get_robot(true, command.waller_parent).pose.position(); - - auto waller_id = command.waller_id; - - // Find ball_direction unit vector - rj_geometry::Point ball_dir_vector{(ball_pos - goal_pos)}; - - ball_dir_vector = ball_dir_vector.normalized(); - - // Find target Point - rj_geometry::Point mid_point{(goal_pos) + (ball_dir_vector * waller_radius)}; - - auto wall_spacing = kRobotDiameterMultiplier * kRobotDiameter + kBallRadius; - - rj_geometry::Point target_point{}; - SPDLOG_INFO("Radius: {}", waller_radius); - SPDLOG_INFO("Midpoint: ({}, {})", mid_point.x(), mid_point.y()); - SPDLOG_INFO("Waller ID: {}", waller_id); - - - if (abs(robot_pos.dist_to(goal_pos) - waller_radius) > kRobotRadius) { - SPDLOG_INFO("CASE ONE"); - auto angle = (mid_point - goal_pos).angle(); - auto delta_angle = (wall_spacing * abs(waller_id - num_wallers / 2. + 0.5)) / waller_radius; - auto target_angle = angle - delta_angle * (signbit(waller_id - num_wallers / 2. + 0.5) ? -1 : 1); - - target_point = (goal_pos + Point{1, 0}).normalized(waller_radius).rotated(target_angle); - - } else { - auto angle = (mid_point - goal_pos).angle(); - auto delta_angle = (wall_spacing * abs(waller_id - num_wallers / 2. + 0.5)) / waller_radius; - auto target_angle = angle - delta_angle * (signbit(waller_id - num_wallers / 2. + 0.5) ? -1 : 1); - - target_point = (goal_pos + Point{1, 0}).normalized(waller_radius).rotated(target_angle); - - if (target_point.x() < robot_pos.x()) { - SPDLOG_INFO("CASE TWO"); - - } else { - SPDLOG_INFO("CASE THREE"); - } - - if ((target_point.x() < robot_pos.x() && waller_id != 1) || (target_point.x() > robot_pos.x() && waller_id != num_wallers)) { - SPDLOG_INFO("FOLLOWING PARENT AT ({}, {})", parent_point.x(), parent_point.y()); - angle = (parent_point - goal_pos).angle(); - delta_angle = wall_spacing / waller_radius; - target_angle = angle - delta_angle; - - target_point = (goal_pos + Point{1, 0}).normalized(waller_radius).rotated(target_angle); - } - } - - SPDLOG_INFO("TARGET POS: ({}, {})", target_point.x(), target_point.y()); - - PlanRequest modified_request = request; - LinearMotionInstant target{target_point}; - - MotionCommand modified_command{"path_target", target}; - modified_request.motion_command = modified_command; - - return path_target_.plan(modified_request); -} - -bool WallerPathPlanner::is_done() const { - return false; -} - -} \ No newline at end of file diff --git a/soccer/src/soccer/planning/planner/waller_path_planner.hpp b/soccer/src/soccer/planning/planner/waller_path_planner.hpp deleted file mode 100644 index dc01acd8bc2..00000000000 --- a/soccer/src/soccer/planning/planner/waller_path_planner.hpp +++ /dev/null @@ -1,40 +0,0 @@ -#pragma once - -#include "path_planner.hpp" -#include "path_target_path_planner.hpp" - -namespace planning { - -/** - * PathPlanner which governs the movement of the wallers - * - * Each waller will follow the waller in front of it, - * ensuring that they move in a smooth path - * - * Params taken from MotionCommand: - * command.waller_radius - radius of the waller arc - * command.walling_robots - vector of current wallers - */ - -class WallerPathPlanner : public PathPlanner { -public: - WallerPathPlanner() : PathPlanner("waller") {} - ~WallerPathPlanner() override = default; - - WallerPathPlanner(WallerPathPlanner&&) noexcept = default; - WallerPathPlanner& operator=(WallerPathPlanner&&) noexcept = default; - WallerPathPlanner(const WallerPathPlanner&) = default; - WallerPathPlanner& operator=(const WallerPathPlanner&) = default; - - Trajectory plan(const PlanRequest& request) override; - - void reset() override {} - - [[nodiscard]] bool is_done() const override; - -private: - static constexpr double kRobotDiameterMultiplier = 1.5; - - PathTargetPathPlanner path_target_{}; -}; -} // namespace planning \ No newline at end of file diff --git a/soccer/src/soccer/planning/planner_for_robot.cpp b/soccer/src/soccer/planning/planner_for_robot.cpp index be124872ec9..6a6cfc22042 100644 --- a/soccer/src/soccer/planning/planner_for_robot.cpp +++ b/soccer/src/soccer/planning/planner_for_robot.cpp @@ -8,7 +8,6 @@ #include "planning/planner/line_kick_path_planner.hpp" #include "planning/planner/line_pivot_path_planner.hpp" #include "planning/planner/path_target_path_planner.hpp" -#include "planning/planner/waller_path_planner.hpp" #include "planning/planner/pivot_path_planner.hpp" #include "planning/planner/settle_path_planner.hpp" @@ -32,7 +31,6 @@ PlannerForRobot::PlannerForRobot(int robot_id, rclcpp::Node* node, path_planners_[CollectPathPlanner().name()] = std::make_unique(); path_planners_[LineKickPathPlanner().name()] = std::make_unique(); path_planners_[PivotPathPlanner().name()] = std::make_unique(); - path_planners_[WallerPathPlanner().name()] = std::make_unique(); path_planners_[LinePivotPathPlanner().name()] = std::make_unique(); path_planners_[EscapeObstaclesPathPlanner().name()] = std::make_unique(); diff --git a/soccer/src/soccer/strategy/agent/position/waller.cpp b/soccer/src/soccer/strategy/agent/position/waller.cpp index 9067e6f5e4a..c841f7dc4d1 100644 --- a/soccer/src/soccer/strategy/agent/position/waller.cpp +++ b/soccer/src/soccer/strategy/agent/position/waller.cpp @@ -10,7 +10,6 @@ Waller::Waller(int waller_num, std::vector walling_robots) { std::optional Waller::get_task(RobotIntent intent, const WorldState* world_state, FieldDimensions field_dimensions) { - // Creates Minimum wall radius is slightly greater than box bounds // Dimension accessors should be edited when we figure out how we are doing dimensions realtime // from vision @@ -20,27 +19,61 @@ std::optional Waller::get_task(RobotIntent intent, const WorldState double min_wall_rad{(kRobotRadius * 4.0f) + line_w + hypot(static_cast(box_w) / 2, static_cast((box_h)))}; - auto motion_command = planning::MotionCommand{"waller"}; - motion_command.waller_radius = min_wall_rad; - motion_command.waller_id = waller_pos_; - - SPDLOG_INFO("IN: {}", waller_pos_); - auto ball_pos = world_state->ball.position; - uint8_t parent_id; - if (waller_pos_ > 1 && ball_pos.x() < world_state->get_robot(true, intent.robot_id).pose.position().x()) { - parent_id = walling_robots_[waller_pos_ - 2]; - } else if (waller_pos_ < walling_robots_.size()) { - parent_id = walling_robots_[waller_pos_]; - } + auto robot_id = intent.robot_id; + + auto robot_pos = world_state->get_robot(true, robot_id).pose.position(); + auto goal_pos = rj_geometry::Point{0, 0}; + auto num_wallers = walling_robots_.size(); + + // Find ball_direction unit vector + rj_geometry::Point ball_dir_vector{(ball_pos - goal_pos)}; + + ball_dir_vector = ball_dir_vector.normalized(); + + // Find target Point + rj_geometry::Point mid_point{(goal_pos) + (ball_dir_vector * min_wall_rad)}; - motion_command.waller_parent = parent_id; + auto wall_spacing = kRobotDiameterMultiplier * kRobotDiameter + kBallRadius; - motion_command.num_wallers = walling_robots_.size(); + rj_geometry::Point target_point{}; + auto angle = (mid_point - goal_pos).angle(); + auto delta_angle = (wall_spacing * (waller_pos_ - num_wallers / 2. - 0.5)) / min_wall_rad; + auto target_angle = angle - delta_angle; + + target_point = + (goal_pos + rj_geometry::Point{1, 0}).normalized(min_wall_rad).rotated(target_angle); + + if (abs(robot_pos.dist_to(goal_pos) - min_wall_rad) < kRobotRadius && + robot_pos.dist_to(target_point) > kRobotRadius) { + uint8_t parent_id; + + if (target_point.x() < robot_pos.x()) { + parent_id = walling_robots_[waller_pos_ - 2]; + } else { + parent_id = walling_robots_[waller_pos_]; + } + + if ((target_point.x() < robot_pos.x() && waller_pos_ != 1) || + (target_point.x() > robot_pos.x() && waller_pos_ != num_wallers)) { + auto parent_point = world_state->get_robot(true, parent_id).pose.position(); + angle = (parent_point - goal_pos).angle(); + delta_angle = wall_spacing / min_wall_rad; + target_angle = + angle + delta_angle * (signbit(target_point.x() - robot_pos.x()) ? -1 : 1); + + target_point = (goal_pos + rj_geometry::Point{1, 0}) + .normalized(min_wall_rad) + .rotated(target_angle); + } + } + auto location_instant = planning::LinearMotionInstant{target_point, rj_geometry::Point{}}; - intent.motion_command = motion_command; + auto target_cmd = + planning::MotionCommand{"path_target", location_instant, planning::FaceBall{}}; + intent.motion_command = target_cmd; return intent; } diff --git a/soccer/src/soccer/strategy/agent/position/waller.hpp b/soccer/src/soccer/strategy/agent/position/waller.hpp index 80d235cdbfd..41a3bf3a6bd 100644 --- a/soccer/src/soccer/strategy/agent/position/waller.hpp +++ b/soccer/src/soccer/strategy/agent/position/waller.hpp @@ -42,6 +42,7 @@ class Waller : public RoleInterface { private: std::string defense_type_; int waller_pos_; + static constexpr double kRobotDiameterMultiplier = 1.5; std::vector walling_robots_; };