Waller Planner

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

@@ -142,8 +142,8 @@ std::optional<RobotIntent> Defense::state_to_task(RobotIntent intent) {
         intent.is_active = true;
         return intent;
     } else if (current_state_ == WALLING) {
-        if (!walling_robots_.empty()) {
-            Waller waller{waller_id_, (int)walling_robots_.size()};
+        if (!walling_robots_.empty() && waller_id_ != -1) {
+            Waller waller{waller_id_, walling_robots_};
             return waller.get_task(intent, last_world_state_, this->field_dimensions_);
         }
     } else if (current_state_ == FACING) {

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

@@ -2,19 +2,14 @@
 
 namespace strategy {
 
-Waller::Waller(int waller_num, int total_wallers) {
+Waller::Waller(int waller_num, std::vector<u_int8_t> walling_robots) {
     defense_type_ = "Waller";
     waller_pos_ = waller_num;
-    total_wallers_ = total_wallers;
+    walling_robots_ = walling_robots;
 }
 
 std::optional<RobotIntent> Waller::get_task(RobotIntent intent, const WorldState* world_state,
                                             FieldDimensions field_dimensions) {
-    rj_geometry::Point ball_location{world_state->ball.position};
-
-    // Get Goal Location (Always (0,0)) as of creation
-    rj_geometry::Point goal_center_point{0, 0};
-
     // 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
@@ -24,39 +19,62 @@
     double min_wall_rad{(kRobotRadius * 4.0f) + line_w +
                         hypot(static_cast<double>(box_w) / 2, static_cast<double>((box_h)))};
 
-    // Find ball_direction unit vector
-    rj_geometry::Point ball_dir_vector{(ball_location - goal_center_point)};
-    // (avoid div by 0)
-    ball_dir_vector /= (ball_dir_vector.mag() + 0.000001);
+    auto ball_pos = world_state->ball.position;
 
-    // Find target Point
-    rj_geometry::Point mid_point{(goal_center_point) + (ball_dir_vector * min_wall_rad)};
+    auto robot_id = intent.robot_id;
 
-    // Calculate the wall spacing
-    auto wall_spacing = robot_diameter_multiplier_ * kRobotDiameter + kBallRadius;
+    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();
 
-    // Calculate the target point
-    rj_geometry::Point target_point{};
+    // Find ball_direction unit vector
+    rj_geometry::Point ball_dir_vector{(ball_pos - goal_pos)};
 
-    target_point =
-        mid_point +
-        ball_dir_vector * ((total_wallers_ - waller_pos_ - (total_wallers_ / 2)) * wall_spacing);
+    ball_dir_vector = ball_dir_vector.normalized();
 
-    target_point = target_point.rotate(mid_point, M_PI / 2);
+    // Find target Point
+    rj_geometry::Point mid_point{(goal_pos) + (ball_dir_vector * min_wall_rad)};
 
-    // Stop at end of path
-    rj_geometry::Point target_vel{0.0, 0.0};
+    auto wall_spacing = kRobotDiameterMultiplier * kRobotDiameter + kBallRadius;
 
-    // Face ball
-    planning::PathTargetFaceOption face_option{planning::FaceBall{}};
+    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;
 
-    // Avoid ball
-    bool ignore_ball{true};
+    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{}};
+
+    auto target_cmd =
+        planning::MotionCommand{"path_target", location_instant, planning::FaceBall{}};
+    intent.motion_command = target_cmd;
 
-    // Create Motion Command
-    planning::LinearMotionInstant target{target_point, target_vel};
-    intent.motion_command =
-        planning::MotionCommand{"path_target", target, face_option, ignore_ball};
     return intent;
 }
 

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

@@ -26,7 +26,7 @@ namespace strategy {
  */
 class Waller : public RoleInterface {
 public:
-    Waller(int waller_num, int total_wallers);
+    Waller(int waller_num, std::vector<u_int8_t> walling_robots);
     ~Waller() = default;
 
     /**
@@ -42,9 +42,8 @@ class Waller : public RoleInterface {
 private:
     std::string defense_type_;
     int waller_pos_;
-    int total_wallers_;
-
-    static constexpr double robot_diameter_multiplier_ = 1.5;
+    static constexpr double kRobotDiameterMultiplier = 1.5;
+    std::vector<u_int8_t> walling_robots_;
 };
 
 }  // namespace strategy