Zoner defense 2 (#2256)

---------

Co-authored-by: Shourik <[email protected]>
Co-authored-by: Sid Parikh <[email protected]>
Co-authored-by: oddlyspookycherry <[email protected]>
Co-authored-by: sanatd33 <[email protected]>
Co-authored-by: github-actions[bot] <41898282+github-actions[bot]@users.noreply.github.com>
Co-authored-by: sanatd33 <[email protected]>
Co-authored-by: petergarud <[email protected]>
Co-authored-by: jacksherling <[email protected]>
Co-authored-by: sid-parikh <[email protected]>

soccer/src/soccer/CMakeLists.txt

@@ -90,6 +90,7 @@ set(ROBOCUP_LIB_SRC
  strategy/agent/position/penalty_player.cpp
  strategy/agent/position/penalty_non_kicker.cpp
  strategy/agent/position/smartidling.cpp
+ strategy/agent/position/zoner.cpp
  strategy/agent/position/pivot_test.cpp
 
  )

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

@@ -216,6 +216,18 @@ bool RobotFactoryPosition::am_closest_kicker() {
 }
 
 void RobotFactoryPosition::set_default_position() {
+ // zoner defense testing
+ // if (robot_id_ == goalie_id_) {
+ // return;
+ // }
+ // if (robot_id_ == 1) {
+ // set_current_position<Zoner>();
+ // } else {
+ // set_current_position<Defense>();
+ // }
+ // return;
+ // end zoner defense testing
+
  // TODO (Rishi and Jack): Make this synchronized across all robots to avoid race conditions
  // Get sorted positions of all friendly robots
  using RobotPos = std::pair<int, double>; // (robotId, yPosition)

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

@@ -26,6 +26,7 @@
 #include "strategy/agent/position/pivot_test.hpp"
 #include "strategy/agent/position/position.hpp"
 #include "strategy/agent/position/smartidling.hpp"
+#include "strategy/agent/position/zoner.hpp"
 
 namespace strategy {
 

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

@@ -0,0 +1,133 @@
+#include "zoner.hpp"
+
+namespace strategy {
+
+Zoner::Zoner(int r_id) : Position(r_id, "Zoner") {}
+
+Zoner::Zoner(const Position& other) : Position{other} { position_name_ = "Zoner"; }
+
+std::optional<RobotIntent> Zoner::derived_get_task(RobotIntent intent) {
+ current_state_ = next_state();
+ // waller_id_ = get_waller_id();
+ return state_to_task(intent);
+}
+
+Zoner::State Zoner::next_state() {
+ rj_geometry::Point ball_pos = last_world_state_->ball.position;
+ rj_geometry::Segment block_path{ball_pos, field_dimensions_.our_goal_loc()};
+
+ double min_dist = std::numeric_limits<double>::infinity();
+ for (int i = 0; i < static_cast<int>(kNumShells); i++) {
+ // TODO: Add solo offense here as well
+ if (i == robot_id_ || i == goalie_id_) {
+ continue;
+ }
+ if (alive_robots_[i]) {
+ RobotState robot = last_world_state_->get_robot(true, i);
+ double dist = block_path.dist_to(robot.pose.position());
+ min_dist = std::min(min_dist, dist);
+ }
+ }
+
+ if (min_dist < (2 * kRobotRadius)) {
+ return ZONE;
+ }
+ return WALL;
+}
+
+std::optional<RobotIntent> Zoner::state_to_task(RobotIntent intent) {
+ switch (current_state_) {
+ case WALL: {
+ float box_w{field_dimensions_.penalty_long_dist()};
+ float box_h{field_dimensions_.penalty_short_dist()};
+ float line_w{field_dimensions_.line_width()};
+ double min_wall_rad{
+ (kRobotRadius * 4.0f) + line_w +
+ hypot(static_cast<double>(box_w) / 2, static_cast<double>((box_h)))};
+ min_wall_rad *= 1.5;
+
+ rj_geometry::Point goal_pos = field_dimensions_.our_goal_loc();
+ rj_geometry::Point ball_pos = last_world_state_->ball.position;
+
+ rj_geometry::Point target_point =
+ ((ball_pos - goal_pos).normalized(min_wall_rad)) + goal_pos;
+
+ rj_geometry::Point target_vel{0.0, 0.0};
+
+ // Face ball
+ planning::PathTargetFaceOption face_option{planning::FaceBall{}};
+
+ // 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};
+ return intent;
+ }
+
+ case ZONE: {
+ // can change this point depending on zoner behavior
+ rj_geometry::Point center{1.35, 1.5};
+ rj_geometry::Point ball_pos = last_world_state_->ball.position;
+
+ if (field_dimensions_.our_goal_loc().y() > 4.5) {
+ center.y() = -1 * center.y();
+ }
+ if (ball_pos.x() > 0) {
+ center.x() = -1 * center.x();
+ }
+
+ std::vector<rj_geometry::Point> opp_poses{};
+
+ for (RobotState opp : last_world_state_->their_robots) {
+ double dist = center.dist_to(opp.pose.position());
+ if (dist < kZonerRadius) {
+ opp_poses.emplace_back(opp.pose.position());
+ }
+ }
+ // SPDLOG_INFO("opp pose size: {}", opp_poses.size());
+
+ rj_geometry::Point target_point{};
+ if (opp_poses.size() >= 2) {
+ // finding point that minimizes distance from opponent robots
+ target_point = find_centroid(opp_poses);
+ } else if (opp_poses.size() == 1) {
+ // mark if just one robot
+ double mark_rad = kRobotRadius * 4;
+ target_point = (ball_pos - opp_poses[0]).normalized(mark_rad) + opp_poses[0];
+ } else {
+ target_point = center;
+ }
+
+ rj_geometry::Point target_vel{0.0, 0.0};
+
+ // Face ball
+ planning::PathTargetFaceOption face_option{planning::FaceBall{}};
+
+ // 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};
+ return intent;
+ }
+ }
+}
+
+rj_geometry::Point Zoner::find_centroid(const std::vector<rj_geometry::Point> opp_poses) {
+ rj_geometry::Point p{};
+
+ for (rj_geometry::Point pos : opp_poses) {
+ p += pos;
+ }
+
+ return p / static_cast<double>(opp_poses.size());
+}
+
+std::string Zoner::get_current_state() { return "Zoner"; }
+
+} // namespace strategy

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

@@ -0,0 +1,61 @@
+#pragma once
+
+#include <cmath>
+
+#include <rclcpp/rclcpp.hpp>
+#include <rclcpp_action/rclcpp_action.hpp>
+#include <spdlog/spdlog.h>
+
+#include <rj_msgs/action/robot_move.hpp>
+
+#include "planning/instant.hpp"
+#include "position.hpp"
+#include "rj_common/field_dimensions.hpp"
+#include "rj_common/time.hpp"
+#include "rj_constants/constants.hpp"
+#include "rj_geometry/geometry_conversions.hpp"
+#include "rj_geometry/point.hpp"
+#include "role_interface.hpp"
+
+namespace strategy {
+
+/*
+ * The Waller role provides the implementation for a defensive robot that implements
+ * this class to have a waller-like behavior where it aims to serve as a wall between
+ * the ball and the goal.
+ */
+class Zoner : public Position {
+public:
+ Zoner(int r_id);
+ ~Zoner() override = default;
+ Zoner(const Position& other);
+ Zoner(Zoner&& other) = default;
+ Zoner& operator=(const Zoner& other) = default;
+ Zoner& operator=(Zoner&& other) = default;
+
+private:
+ std::optional<RobotIntent> derived_get_task(RobotIntent intent) override;
+
+ // static constexpr double robot_diameter_multiplier_ = 1.5;
+ enum State { ZONE, WALL };
+
+ State current_state_ = ZONE;
+
+ /**
+ * @return what the state should be right now. called on each get_task tick
+ */
+ State next_state();
+
+ /**
+ * @return the task to execute. called on each get_task tick AFTER next_state()
+ */
+ std::optional<RobotIntent> state_to_task(RobotIntent intent);
+
+ static rj_geometry::Point find_centroid(const std::vector<rj_geometry::Point> opp_poses);
+
+ std::string get_current_state() override;
+
+ static constexpr double kZonerRadius = 1.5;
+};
+
+} // namespace strategy