Doubling Wall Space for SW Tutorial

launch/soccer.launch.py

@@ -147,6 +147,14 @@ def generate_launch_description():
                 parameters=[param_config_filepath],
                 on_exit=Shutdown(),
             ),
+            Node(
+                condition=IfCondition(PythonExpression([run_sim])),
+                package="rj_robocup",
+                executable="soccer_mom_node",
+                output="screen",
+                parameters=[param_config_filepath],
+                on_exit=Shutdown(),
+            ),
             Node(
                 condition=IfCondition(PythonExpression(["not ", run_sim])),
                 package="rj_robocup",

soccer/CMakeLists.txt

@@ -82,6 +82,7 @@ add_executable(external_referee_node)
 add_executable(planner_node)
 add_executable(control_node)
 add_executable(sim_radio_node)
+add_executable(soccer_mom_node)
 add_executable(network_radio_node)
 add_executable(manual_control_node)
 add_executable(global_param_server_node)
@@ -225,6 +226,9 @@ target_link_libraries(internal_referee_node PRIVATE ${REFEREE_NODE_DEPS_SYSTEM_L
 target_link_libraries(sim_radio_node PRIVATE ${RADIO_NODE_DEPS_SYSTEM_LIBRARIES}
         ${RADIO_NODE_DEPS_LIBRARIES})
 
+# -- soccer_mom_node --
+target_link_libraries(soccer_mom_node PRIVATE ${RADIO_NODE_DEPS_SYSTEM_LIBRARIES}
+        ${RADIO_NODE_DEPS_LIBRARIES})
 # -- network_radio_node --
 target_link_libraries(network_radio_node PRIVATE ${RADIO_NODE_DEPS_SYSTEM_LIBRARIES}
         ${RADIO_NODE_DEPS_LIBRARIES})
@@ -290,6 +294,7 @@ install(
         control_node
         sim_radio_node
         network_radio_node
+        soccer_mom_node
         manual_control_node
         global_param_server_node
         agent_action_client_node

soccer/src/soccer/CMakeLists.txt

@@ -53,6 +53,7 @@ set(ROBOCUP_LIB_SRC
     radio/network_radio.cpp
     radio/packet_convert.cpp
     radio/sim_radio.cpp
+    soccer_mom.cpp
     radio/radio.cpp
     referee/referee_base.cpp
     referee/external_referee.cpp
@@ -92,8 +93,11 @@ set(ROBOCUP_LIB_SRC
     strategy/agent/position/penalty_non_kicker.cpp
     strategy/agent/position/smartidling.cpp
     strategy/agent/position/zoner.cpp
+    strategy/agent/position/runner.cpp
     strategy/agent/position/pivot_test.cpp
     strategy/agent/position/solo_offense.cpp
+    strategy/agent/position/runner.cpp
+
 
         )
 
@@ -131,6 +135,7 @@ set(PLANNER_NODE_SRC planning/planner_node_main.cpp)
 
 set(CONTROL_NODE_SRC control/control_node_main.cpp)
 
+set(SOCCER_MOM_NODE_SRC soccer_mom_node_main.cpp)
 set(SIM_RADIO_NODE_SRC radio/sim_radio_node_main.cpp)
 
 set(NETWORK_RADIO_NODE_SRC radio/network_radio_node_main.cpp)
@@ -172,6 +177,9 @@ target_sources(control_node PRIVATE ${CONTROL_NODE_SRC})
 # ---- sim_radio_node ----
 target_sources(sim_radio_node PRIVATE ${SIM_RADIO_NODE_SRC})
 
+# ---- soccer_mom_node ----
+target_sources(soccer_mom_node PRIVATE ${SOCCER_MOM_NODE_SRC})
+
 # ---- sim_radio_node ----
 target_sources(network_radio_node PRIVATE ${NETWORK_RADIO_NODE_SRC})
 

soccer/src/soccer/soccer_mom.cpp

@@ -0,0 +1,18 @@
+#include "soccer_mom.hpp"
+
+SoccerMom::SoccerMom() : rclcpp::Node{"SoccerMom"} {
+    soccer_mom_pub_ = create_publisher<std_msgs::msg::String>("team_fruit", rclcpp::QoS(1));
+
+    team_color_sub_ = create_subscription<rj_msgs::msg::TeamColor>(
+        referee::topics::kTeamColorTopic, rclcpp::QoS(1),
+        [this](rj_msgs::msg::TeamColor::SharedPtr color) {
+            auto message = std_msgs::msg::String();
+            if (color->is_blue) {
+                message.data = "blueberries";
+            } else {
+                message.data = "bananas";
+            }
+            soccer_mom_pub_->publish(message);
+        }
+    );
+}

soccer/src/soccer/soccer_mom.hpp

@@ -0,0 +1,17 @@
+#pragma once
+
+#include <rclcpp/rclcpp.hpp>
+#include <rj_constants/topic_names.hpp>
+
+#include "std_msgs/msg/string.hpp"
+#include "rj_msgs/msg/team_color.hpp"
+
+class SoccerMom : public rclcpp::Node {
+    public:
+        SoccerMom();
+
+    private:
+        rclcpp::Subscription<rj_msgs::msg::TeamColor>::SharedPtr team_color_sub_;
+        rclcpp::Publisher<std_msgs::msg::String>::SharedPtr soccer_mom_pub_;
+
+};

soccer/src/soccer/soccer_mom_node_main.cpp

@@ -0,0 +1,7 @@
+#include "soccer_mom.hpp"
+
+int main(int argc, char** argv) {
+	rclcpp::init(argc, argv);
+	auto soccer_mom = std::make_shared<SoccerMom>(); 
+	rclcpp::spin(soccer_mom); 
+}

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

@@ -216,6 +216,14 @@ bool RobotFactoryPosition::am_closest_kicker() {
 }
 
 void RobotFactoryPosition::set_default_position() {
+
+	if (robot_id_ == 1) {
+		set_current_position<Runner>();
+
+	} else {
+		set_current_position<SmartIdle>();
+	}
+	return; 
     // zoner defense testing
     // if (robot_id_ == goalie_id_) {
     //     return;

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

@@ -22,6 +22,7 @@
 #include "strategy/agent/position/goalie.hpp"
 #include "strategy/agent/position/line.hpp"
 #include "strategy/agent/position/offense.hpp"
+#include "strategy/agent/position/runner.hpp"
 #include "strategy/agent/position/penalty_non_kicker.hpp"
 #include "strategy/agent/position/penalty_player.hpp"
 #include "strategy/agent/position/pivot_test.hpp"

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

@@ -0,0 +1,77 @@
+#include "runner.hpp"
+
+#include <rclcpp/rclcpp.hpp>
+
+#include "position.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"; }
+
+/**
+ * @brief Does nothing; this position is a special case
+ */
+void Runner::derived_acknowledge_pass() {}
+/**
+ * @brief Does nothing; this position is a special case
+ */
+void Runner::derived_pass_ball(){
+
+};
+/**
+ * @brief Does nothing; this position is a special case
+ */
+void Runner::derived_acknowledge_ball_in_transit() {}
+
+std::optional<RobotIntent> Runner::derived_get_task(RobotIntent intent) {
+    // update current state 
+    current_state_ = next_state(); 
+    return state_to_task(intent); 
+};
+
+Runner::State Runner::next_state() {
+	if(!check_is_done()) {
+		return current_state_; // if ur not done yet, stay in same state 
+	}
+
+	// if check is done: 
+	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::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 command{"path_target", target, planning::FaceTarget{}};
+	intent.motion_command = command; 
+	return intent; 
+}
+
+}  // namespace strategy

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

@@ -0,0 +1,43 @@
+#include <rclcpp/rclcpp.hpp>
+
+#include "position.hpp"
+
+namespace strategy {
+class Runner : public Position {
+public:
+    Runner(int r_id);
+    ~Runner() = default;
+    Runner(const Position& other);
+
+    /**
+     * @brief Does nothing; this position is a special case
+     */
+    void derived_acknowledge_pass() override;
+    /**
+     * @brief Does nothing; this position is a special case
+     */
+    void derived_pass_ball() override;
+    /**
+     * @brief Does nothing; this position is a special case
+     */
+    void derived_acknowledge_ball_in_transit() override;
+
+    std::string get_current_state() override;
+
+private:
+    std::optional<RobotIntent> derived_get_task(RobotIntent intent) override;
+
+    enum State {
+    	TOP_LEFT,
+    	TOP_RIGHT,
+    	BOTTOM_LEFT,
+    	BOTTOM_RIGHT
+    };
+    State current_state_ = State::TOP_LEFT; 
+    State next_state(); 
+    std::optional<RobotIntent> state_to_task(RobotIntent intent);
+
+
+
+};
+}  // namespace strategy

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

@@ -33,7 +33,8 @@ std::optional<RobotIntent> Waller::get_task(RobotIntent intent, const WorldState
     rj_geometry::Point mid_point{(goal_center_point) + (ball_dir_vector * min_wall_rad)};
 
     // Calculate the wall spacing
-    auto wall_spacing = robot_diameter_multiplier_ * kRobotDiameter + kBallRadius;
+    auto wall_spacing = 2 * (robot_diameter_multiplier_ * kRobotDiameter + kBallRadius);
+
 
     // Calculate the target point
     rj_geometry::Point target_point{};