New solo offense (old branch died)

soccer/src/soccer/CMakeLists.txt

@@ -92,6 +92,7 @@ set(ROBOCUP_LIB_SRC
     strategy/agent/position/smartidling.cpp
     strategy/agent/position/zoner.cpp
     strategy/agent/position/pivot_test.cpp
+    strategy/agent/position/solo_offense.cpp
 
         )
 

soccer/src/soccer/planning/planner/line_pivot_path_planner.cpp

@@ -51,7 +51,8 @@ LinePivotPathPlanner::State LinePivotPathPlanner::next_state(const PlanRequest&
     double vel = plan_request.world_state->get_robot(true, static_cast<int>(plan_request.shell_id))
                      .velocity.linear()
                      .mag();
-    if (current_state_ == LINE && (target_point.dist_to(current_point) < 0.3) && (vel < 0.3)) {
+    if (current_state_ == LINE && (target_point.dist_to(current_point) < 0.3) && (vel < 0.3) &&
+        (!plan_request.play_state.is_stop())) {
         return PIVOT;
     }
     if (current_state_ == PIVOT && (pivot_point.dist_to(current_point) > (dist_from_point * 5))) {

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

@@ -262,18 +262,18 @@ void RobotFactoryPosition::set_default_position() {
                                field_dimensions_.center_field_loc().y() - kBallDiameter) {
         // Offensive mode
         // Closest 2 robots on defense, rest on offense
-        if (i <= 1) {
+        if (i <= 3) {
             set_current_position<Defense>();
         } else {
-            set_current_position<Offense>();
+            set_current_position<SoloOffense>();
         }
     } else {
         // Defensive mode
         // Closest 4 robots on defense, rest on offense
         if (i <= 3) {
             set_current_position<Defense>();
         } else {
-            set_current_position<Offense>();
+            set_current_position<SoloOffense>();
         }
     }
 }

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/solo_offense.hpp"
 #include "strategy/agent/position/zoner.hpp"
 
 namespace strategy {

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

@@ -0,0 +1,153 @@
+#include "solo_offense.hpp"
+
+namespace strategy {
+
+SoloOffense::SoloOffense(const Position& other) : Position{other} {
+    position_name_ = "SoloOffense";
+}
+
+SoloOffense::SoloOffense(int r_id) : Position{r_id, "SoloOffense"} {}
+
+std::optional<RobotIntent> SoloOffense::derived_get_task(RobotIntent intent) {
+    // Get next state, and if different, reset clock
+    State new_state = next_state();
+    if (new_state != current_state_) {
+        // SPDLOG_INFO("New State: {}", std::to_string(static_cast<int>(new_state)));
+    }
+    current_state_ = new_state;
+
+    // Calculate task based on state
+    return state_to_task(intent);
+}
+
+std::string SoloOffense::get_current_state() {
+    return std::string{"Solo Offense"} + std::to_string(static_cast<int>(current_state_));
+}
+
+SoloOffense::State SoloOffense::next_state() {
+    // handle transitions between current state
+    double closest_dist = std::numeric_limits<double>::infinity();
+    auto current_point = last_world_state_->ball.position;
+
+    for (int i = 0; i < 6; i++) {
+        RobotState robot = last_world_state_->get_robot(false, i);
+        rj_geometry::Point opp_pos = robot.pose.position();
+        auto robot_dist = opp_pos.dist_to(current_point);
+        if (robot_dist < closest_dist) {
+            marking_id_ = i;
+            closest_dist = robot_dist;
+        }
+    }
+
+    // SPDLOG_INFO("Closest dist: {}, i-{}", closest_dist,  marking_id_);
+
+    if (closest_dist < (0.5) || field_dimensions_.their_goal_area().contains_point(current_point) ||
+        field_dimensions_.their_defense_area().contains_point(current_point) ||
+        !field_dimensions_.field_coordinates().contains_point(current_point)) {
+        return MARKER;
+    }
+    switch (current_state_) {
+        case MARKER: {
+            return TO_BALL;
+        }
+        case TO_BALL: {
+            if (check_is_done()) {
+                target_ = calculate_best_shot();
+                return KICK;
+            }
+        }
+        case KICK: {
+            if (check_is_done()) {
+                return TO_BALL;
+            }
+        }
+    }
+    return current_state_;
+}
+
+std::optional<RobotIntent> SoloOffense::state_to_task(RobotIntent intent) {
+    switch (current_state_) {
+        case MARKER: {
+            auto marker_target_pos =
+                last_world_state_->get_robot(false, marking_id_).pose.position();
+            auto target =
+                marker_target_pos +
+                (field_dimensions_.our_goal_loc() - marker_target_pos).normalized(kRobotRadius * 5);
+            auto mark_cmd = planning::MotionCommand{
+                "path_target", planning::LinearMotionInstant{target}, planning::FaceBall{}, true};
+            intent.motion_command = mark_cmd;
+
+            return intent;
+        }
+        case TO_BALL: {
+            planning::LinearMotionInstant target{field_dimensions_.their_goal_loc()};
+            auto pivot_cmd = planning::MotionCommand{"line_pivot", target, planning::FaceTarget{},
+                                                     false, last_world_state_->ball.position};
+            pivot_cmd.pivot_radius = kRobotRadius * 2.5;
+            intent.motion_command = pivot_cmd;
+            return intent;
+        }
+        case KICK: {
+            auto line_kick_cmd =
+                planning::MotionCommand{"line_kick", planning::LinearMotionInstant{target_}};
+
+            intent.motion_command = line_kick_cmd;
+            intent.shoot_mode = RobotIntent::ShootMode::KICK;
+            intent.trigger_mode = RobotIntent::TriggerMode::ON_BREAK_BEAM;
+            intent.kick_speed = 4.0;
+
+            return intent;
+        }
+    }
+    return intent;
+}
+
+rj_geometry::Point SoloOffense::calculate_best_shot() const {
+    // Goal location
+    rj_geometry::Point their_goal_pos = field_dimensions_.their_goal_loc();
+    double goal_width = field_dimensions_.goal_width();  // 1.0 meters
+
+    // Ball location
+    rj_geometry::Point ball_position = this->last_world_state_->ball.position;
+
+    rj_geometry::Point best_shot = their_goal_pos;
+    double best_distance = -1.0;
+    rj_geometry::Point increment(0.05, 0);
+    rj_geometry::Point curr_point =
+        their_goal_pos - rj_geometry::Point(goal_width / 2.0, 0) + increment;
+    for (int i = 0; i < 19; i++) {
+        double distance = distance_from_their_robots(ball_position, curr_point);
+        if (distance > best_distance) {
+            best_distance = distance;
+            best_shot = curr_point;
+        }
+        curr_point = curr_point + increment;
+    }
+    return best_shot;
+}
+
+double SoloOffense::distance_from_their_robots(rj_geometry::Point tail,
+                                               rj_geometry::Point head) const {
+    rj_geometry::Point vec = head - tail;
+    auto& their_robots = this->last_world_state_->their_robots;
+
+    double min_angle = -0.5;
+    for (auto enemy : their_robots) {
+        rj_geometry::Point enemy_vec = enemy.pose.position() - tail;
+        if (enemy_vec.dot(vec) < 0) {
+            continue;
+        }
+        auto projection = (enemy_vec.dot(vec) / vec.dot(vec));
+        enemy_vec = enemy_vec - (projection)*vec;
+        double distance = enemy_vec.mag();
+        if (distance < (kRobotRadius + kBallRadius)) {
+            return -1.0;
+        }
+        double angle = distance / projection;
+        if ((min_angle < 0) || (angle < min_angle)) {
+            min_angle = angle;
+        }
+    }
+    return min_angle;
+}
+}  // namespace strategy

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

@@ -0,0 +1,61 @@
+#pragma once
+
+#include <chrono>
+#include <cmath>
+#include <string>
+#include <unordered_map>
+
+#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/time.hpp"
+#include "rj_constants/constants.hpp"
+#include "rj_geometry/geometry_conversions.hpp"
+#include "rj_geometry/point.hpp"
+
+namespace strategy {
+
+class SoloOffense : public Position {
+public:
+    SoloOffense(const Position& other);
+    SoloOffense(int r_id);
+    ~SoloOffense() override = default;
+    SoloOffense(const SoloOffense& other) = default;
+    SoloOffense(SoloOffense&& other) = default;
+
+    std::string get_current_state() override;
+
+private:
+    /**
+     * @brief Overriden from Position. Calls next_state and then state_to_task on each tick.
+     */
+    std::optional<RobotIntent> derived_get_task(RobotIntent intent) override;
+
+    enum State { TO_BALL, KICK, MARKER };
+
+    State current_state_ = TO_BALL;
+
+    rj_geometry::Point target_;
+
+    int marking_id_;
+
+    /**
+     * @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);
+
+    rj_geometry::Point calculate_best_shot() const;
+    double distance_from_their_robots(rj_geometry::Point tail, rj_geometry::Point head) const;
+};
+
+}  // namespace strategy