complete waller planner

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

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

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<planning::MotionCommand, rj_msgs::msg::MotionCommand> {
         // 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<planning::MotionCommand, rj_msgs::msg::MotionCommand> {
             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;
     }
 };

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<CollectPathPlanner>();
     path_planners_[LineKickPathPlanner().name()] = std::make_unique<LineKickPathPlanner>();
     path_planners_[PivotPathPlanner().name()] = std::make_unique<PivotPathPlanner>();
-    path_planners_[WallerPathPlanner().name()] = std::make_unique<WallerPathPlanner>();
     path_planners_[LinePivotPathPlanner().name()] = std::make_unique<LinePivotPathPlanner>();
     path_planners_[EscapeObstaclesPathPlanner().name()] =
         std::make_unique<EscapeObstaclesPathPlanner>();

soccer/src/soccer/strategy/agent/position/waller.cpp

@@ -10,7 +10,6 @@ Waller::Waller(int waller_num, std::vector<u_int8_t> walling_robots) {
 
 std::optional<RobotIntent> 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<RobotIntent> Waller::get_task(RobotIntent intent, const WorldState
     double min_wall_rad{(kRobotRadius * 4.0f) + line_w +
                         hypot(static_cast<double>(box_w) / 2, static_cast<double>((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;
 }

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<u_int8_t> walling_robots_;
 };