Eduardo rodriguez/robocup 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)
@@ -176,6 +177,9 @@ set(COACH_NODE_DEPS_LIBRARIES robocup)
 set(RADIO_NODE_DEPS_SYSTEM_LIBRARIES)
 set(RADIO_NODE_DEPS_LIBRARIES robocup)
 
+set(SOCCER_MOM_NODE_DEPS_SYSTEM_LIBRARIES)
+set(SOCCER_MOM_NODE_DEPS_LIBRARIES robocup)
+
 set(CONTROL_NODE_DEPS_SYSTEM_LIBRARIES)
 set(CONTROL_NODE_DEPS_LIBRARIES robocup)
 
@@ -229,6 +233,10 @@ 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 ${SOCCER_MOM_NODE_DEPS_SYSTEM_LIBRARIES}
+	${SOCCER_MOM_NODE_DEPS_LIBRARIES})
+
 target_link_libraries(coach_node PRIVATE ${COACH_NODE_DEPS_SYSTEM_LIBRARIES}
         ${COACH_NODE_DEPS_LIBRARIES})
 
@@ -296,6 +304,7 @@ install(
         planner_node
         control_node
         sim_radio_node
+        soccer_mom_node
         coach_node
         network_radio_node
         manual_control_node

soccer/src/soccer/CMakeLists.txt

@@ -50,6 +50,7 @@ set(ROBOCUP_LIB_SRC
     radio/network_radio.cpp
     radio/packet_convert.cpp
     radio/sim_radio.cpp
+    soccer_mom/soccer_mom.cpp
     strategy/coach/coach_node.cpp
     radio/radio.cpp
     referee/referee_base.cpp
@@ -79,6 +80,7 @@ set(ROBOCUP_LIB_SRC
     strategy/agent/position/goalie.cpp
     strategy/agent/position/offense.cpp
     strategy/agent/position/defense.cpp
+    strategy/agent/position/runner.cpp
     strategy/agent/position/waller.cpp
     strategy/agent/position/goal_kicker.cpp
     strategy/agent/position/penalty_player.cpp
@@ -120,6 +122,8 @@ set(CONTROL_NODE_SRC control/control_node_main.cpp)
 
 set(SIM_RADIO_NODE_SRC radio/sim_radio_node_main.cpp)
 
+set(SOCCER_MOM_NODE_SRC soccer_mom/soccer_mom_node_main.cpp)
+
 set(COACH_NODE_SRC strategy/coach/coach_node_main.cpp)
 
 set(NETWORK_RADIO_NODE_SRC radio/network_radio_node_main.cpp)
@@ -161,6 +165,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})
+
 # ---- coach_node ----
 target_sources(coach_node PRIVATE ${COACH_NODE_SRC})
 

soccer/src/soccer/soccer_mom/soccer_mom.cpp

@@ -0,0 +1,24 @@
+#include "soccer_mom.hpp"
+
+#include <spdlog/spdlog.h>
+
+namespace soccer_mom {
+
+SoccerMom::SoccerMom()
+    : Node{"soccer_mom", rclcpp::NodeOptions{}
+                             .automatically_declare_parameters_from_overrides(true)
+                             .allow_undeclared_parameters(true)} {
+    team_fruit_pub_ = create_publisher<std_msgs::msg::String>("/team_fruit", 10);
+    team_color_sub_ = create_subscription<rj_msgs::msg::TeamColor>(
+        referee::topics::kTeamColorTopic, rclcpp::QoS(1).transient_local(),
+        [this](rj_msgs::msg::TeamColor::SharedPtr color) {  // NOLINT
+            auto message = std_msgs::msg::String();
+            message.data = "blueberries";
+            if (!color->is_blue) {
+                message.data = "banana";
+            }
+            team_fruit_pub_->publish(message);
+        });
+}
+
+}  // namespace soccer_mom

soccer/src/soccer/soccer_mom/soccer_mom.hpp

@@ -0,0 +1,31 @@
+#pragma once
+
+#include <deque>
+#include <mutex>
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <rj_constants/topic_names.hpp>
+#include <rj_msgs/msg/manipulator_setpoint.hpp>
+#include <rj_msgs/msg/motion_setpoint.hpp>
+#include <rj_msgs/msg/robot_status.hpp>
+#include <rj_msgs/msg/team_color.hpp>
+#include <rj_param_utils/param.hpp>
+#include <rj_param_utils/ros2_local_param_provider.hpp>
+#include <std_msgs/msg/string.hpp>
+
+#include "robot_intent.hpp"
+#include "strategy/coach/coach_node.hpp"
+
+namespace soccer_mom {
+
+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 team_fruit_pub_;
+};
+
+}  // namespace soccer_mom

