add soccermom node

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="soccermom_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(soccermom_node)
 add_executable(network_radio_node)
 add_executable(manual_control_node)
 add_executable(global_param_server_node)
@@ -225,6 +226,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})
 
+# -- soccermom_node --
+target_link_libraries(soccermom_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})
@@ -289,6 +294,7 @@ install(
         planner_node
         control_node
         sim_radio_node
+        soccermom_node
         network_radio_node
         manual_control_node
         global_param_server_node

soccer/src/soccer/CMakeLists.txt

@@ -54,6 +54,7 @@ set(ROBOCUP_LIB_SRC
     radio/packet_convert.cpp
     radio/sim_radio.cpp
     radio/radio.cpp
+    soccermom/soccermom.cpp
     referee/referee_base.cpp
     referee/external_referee.cpp
     referee/internal_referee.cpp
@@ -135,6 +136,8 @@ set(CONTROL_NODE_SRC control/control_node_main.cpp)
 
 set(SIM_RADIO_NODE_SRC radio/sim_radio_node_main.cpp)
 
+set(SOCCERMOM_NODE_SRC soccermom/soccermom_node_main.cpp)
+
 set(NETWORK_RADIO_NODE_SRC radio/network_radio_node_main.cpp)
 
 set(MANUAL_CONTROL_NODE_SRC joystick/manual_control_node_main.cpp)
@@ -177,6 +180,9 @@ target_sources(sim_radio_node PRIVATE ${SIM_RADIO_NODE_SRC})
 # ---- sim_radio_node ----
 target_sources(network_radio_node PRIVATE ${NETWORK_RADIO_NODE_SRC})
 
+# ---- soccermom_node ----
+target_sources(soccermom_node PRIVATE ${SOCCERMOM_NODE_SRC})
+
 # ---- manual_control_node ----
 target_sources(manual_control_node PRIVATE ${MANUAL_CONTROL_NODE_SRC})
 

soccer/src/soccer/soccermom/soccermom.cpp

@@ -0,0 +1,44 @@
+#include "soccermom.hpp"
+
+#include <spdlog/spdlog.h>
+
+Soccermom::Soccermom(bool blue_team)
+    : Node{"soccermom", rclcpp::NodeOptions{}
+                            .automatically_declare_parameters_from_overrides(true)
+                            .allow_undeclared_parameters(true)}
+
+{
+    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
+            team_fruit_pub_ =
+                create_publisher<std_msgs::msg::String>("/team_fruit", rclcpp::QoS(10));
+
+            if (color->is_blue) {
+                // publish "blueberries"
+                auto message = std_msgs::msg::String();
+                message.data = "blueberries";
+                team_fruit_pub_->publish(message);
+            } else {
+                // publish "bananas"
+                auto message = std_msgs::msg::String();
+                message.data = "bananas";
+                team_fruit_pub_->publish(message);
+            }
+
+        });
+
+    team_fruit_pub_ = create_publisher<std_msgs::msg::String>("/team_fruit", rclcpp::QoS(10));
+
+    if (blue_team) {
+        // publish "blueberries"
+        auto message = std_msgs::msg::String();
+        message.data = "blueberries";
+        team_fruit_pub_->publish(message);
+    } else {
+        // publish "bananas"
+        auto message = std_msgs::msg::String();
+        message.data = "bananas";
+        team_fruit_pub_->publish(message);
+    }
+}

soccer/src/soccer/soccermom/soccermom.hpp

@@ -0,0 +1,76 @@
+#pragma once
+
+#include <deque>
+#include <mutex>
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <rj_constants/topic_names.hpp>
+#include <rj_msgs/msg/alive_robots.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 "robot_intent.hpp"
+#include "std_msgs/msg/string.hpp"
+#include "strategy/agent/position/positions.hpp"
+
+/**
+ * @brief Sends and receives information to/from our robots.
+ *
+ * @details This is the abstract superclass for NetworkRadio and SimRadio, which do
+ * the actual work - this just declares the interface and handles sending stop commands when no new
+ * commands come in for a while.
+ *
+ * The radio should handle:
+ *  1. Sending Control Messages to the Robots
+ *      * Alive Robots should be receiving real commands
+ *      * Non-responsive (dead) robots should receive stop commands
+ * 2. Receiving Robot Status Messages and Publish to the robot status topic
+ * 3. Calculate Alive Robots and Publish to the alive robots topic
+ */
+class Soccermom : public rclcpp::Node {
+public:
+    Soccermom(bool blue_team = false);
+
+protected:
+    // Time between consecutive calls to tick().
+    std::chrono::milliseconds tick_period_ = std::chrono::milliseconds(100);
+    // Ros timer to trigger tick every tick_period
+    rclcpp::TimerBase::SharedPtr tick_timer_;
+
+    // The position of each robot
+    std::array<strategy::Positions, kNumShells> positions_;
+
+    // Ros publishers to send robot statuses
+    std::array<rclcpp::Publisher<rj_msgs::msg::RobotStatus>::SharedPtr, kNumShells>
+        robot_status_pubs_;
+
+    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr team_fruit_pub_;
+
+    // Ros publisher to update alive robots
+    rclcpp::Publisher<rj_msgs::msg::AliveRobots>::SharedPtr alive_robots_pub_;
+
+    // Ros subscribers to receive velocity commands, which are sent to the robot
+    std::array<rclcpp::Subscription<rj_msgs::msg::MotionSetpoint>::SharedPtr, kNumShells>
+        motion_subs_;
+    // Last Update Timestamps (per robot)
+    std::array<RJ::Time, kNumShells> last_updates_ = {};
+    // Cached last velocity command
+    std::array<rj_msgs::msg::MotionSetpoint::SharedPtr, kNumShells> motions_;
+
+    // Ros subscribers to receive auxillary control (i.e. shoot_mode, trigger_mode, kick_speed, and
+    // dribbler_speed) which are stored and sent to the robot
+    std::array<rclcpp::Subscription<rj_msgs::msg::ManipulatorSetpoint>::SharedPtr, kNumShells>
+        manipulator_subs_;
+    // Cached auxillary control information
+    std::array<rj_msgs::msg::ManipulatorSetpoint, kNumShells> manipulators_cached_;
+
+    // Ros subscriber for the team's color.
+    rclcpp::Subscription<rj_msgs::msg::TeamColor>::SharedPtr team_color_sub_;
+    // Whether or not the current team color is blue
+    bool blue_team_;
+};

soccer/src/soccer/soccermom/soccermom_node_main.cpp

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