Waller steal

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

@@ -106,7 +106,7 @@ std::optional<RobotIntent> Defense::state_to_task(RobotIntent intent) {
         return intent;
     } else if (current_state_ == WALLING) {
         if (!walling_robots_.empty()) {
-            Waller waller{waller_id_, (int)walling_robots_.size()};
+            Waller waller{waller_id_, robot_id_, (int)walling_robots_.size(), walling_robots_};
             return waller.get_task(intent, last_world_state_, this->field_dimensions_);
         }
     } else if (current_state_ = FACING) {
@@ -200,6 +200,7 @@ communication::JoinWallResponse Defense::handle_join_wall_request(
             break;
         } else if (i == walling_robots_.size() - 1) {
             walling_robots_.push_back(join_request.robot_id);
+
             waller_id_ = get_waller_id();
         }
     }

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

@@ -1,6 +1,7 @@
 #pragma once
 
 #include <cmath>
+#include <vector>
 
 #include <rclcpp/rclcpp.hpp>
 #include <rclcpp_action/rclcpp_action.hpp>

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

@@ -2,10 +2,12 @@
 
 namespace strategy {
 
-Waller::Waller(int waller_num, int total_wallers) {
+Waller::Waller(int waller_num, int robot_id, int total_wallers, std::vector<u_int8_t> waller_ids) {
     defense_type_ = "Waller";
     waller_pos_ = waller_num;
+    robot_id_ = robot_id;
     total_wallers_ = total_wallers;
+    waller_ids_ = waller_ids;
 }
 
 std::optional<RobotIntent> Waller::get_task(RobotIntent intent, const WorldState* world_state,
@@ -53,10 +55,41 @@
     // Avoid ball
     bool ignore_ball{true};
 
-    // Create Motion Command
-    planning::LinearMotionInstant target{target_point, target_vel};
-    intent.motion_command =
-        planning::MotionCommand{"path_target", target, face_option, ignore_ball};
+    // find distance to ball
+    rj_geometry::Point ball_pt = world_state->ball.position;
+    rj_geometry::Point robot_position = world_state->get_robot(true, robot_id_).pose.position();
+    double distance_to_ball = robot_position.dist_to(ball_pt);
+
+    // 0.75 or less is the starting value for distance
+    bool dont_kick = true;
+
+    // checks to see if ball is within the kicking distance
+    if (distance_to_ball < CLEAR_DIST) {
+        std::sort(waller_ids_.begin(), waller_ids_.end());
+        int median_id = waller_ids_[waller_ids_.size() / 2];
+
+        // if the robot is the median robot id, kick
+        // this is just an arbitrary decision that works good enough
+        if (robot_id_ == median_id) {
+            planning::LinearMotionInstant target{field_dimensions.their_goal_loc()};
+            intent.motion_command = planning::MotionCommand{"line_kick", target};
+
+            intent.shoot_mode = RobotIntent::ShootMode::CHIP;
+            intent.trigger_mode = RobotIntent::TriggerMode::ON_BREAK_BEAM;
+            intent.kick_speed = 4.0;
+            intent.dribbler_speed = 255.0;
+            intent.is_active = true;
+            dont_kick = false;
+        }
+    }
+
+    // standard waller behavior
+    if (dont_kick) {
+        // 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

@@ -1,15 +1,18 @@
 #pragma once
 
+#include <algorithm>
 #include <cmath>
 
 #include <rclcpp/rclcpp.hpp>
 #include <rclcpp_action/rclcpp_action.hpp>
 #include <spdlog/spdlog.h>
 
 #include <rj_msgs/action/robot_move.hpp>
+#include <rj_utils/logging.hpp>
 
 #include "planning/instant.hpp"
 #include "position.hpp"
+#include "rclcpp/utilities.hpp"
 #include "rj_common/field_dimensions.hpp"
 #include "rj_common/time.hpp"
 #include "rj_constants/constants.hpp"
@@ -26,7 +29,7 @@
  */
 class Waller : public RoleInterface {
 public:
-    Waller(int waller_num, int total_wallers);
+    Waller(int waller_num, int robot_id, int total_wallers, std::vector<u_int8_t> waller_ids);
     ~Waller() = default;
 
     /**
@@ -42,7 +45,12 @@
 private:
     std::string defense_type_;
     int waller_pos_;
+    int robot_id_;
     int total_wallers_;
+    std::vector<u_int8_t> waller_ids_;  // list of wallers
+
+    // max distance away the ball can be from a waller for it to jump out and kick it
+    const float CLEAR_DIST = 0.75;
 
     static constexpr double robot_diameter_multiplier_ = 1.5;
 };