soccer/src/soccer/soccer_mom/soccer_mom_node_main.cpp

@@ -0,0 +1,14 @@
+#include <rj_common/network.hpp>
+#include <rj_utils/logging.hpp>
+
+#include "global_params.hpp"
+#include "soccer_mom.hpp"
+
+int main(int argc, char** argv) {
+    rclcpp::init(argc, argv);
+    rj_utils::set_spdlog_default_ros2("processor");
+
+    auto radio = std::make_shared<soccer_mom::SoccerMom>();
+    start_global_param_provider(radio.get(), kGlobalParamServerNode);
+    rclcpp::spin(radio);
+}

soccer/src/soccer/strategy/agent/agent_action_client.cpp

@@ -75,8 +75,11 @@
 
     if (r_id == 0) {
         current_position_ = std::make_unique<Goalie>(r_id);
-    } else if (r_id == 1 || r_id == 3 || r_id == 5) {
+
+    } else if (r_id == 3 || r_id == 5) {
         current_position_ = std::make_unique<Defense>(r_id);
+    } else if (r_id == 1) {
+        current_position_ = std::make_unique<Runner>(r_id);
     } else if (r_id == 2 || r_id == 4) {
         current_position_ = std::make_unique<Offense>(r_id);
     }
@@ -144,7 +147,7 @@
         if (robot_id_ == 0) {
             current_position_ = std::make_unique<Goalie>(robot_id_);
         } else if (robot_id_ == 1) {
-            current_position_ = std::make_unique<Defense>(robot_id_);
+            current_position_ = std::make_unique<Runner>(robot_id_);
         } else {
             current_position_ = std::make_unique<Offense>(robot_id_);
         }
@@ -182,6 +185,7 @@
             next_position_ = std::make_unique<GoalKicker>(robot_id_);
             break;
     };
