Position Monitoring Framework

rj_msgs/msg/AgentState.msg

@@ -0,0 +1 @@
+string state

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

@@ -9,6 +9,10 @@ std::optional<RobotIntent> Defense::derived_get_task(RobotIntent intent) {
     return state_to_task(intent);
 }
 
+std::string Defense::get_current_state() {
+    return std::string{"Defense"} + std::to_string(static_cast<int>(current_state_));
+}
+
 Defense::State Defense::update_state() {
     State next_state = current_state_;
     // handle transitions between states

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

@@ -1,6 +1,7 @@
 #pragma once
 
 #include <cmath>
+#include <string>
 
 #include <rclcpp/rclcpp.hpp>
 #include <rclcpp_action/rclcpp_action.hpp>
@@ -35,6 +36,7 @@ class Defense : public Position {
     void derived_acknowledge_pass() override;
     void derived_pass_ball() override;
     void derived_acknowledge_ball_in_transit() override;
+    std::string get_current_state() override;
 
     void die() override;
     void revive() override;

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

@@ -28,6 +28,8 @@ std::optional<RobotIntent> GoalKicker::derived_get_task(RobotIntent intent) {
     return intent;
 }
 
+std::string GoalKicker::get_current_state() { return "GoalKicker"; }
+
 void GoalKicker::derived_acknowledge_pass() {}
 
 void GoalKicker::derived_pass_ball() {}

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

@@ -1,5 +1,7 @@
 #pragma once
 
+#include <string>
+
 #include <rclcpp/rclcpp.hpp>
 
 #include "planning/instant.hpp"
@@ -32,6 +34,8 @@ class GoalKicker : public Position {
      */
     void derived_acknowledge_ball_in_transit() override;
 
+    std::string get_current_state() override;
+
 private:
     std::optional<RobotIntent> derived_get_task(RobotIntent intent) override;
 };

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

@@ -11,6 +11,8 @@ std::optional<RobotIntent> Goalie::derived_get_task(RobotIntent intent) {
     return state_to_task(intent);
 }
 
+std::string Goalie::get_current_state() { return "Goalie"; }
+
 Goalie::State Goalie::update_state() {
     // if a shot is coming, override all and go block it
     WorldState* world_state = last_world_state_;

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

@@ -1,6 +1,7 @@
 #pragma once
 
 #include <cmath>
+#include <string>
 
 #include <spdlog/spdlog.h>
 
@@ -28,6 +29,8 @@ class Goalie : public Position {
     void derived_pass_ball() override;
     void derived_acknowledge_ball_in_transit() override;
 
+    std::string get_current_state() override;
+
 private:
     // point goalie will aim for when clearing balls
     const rj_geometry::Point clear_point_{0.0, 4.5};

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

@@ -14,6 +14,10 @@ std::optional<RobotIntent> Offense::derived_get_task(RobotIntent intent) {
     return state_to_task(intent);
 }
 
+std::string Offense::get_current_state() {
+    return std::string{"Offense"} + std::to_string(static_cast<int>(current_state_));
+}
+
 Offense::State Offense::update_state() {
     State next_state = current_state_;
     // handle transitions between current state

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

@@ -2,6 +2,7 @@
 
 #include <chrono>
 #include <cmath>
+#include <string>
 
 #include <rclcpp/rclcpp.hpp>
 #include <rclcpp_action/rclcpp_action.hpp>
@@ -36,6 +37,8 @@ class Offense : public Position {
     void derived_pass_ball() override;
     void derived_acknowledge_ball_in_transit() override;
 
+    std::string get_current_state() override;
+
 private:
     bool kicking_{true};
 

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

@@ -31,6 +31,8 @@ std::optional<RobotIntent> PenaltyPlayer::derived_get_task(RobotIntent intent) {
     return intent;
 }
 
+std::string PenaltyPlayer::get_current_state() { return "PenaltyPlayer"; }
+
 void PenaltyPlayer::derived_acknowledge_pass() {}
 
 void PenaltyPlayer::derived_pass_ball() {}

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

@@ -1,5 +1,7 @@
 #pragma once
 
+#include <string>
+
 #include <rclcpp/rclcpp.hpp>
 
 #include "planning/instant.hpp"
@@ -32,6 +34,8 @@ class PenaltyPlayer : public Position {
      */
     void derived_acknowledge_ball_in_transit() override;
 
+    std::string get_current_state() override;
+
 private:
     std::optional<RobotIntent> derived_get_task(RobotIntent intent) override;
 };

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

@@ -1,6 +1,7 @@
 #pragma once
 
 #include <cstdlib>
+#include <string>
 #include <vector>
 
 #include <spdlog/spdlog.h>
@@ -75,6 +76,9 @@ class Position {
     void update_alive_robots(std::vector<u_int8_t> alive_robots);
     const std::string get_name();
 
+    // returns the current state of the robot
+    virtual std::string get_current_state() = 0;
+
     /**
      * @brief setter for time_left_
      */

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

@@ -120,4 +120,8 @@ void RobotFactoryPosition::die() { current_position_->die(); }
 
 void RobotFactoryPosition::revive() { current_position_->revive(); }
 
+std::string RobotFactoryPosition::get_current_state() {
+    return current_position_->get_current_state();
+}
+
 }  // namespace strategy

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

@@ -1,6 +1,7 @@
 #pragma once
 
 #include <cmath>
+#include <string>
 
 #include <rclcpp/rclcpp.hpp>
 #include <rclcpp_action/rclcpp_action.hpp>
@@ -54,6 +55,8 @@ class RobotFactoryPosition : public Position {
     void derived_pass_ball() override;
     void derived_acknowledge_ball_in_transit() override;
 
+    std::string get_current_state() override;
+
     void set_is_done() override;
     void die() override;
     void revive() override;