double wall spacing

src/rj_radio/CMakeLists.txt

@@ -13,6 +13,7 @@ find_package(ament_cmake REQUIRED)
 # further dependencies manually.
 # find_package(<dependency> REQUIRED)
 find_package(spdlog REQUIRED)
+find_package(std_msgs REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(rclcpp_components REQUIRED)
 find_package(Boost REQUIRED)
@@ -37,6 +38,10 @@ add_executable(sim_radio_node
   src/radio.cpp
 )
 
+add_executable(soccermom
+    src/soccermom.cpp
+)
+
 target_link_libraries(network_radio_node
   PUBLIC
   spdlog
@@ -63,6 +68,12 @@ target_include_directories(sim_radio_node
   $<INSTALL_INTERFACE:include>
 )
 
+target_include_directories(soccermom
+  PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>
+)
+
 ament_target_dependencies(network_radio_node
   PUBLIC
   spdlog
@@ -99,6 +110,13 @@ ament_target_dependencies(sim_radio_node
   rj_constants
 )
 
+ament_target_dependencies(soccermom
+    rclcpp
+    std_msgs
+    rj_constants
+    rj_msgs
+)
+
 install(
   DIRECTORY include/
   DESTINATION include
@@ -109,4 +127,9 @@ install(
   DESTINATION lib/${PROJECT_NAME}
 )
 
-ament_package()
+install(
+  TARGETS soccermom
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+ament_package()

src/rj_radio/include/rj_radio/soccermom.hpp

@@ -0,0 +1,21 @@
+#pragma once
+
+#include <rclcpp/rclcpp.hpp>
+#include <std_msgs/msg/string.hpp>
+#include <rj_msgs/msg/team_color.hpp>
+#include <rj_constants/topic_names.hpp>
+
+namespace rj_radio {
+
+class SoccerMomNode : public rclcpp::Node {
+public:
+    SoccerMomNode();
+
+private:
+    void team_color_callback(const rj_msgs::msg::TeamColor::SharedPtr msg);
+
+    rclcpp::Subscription<rj_msgs::msg::TeamColor>::SharedPtr team_color_sub_;
+    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr fruit_pub_;
+};
+
+}  // namespace rj_radio

src/rj_radio/src/soccermom.cpp

@@ -0,0 +1,42 @@
+#include "rj_radio/soccermom.hpp"
+
+namespace rj_radio {
+
+SoccerMomNode::SoccerMomNode()
+    : Node("soccermom")
+{
+    // Publisher for /team_fruit
+    fruit_pub_ = this->create_publisher<std_msgs::msg::String>("/team_fruit", 10);
+
+    // Subscriber to team color
+    team_color_sub_ = this->create_subscription<rj_msgs::msg::TeamColor>(
+        referee::topics::kTeamColorTopic,
+        10,
+        std::bind(&SoccerMomNode::team_color_callback, this, std::placeholders::_1)
+    );
+
+    RCLCPP_INFO(this->get_logger(), "SoccerMom node started.");
+}
+
+void SoccerMomNode::team_color_callback(const rj_msgs::msg::TeamColor::SharedPtr msg)
+{
+    std_msgs::msg::String fruit;
+    if (msg->is_blue) {
+        fruit.data = "blueberries";
+    } else {
+        fruit.data = "banana";
+    }
+
+    RCLCPP_INFO(this->get_logger(), "Publishing fruit: %s", fruit.data.c_str());
+    fruit_pub_->publish(fruit);
+}
+
+}  // namespace rj_radio
+
+int main(int argc, char **argv)
+{
+    rclcpp::init(argc, argv);
+    rclcpp::spin(std::make_shared<rj_radio::SoccerMomNode>());
+    rclcpp::shutdown();
+    return 0;
+}

src/rj_strategy/include/rj_strategy/agent/position/runner.hpp

@@ -0,0 +1,35 @@
+// ros specific imports
+#include <rclcpp/rclcpp.hpp>
+
+//position class that runner is inheriting
+#include "rj_strategy/agent/position.hpp"
+
+namespace strategy {
+    // class Runner that inherits from Position
+    class Runner : public Position {
+        public:
+            // "standard" constructor we will be using
+            Runner(int runner_id);
+            /* Class destructor*/
+            // things you can do right before constructor is erased from memory
+            ~Runner() = default;
+            Runner(const Position& other);
+
+            // overrides the get_current_state method from position 
+            std::string get_current_state() override;
+
+        private:
+            // updates current state, based on update return state_to_task (kind of like a wrapper for state_to_task)
+            std::optional<RobotIntent> derived_get_task(RobotIntent intent) override;
+
+            // enum of all the different States we can have
+            enum State {TOP_LEFT, TOP_RIGHT, BOTTOM_RIGHT, BOTTOM_LEFT};
+
+            State current_state_ = State::TOP_LEFT;
+
+            State next_state();
+
+            // based the current state, what do we need to do
+            std::optional<RobotIntent> state_to_task(RobotIntent intent);
+    };
+}

src/rj_strategy/src/agent/position/robot_factory_position.cpp

@@ -1,15 +1,15 @@
 #include "rj_strategy/agent/position/robot_factory_position.hpp"
+#include "rj_strategy/agent/position/runner.hpp"
 
 namespace strategy {
 
 RobotFactoryPosition::RobotFactoryPosition(int r_id, rclcpp::Node::SharedPtr node)
     : Position(r_id, "RobotFactoryPosition"), kicker_picker_(std::move(node), r_id) {
-    if (robot_id_ == 0) {
-        current_position_ = std::make_unique<Goalie>(robot_id_);
-    } else if (robot_id_ == 1 || robot_id_ == 2) {
-        current_position_ = std::make_unique<Offense>(robot_id_);
+        // changing this so that I can just watch robot 1
+    if (robot_id_ == 1) {
+        current_position_ = std::make_unique<strategy::Runner>(robot_id_);
     } else {
-        current_position_ = std::make_unique<Defense>(robot_id_);
+        current_position_ = std::make_unique<SmartIdle>(robot_id_);
     }
 }
 
@@ -188,52 +188,12 @@ void RobotFactoryPosition::update_position() {
 }
 
 void RobotFactoryPosition::set_default_position() {
-    // Get sorted positions of all friendly robots
-    using RobotPos = std::pair<int, double>;  // (robotId, yPosition)
-
-    std::vector<RobotPos> robots_copy;
-    for (int i = 0; i < static_cast<int>(kNumShells); i++) {
-        // Ignore goalie
-        if (i == goalie_id_) {
-            continue;
-        }
-        if (alive_robots_[i]) {
-            robots_copy.emplace_back(i, last_world_state_->our_robots[i].pose.position().y());
-        }
-    }
-
-    std::sort(robots_copy.begin(), robots_copy.end(),
-              [](RobotPos const& a, RobotPos const& b) { return a.second < b.second; });
-
-    // Find relative location of current robot
-    int i = 0;
-    for (RobotPos r : robots_copy) {
-        if (r.first == robot_id_) {
-            break;
-        }
-        i++;
+    if (robot_id_ == 1) {
+        set_current_position<strategy::Runner>();
+        return;
     }
 
-    // Assigning new position
-    // Checking whether we have possesion or if the ball is on their half
-    if (our_possession_ || last_world_state_->ball.position.y() >
-                               field_dimensions_.center_field_loc().y() - kBallDiameter) {
-        // Offensive mode
-        // Closest 2 robots on defense, rest on offense
-        if (i <= 1) {
-            set_current_position<Defense>();
-        } else {
-            set_current_position<Offense>();
-        }
-    } 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<SmartIdle>();
 }
 
 std::deque<communication::PosAgentRequestWrapper>

src/rj_strategy/src/agent/position/runner.cpp

@@ -0,0 +1,61 @@
+#include "rj_strategy/agent/position/runner.hpp"
+
+namespace strategy {
+    Runner::Runner(int r_id) : Position{r_id, "Runner"} {}
+    Runner::Runner(const Position& other) : Position{other} {}
+
+    std::string Runner::get_current_state() { 
+        return "Runner"; 
+    }
+
+    Runner::State Runner::next_state() {
+        if (!check_is_done()) {
+            return current_state_;
+        }
+
+        switch(current_state_) {
+            case TOP_LEFT:
+                return TOP_RIGHT;
+            case TOP_RIGHT:
+                return BOTTOM_RIGHT;
+            case BOTTOM_RIGHT:
+                return BOTTOM_LEFT;
+            case BOTTOM_LEFT:
+                return TOP_LEFT;
+            default:
+                return current_state_;
+        }
+    }
+
+    std::optional<RobotIntent> Runner::derived_get_task(RobotIntent intent) {
+        // set current state to next state
+        current_state_ = next_state();
+        return state_to_task(intent);
+    }
+
+    // gets called once per 'tick'
+    std::optional<RobotIntent> Runner::state_to_task(RobotIntent intent) {
+        planning::LinearMotionInstant target;
+        switch (current_state_) {
+            case TOP_LEFT:
+                target = planning::LinearMotionInstant{rj_geometry::Point{2.0, 2.5}};
+                break;
+            case TOP_RIGHT:
+                target = planning::LinearMotionInstant{rj_geometry::Point{-2.0, 2.5}};
+                break;
+            case BOTTOM_RIGHT:
+                target = planning::LinearMotionInstant{rj_geometry::Point{-2.0, 6.5}};
+                break;
+            case BOTTOM_LEFT:
+                target = planning::LinearMotionInstant{rj_geometry::Point{2.0, 6.5}};
+                break;
+        }
+
+        planning::MotionCommand prep_command{"path_target", target, planning::FaceTarget{}};
+        intent.motion_command = prep_command;
+        // intent defines what the robot is doing in that instant
+        // the returned intent is the direction the robot wants to do towards
+        return intent;
+    }
+
+}

src/rj_strategy/src/agent/position/waller.cpp

@@ -35,7 +35,7 @@ std::optional<RobotIntent> Waller::get_task(RobotIntent intent, const WorldState
     // Find target Point
     rj_geometry::Point mid_point{(goal_pos) + (ball_dir_vector * min_wall_rad)};
 
-    auto wall_spacing = kRobotDiameterMultiplier * kRobotDiameter + kBallRadius;
+    auto wall_spacing = (kRobotDiameterMultiplier * kRobotDiameter + kBallRadius) * 2.0;
 
     rj_geometry::Point target_point{};
     auto angle = (mid_point - goal_pos).angle();