+    if (robot_id_ == 1) next_position_ = std::make_unique<Runner>(robot_id_);
 
     if (current_position_ == nullptr) {
         current_position_ = std::move(next_position_);
@@ -223,7 +227,7 @@
         if (robot_id_ == 0) {
             current_position_ = std::make_unique<Goalie>(robot_id_);
         } else if (robot_id_ == 1) {
-            current_position_ = std::make_unique<Defense>(robot_id_);
+            current_position_ = std::make_unique<Runner>(robot_id_);
         } else {
             current_position_ = std::make_unique<Offense>(robot_id_);
         }
@@ -242,7 +246,7 @@
         if (robot_id_ == 0) {
             current_position_ = std::make_unique<Goalie>(robot_id_);
         } else if (robot_id_ == 1) {
-            current_position_ = std::make_unique<Defense>(robot_id_);
+            current_position_ = std::make_unique<Runner>(robot_id_);
         } else {
             current_position_ = std::make_unique<Offense>(robot_id_);
         }
@@ -258,7 +262,7 @@
         if (robot_id_ == 0) {
             current_position_ = std::make_unique<Goalie>(robot_id_);
         } else if (robot_id_ == 1) {
-            current_position_ = std::make_unique<Defense>(robot_id_);
+            current_position_ = std::make_unique<Runner>(robot_id_);
         } else {
             current_position_ = std::make_unique<Offense>(robot_id_);
         }
@@ -289,7 +293,7 @@
         if (robot_id_ == 0) {
             current_position_ = std::make_unique<Goalie>(robot_id_);
         } else if (robot_id_ == 1) {
-            current_position_ = std::make_unique<Defense>(robot_id_);
+            current_position_ = std::make_unique<Runner>(robot_id_);
         } else {
             current_position_ = std::make_unique<Offense>(robot_id_);
         }
@@ -437,7 +441,7 @@
         if (robot_id_ == 0) {
             current_position_ = std::make_unique<Goalie>(robot_id_);
         } else if (robot_id_ == 1) {
-            current_position_ = std::make_unique<Defense>(robot_id_);
+            current_position_ = std::make_unique<Runner>(robot_id_);
         } else {
             current_position_ = std::make_unique<Offense>(robot_id_);
         }

soccer/src/soccer/strategy/agent/agent_action_client.hpp

@@ -27,6 +27,7 @@
 #include "strategy/agent/position/offense.hpp"
 #include "strategy/agent/position/penalty_player.hpp"
 #include "strategy/agent/position/position.hpp"
+#include "strategy/agent/position/runner.hpp"
 #include "world_state.hpp"
 
 // Communication

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

@@ -0,0 +1,72 @@
+#include "runner.hpp"
+
+namespace strategy {
+
+Runner::Runner(int r_id) : Position(r_id) { position_name_ = "Runner"; }
+
+std::optional<RobotIntent> Runner::derived_get_task(RobotIntent intent) {
+    current_state_ = update_state();
+    return state_to_task(intent);
+}
+
+Runner::State Runner::update_state() {
+    State next_state = current_state_;
+    // handle transitions between states
+    WorldState* world_state = this->world_state();
+    rj_geometry::Point robot_position = world_state->get_robot(true, robot_id_).pose.position();
+
+    switch (current_state_) {
+        case RIGHT:
+            if (robot_position.x() <= field_dimensions_.field_x_left_coord() + 1.1) next_state = UP;
+            break;
+        case UP:
+            if (robot_position.y() <= 1.1) next_state = LEFT;
+            break;
+        case LEFT:
+            if (robot_position.x() >= field_dimensions_.field_x_right_coord() - 1.1)
+                next_state = DOWN;
+            break;
+        case DOWN:
+            if (robot_position.y() >= field_dimensions_.length() - 1.1) next_state = RIGHT;
+            break;
+    }
+
+    return next_state;
+}
+
+std::optional<RobotIntent> Runner::state_to_task(RobotIntent intent) {
+    rj_geometry::Point robot_position = world_state()->get_robot(true, robot_id_).pose.position();
+    rj_geometry::Point destination;
+
+    switch (current_state_) {
+        case RIGHT:
+            destination = rj_geometry::Point{field_dimensions_.field_x_left_coord() + 1,
+                                             field_dimensions_.length() - 1};
+            break;
+        case UP:
+            destination = rj_geometry::Point{field_dimensions_.field_x_left_coord() + 1, 1};
+            break;
+        case LEFT:
+            destination = rj_geometry::Point{field_dimensions_.field_x_right_coord() - 1, 1};
+            break;
+        case DOWN:
+            destination = rj_geometry::Point{field_dimensions_.field_x_right_coord() - 1,
+                                             field_dimensions_.length() - 1};
+            break;
+    }
+
+    auto goal = planning::LinearMotionInstant{destination, {0.0, 0.0}};
+    auto move_direction =
+        planning::MotionCommand{"path_target", goal, planning::FaceTarget{}, true};
+
+    intent.motion_command = move_direction;
+    return intent;
+}
+
+void Runner::derived_acknowledge_pass() {}
+
+void Runner::derived_pass_ball() {}
+
+void Runner::derived_acknowledge_ball_in_transit() {}
+
+}  // namespace strategy

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

@@ -0,0 +1,45 @@
+#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"
+
+namespace strategy {
+
+/*
+ * This robot runs in a square path
+ */
+class Runner : public Position {
+public:
+    Runner(int r_id);
+    ~Runner() override = default;
+
+    void derived_acknowledge_pass() override;
+    void derived_pass_ball() override;
+    void derived_acknowledge_ball_in_transit() override;
+
+private:
+    std::optional<RobotIntent> derived_get_task(RobotIntent intent) override;
+
+    enum State { RIGHT, UP, LEFT, DOWN };
+
+    State update_state();
+
+    std::optional<RobotIntent> state_to_task(RobotIntent intent);
+
+    State current_state_ = LEFT;
+};
+
+}  // namespace strategy

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

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

soccer/src/soccer/strategy/coach/coach_node.cpp

@@ -293,7 +293,7 @@ void CoachNode::assign_positions_normal(std::array<uint32_t, kNumShells>& positi
                     positions[robot_id] = Positions::Goalie;
                     break;
                 case 1:
-                    positions[robot_id] = Positions::Offense;
+                    positions[robot_id] = Positions::Runner;
                     break;
                 case 2:
                     positions[robot_id] = Positions::Defense;