root_robot -> robot_factory change and planning works

launch/soccer.launch.py

@@ -182,6 +182,14 @@ def generate_launch_description():
             ),
             # spawn internal_ref/external_ref based on internal_ref
             # LaunchArgument
+            Node(
+                condition=IfCondition(PythonExpression(["not ", use_manual_control])),
+                package="rj_robocup",
+                executable="coach_node",
+                output="screen",
+                parameters=[param_config_filepath],
+                on_exit=Shutdown(),
+            ),
             Node(
                 condition=IfCondition(PythonExpression(["not ", use_manual_control])),
                 package="rj_robocup",

soccer/src/soccer/CMakeLists.txt

@@ -76,7 +76,7 @@ set(ROBOCUP_LIB_SRC
     strategy/agent/agent_action_client.cpp
     strategy/agent/communication/communication.cpp
     strategy/agent/position/position.cpp
-    strategy/agent/position/root_robot_position.cpp
+    strategy/agent/position/robot_factory_position.cpp
     strategy/agent/position/goalie.cpp
     strategy/agent/position/offense.cpp
     strategy/agent/position/defense.cpp

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

@@ -44,7 +44,7 @@ AgentActionClient::AgentActionClient(int r_id)
         });
 
     //Default Positions with the Position class
-    current_position_ = std::make_unique<RootRobotPosition>(robot_id_);
+    current_position_ = std::make_unique<RobotFactoryPosition>(robot_id_);
 
     // Create clients
     for (size_t i = 0; i < kNumShells; i++) {
@@ -116,12 +116,20 @@ void AgentActionClient::get_task() {
 
     auto optional_task = current_position_->get_task(last_world_state_, field_dimensions_);
 
+    if (robot_id_ == 1) {
+        SPDLOG_INFO("ID: {} get_task: {}", robot_id_, optional_task->motion_command.name);
+    }
+
+
     if (optional_task.has_value()) {
         RobotIntent task = optional_task.value();
 
         // note that because these are our RobotIntent structs, this comparison
         // uses our custom struct overloads
         if (task != last_task_) {
+            if (robot_id_ == 1) {
+                            SPDLOG_INFO("New task: {} Last task: {}", task.motion_command.name, last_task_.motion_command.name);
+            }
             last_task_ = task;
             send_new_goal();
         }
@@ -136,6 +144,10 @@ void AgentActionClient::send_new_goal() {
         rclcpp::shutdown();
     }
 
+    if (robot_id_ == 1) {
+        SPDLOG_INFO("{} gets a new goal", robot_id_);
+    }
+
     auto goal_msg = RobotMove::Goal();
     goal_msg.robot_intent = rj_convert::convert_to_ros(last_task_);
 

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

@@ -23,7 +23,7 @@
 #include "rj_msgs/action/robot_move.hpp"
 
 #include "strategy/agent/position/position.hpp"
-#include "strategy/agent/position/root_robot_position.hpp"
+#include "strategy/agent/position/robot_factory_position.hpp"
 
 #include "world_state.hpp"
 

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

@@ -1,8 +1,8 @@
-#include "root_robot_position.hpp"
+#include "robot_factory_position.hpp"
 
 namespace strategy {
 
-RootRobotPosition::RootRobotPosition(int r_id) : Position(r_id) { position_name_ = "RootRobotPosition";
+RobotFactoryPosition::RobotFactoryPosition(int r_id) : Position(r_id) { position_name_ = "RobotFactoryPosition";
 
 if (robot_id_ == 0) {
         current_position_ = std::make_unique<Goalie>(robot_id_);
@@ -14,41 +14,45 @@ if (robot_id_ == 0) {
 
 }
 
-std::optional<RobotIntent> RootRobotPosition::derived_get_task(RobotIntent intent) {
+std::optional<RobotIntent> RobotFactoryPosition::derived_get_task(RobotIntent intent) {
     // This is where the current_position can be reassigned based on the PlayState
     return current_position_->get_task(*last_world_state_, field_dimensions_);
 }
 
-void RootRobotPosition::receive_communication_response(communication::AgentPosResponseWrapper response) {
+void RobotFactoryPosition::receive_communication_response(communication::AgentPosResponseWrapper response) {
     // Call to super
     current_position_->receive_communication_response(response);
 
 }
 
-communication::PosAgentResponseWrapper RootRobotPosition::receive_communication_request(
+communication::PosAgentResponseWrapper RobotFactoryPosition::receive_communication_request(
     communication::AgentPosRequestWrapper request) {
 
     // Return the response
     return current_position_->receive_communication_request(request);
 }
 
-void RootRobotPosition::derived_acknowledge_pass() {
+void RobotFactoryPosition::derived_acknowledge_pass() {
     current_position_->derived_acknowledge_pass();
 }
 
-void RootRobotPosition::derived_pass_ball() {
+void RobotFactoryPosition::derived_pass_ball() {
     current_position_->derived_pass_ball();
 }
 
-void RootRobotPosition::derived_acknowledge_ball_in_transit() {
+void RobotFactoryPosition::derived_acknowledge_ball_in_transit() {
     current_position_->derived_acknowledge_ball_in_transit();
 }
 
-void RootRobotPosition::die() {
+void RobotFactoryPosition::set_is_done() {
+    current_position_->set_is_done();
+}
+
+void RobotFactoryPosition::die() {
     current_position_->die();
 }
 
-void RootRobotPosition::revive() {
+void RobotFactoryPosition::revive() {
     current_position_->revive();
 }
 

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

@@ -7,6 +7,9 @@
 #include <spdlog/spdlog.h>
 
 #include <rj_msgs/action/robot_move.hpp>
+#include <spdlog/spdlog.h>
+
+
 
 #include "planning/instant.hpp"
 #include "position.hpp"
@@ -25,11 +28,14 @@
 
 namespace strategy {
 
+/*
+* Based on the Factory Design pattern.
+*/
 
-class RootRobotPosition : public Position {
+class RobotFactoryPosition : public Position {
 public:
-    RootRobotPosition(int r_id);
-    ~RootRobotPosition() override = default;
+    RobotFactoryPosition(int r_id);
+    ~RobotFactoryPosition() override = default;
 
     void receive_communication_response(communication::AgentPosResponseWrapper response) override;
     communication::PosAgentResponseWrapper receive_communication_request(