Solo offense

control

@@ -0,0 +1,12 @@
+max_acceleration: 1.0
+max_angular_velocity: 5.0
+max_velocity: 1.0
+rotation_kd: 0.0
+rotation_ki: 0.0
+rotation_kp: 0.0
+rotation_windup: 0
+translation_kd: 0.25
+translation_ki: 0.0005
+translation_kp: 2.0
+translation_windup: 0
+use_sim_time: false

soccer/src/soccer/CMakeLists.txt

@@ -80,6 +80,7 @@ set(ROBOCUP_LIB_SRC
     strategy/agent/position/robot_factory_position.cpp
     strategy/agent/position/goalie.cpp
     strategy/agent/position/offense.cpp
+    strategy/agent/position/line.cpp
     strategy/agent/position/idle.cpp
     strategy/agent/position/defense.cpp
     strategy/agent/position/waller.cpp
@@ -91,6 +92,7 @@ set(ROBOCUP_LIB_SRC
     strategy/agent/position/penalty_non_kicker.cpp
     strategy/agent/position/smartidling.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/radio/network_radio.cpp

@@ -20,7 +20,8 @@ NetworkRadio::NetworkRadio()
     : control_message_socket_(io_service_),
       robot_status_socket_(io_service_),
       alive_robots_socket_(io_service_),
-      send_buffers_(kNumShells) {
+      send_buffers_(kNumShells),
+      Radio() {
     control_message_socket_.open(udp::v4());
     control_message_socket_.bind(udp::endpoint(udp::v4(), kControlMessageSocketPort));
 
@@ -111,6 +112,7 @@ void NetworkRadio::receive_robot_status(const boost::system::error_code& error,
 }
 
 void NetworkRadio::receive_alive_robots(const boost::system::error_code& error, size_t num_bytes) {
+    SPDLOG_INFO("Receiving Alive Robots");
     if (static_cast<bool>(error)) {
         SPDLOG_ERROR("Error Receiving Alive Robots: {}", error.message());
         start_alive_robots_receive();
@@ -123,14 +125,20 @@ void NetworkRadio::receive_alive_robots(const boost::system::error_code& error,
         return;
     }
 
-    uint16_t alive = (alive_robots_buffer_[0] << 8) | (alive_robots_buffer_[0]);
+    uint16_t alive = (((uint16_t) alive_robots_buffer_[1]) << 8) | (alive_robots_buffer_[0]);
+    SPDLOG_INFO("Alive: {}", alive);
+
+    // uint16_t alive = (alive_robots_buffer_[0] << 8) | (alive_robots_buffer_[0]);
     for (uint8_t robot_id = 0; robot_id < kNumShells; robot_id++) {
         if ((alive & (1 << robot_id)) != 0) {
             alive_robots_[robot_id] = true;
         } else {
             alive_robots_[robot_id] = false;
         }
     }
+    for (int robot_id = 0; robot_id < 16; robot_id++) {
+        SPDLOG_INFO("Robot {} is Alive {}", robot_id, alive_robots_[robot_id]);
+    }
 
     rj_msgs::msg::AliveRobots alive_message{};
     alive_message.alive_robots = alive_robots_;

soccer/src/soccer/radio/radio.cpp

@@ -49,6 +49,8 @@ void Radio::publish_robot_status(int robot_id, const rj_msgs::msg::RobotStatus&
 }
 
 void Radio::publish_alive_robots(const rj_msgs::msg::AliveRobots& alive_robots) {
+    // SPDLOG_INFO("publishing alive robots {}", alive_robots);
+    SPDLOG_INFO("publishing alive robots");
     alive_robots_pub_->publish(alive_robots);
 }
 

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

@@ -0,0 +1,35 @@
+#include "line.hpp"
+
+namespace strategy {
+std::optional<RobotIntent> Line::derived_get_task(RobotIntent intent) {
+    if (check_is_done()) {
+        forward_ = !forward_;
+    }
+
+    if (forward_) {
+        auto motion_command = planning::MotionCommand{
+            "path_target",
+            planning::LinearMotionInstant{
+                rj_geometry::Point{
+                    field_dimensions_.center_field_loc().x() - 1.0,
+                    field_dimensions_.center_field_loc().y() - 1.0 + robot_id_ * 0.75},
+                rj_geometry::Point{0.0, 0.0}},
+            planning::FacePoint{rj_geometry::Point{0.0,0.0}}, true};
+
+        intent.motion_command = motion_command;
+    } else {
+        auto motion_command = planning::MotionCommand{
+            "path_target",
+            planning::LinearMotionInstant{
+                rj_geometry::Point{
+                    field_dimensions_.center_field_loc().x() + 1.0,
+                    field_dimensions_.center_field_loc().y() - 1.0 + robot_id_ * 0.75},
+                rj_geometry::Point{0.0, 0.0}},
+            planning::FacePoint{rj_geometry::Point{0.0,0.0}}, true};
+
+        intent.motion_command = motion_command;
+    }
+
+    return intent;
+}
+}  // namespace strategy

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

@@ -0,0 +1,34 @@
+#pragma once
+
+#include "position.hpp"
+#include <spdlog/spdlog.h>
+#include "rclcpp/rclcpp.hpp"
+
+
+namespace strategy {
+    class Line : public Position {
+
+        public: 
+
+        Line(int r_id) : Position{r_id, "line"} {}
+        Line(const Position& other) : Position{other} {
+            position_name_ = "Line";
+        }
+        ~Line() = default;
+        Line(const Line&) = default;
+        Line(Line&&) = default;
+        Line& operator=(const Line&) = default;
+        Line& operator=(Line&&) = default;
+
+
+        std::string get_current_state() override {
+            return "Line";
+        }
+
+        private:
+
+        std::optional<RobotIntent> derived_get_task(RobotIntent intent) override;
+
+        bool forward_ = true;
+    };
+}

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

@@ -15,7 +15,7 @@ std::optional<RobotIntent> Offense::derived_get_task(RobotIntent intent) {
     if (current_state_ != new_state) {
         reset_timeout();
 
-        SPDLOG_INFO("Robot {}: now {}", robot_id_, state_to_name(current_state_));
+        // SPDLOG_INFO("Robot {}: now {}", robot_id_, state_to_name(current_state_));
         if (current_state_ == SEEKING) {
             broadcast_seeker_request(rj_geometry::Point{}, false);
         }

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

@@ -3,6 +3,7 @@
 #include <algorithm>
 
 #include "idle.hpp"
+#include "line.hpp"
 #include "penalty_non_kicker.hpp"
 
 namespace strategy {
@@ -17,10 +18,10 @@ RobotFactoryPosition::RobotFactoryPosition(int r_id) : Position(r_id, "RobotFact
     }
 }
 
-std::optional<RobotIntent> RobotFactoryPosition::derived_get_task([
-    [maybe_unused]] RobotIntent intent) {
+std::optional<RobotIntent> RobotFactoryPosition::derived_get_task(
+    [[maybe_unused]] RobotIntent intent) {
     if (robot_id_ == goalie_id_) {
-        set_current_position<Goalie>();
+        set_current_position<Line>();
         return current_position_->get_task(*last_world_state_, field_dimensions_,
                                            current_play_state_);
     }
@@ -250,18 +251,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"
 
 namespace strategy {
 

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

@@ -0,0 +1,151 @@
+#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)) {
+        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