From b31191b042403c818811c7c7ab6ea9e89372a8ca Mon Sep 17 00:00:00 2001 From: p-nayak11 Date: Sun, 5 Nov 2023 22:14:18 -0500 Subject: [PATCH] communication is properly forwarded to subclasses --- .../strategy/agent/agent_action_client.cpp | 7 ++----- .../strategy/agent/position/defense.cpp | 4 ++++ .../strategy/agent/position/offense.cpp | 4 ++-- .../strategy/agent/position/position.hpp | 2 +- .../agent/position/robot_factory_position.cpp | 20 +++++++++---------- .../agent/position/robot_factory_position.hpp | 4 +++- 6 files changed, 21 insertions(+), 20 deletions(-) diff --git a/soccer/src/soccer/strategy/agent/agent_action_client.cpp b/soccer/src/soccer/strategy/agent/agent_action_client.cpp index 4084ebfb7cd..b933264ff3e 100644 --- a/soccer/src/soccer/strategy/agent/agent_action_client.cpp +++ b/soccer/src/soccer/strategy/agent/agent_action_client.cpp @@ -128,11 +128,6 @@ 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(); @@ -289,6 +284,7 @@ void AgentActionClient::receive_communication_callback( const std::shared_ptr& request, const std::shared_ptr& response) { if (current_position_ == nullptr) { + SPDLOG_INFO("I am getting stuck here!\n"); communication::AgentResponse agent_response; communication::AgentRequest agent_request = rj_convert::convert_from_ros(request->agent_request); @@ -298,6 +294,7 @@ void AgentActionClient::receive_communication_callback( agent_response.response = acknowledge; response->agent_response = rj_convert::convert_to_ros(agent_response); } else { + SPDLOG_INFO("I am NOT getting stuck here!\n"); // Convert agent request into AgentToPosCommRequest communication::AgentPosRequestWrapper agent_request; communication::AgentRequest received_request = diff --git a/soccer/src/soccer/strategy/agent/position/defense.cpp b/soccer/src/soccer/strategy/agent/position/defense.cpp index 78b0ebfb528..75b06acf911 100644 --- a/soccer/src/soccer/strategy/agent/position/defense.cpp +++ b/soccer/src/soccer/strategy/agent/position/defense.cpp @@ -128,6 +128,8 @@ void Defense::receive_communication_response(communication::AgentPosResponseWrap // Call to super Position::receive_communication_response(response); + SPDLOG_INFO("ID: {} is calling comm response!\n"); + // Handle join wall response if (const communication::JoinWallRequest* join_request = std::get_if(&response.associated_request)) { @@ -146,6 +148,8 @@ communication::PosAgentResponseWrapper Defense::receive_communication_request( communication::PosAgentResponseWrapper response = Position::receive_communication_request(request); + SPDLOG_INFO("ID: {} is calling comm request!\n"); + // Handle join and leave wall request if (const communication::JoinWallRequest* join_request = std::get_if(&request.request)) { diff --git a/soccer/src/soccer/strategy/agent/position/offense.cpp b/soccer/src/soccer/strategy/agent/position/offense.cpp index 78a80aeac57..41137100e3f 100644 --- a/soccer/src/soccer/strategy/agent/position/offense.cpp +++ b/soccer/src/soccer/strategy/agent/position/offense.cpp @@ -8,9 +8,9 @@ Offense::Offense(int r_id) : Position(r_id) { } std::optional Offense::derived_get_task(RobotIntent intent) { - SPDLOG_INFO("MY ID: {} in offense derived task!\n", robot_id_); + //SPDLOG_INFO("MY ID: {} in offense derived task!\n", robot_id_); current_state_ = update_state(); - SPDLOG_INFO("My current offense state is {}", current_state_); + //SPDLOG_INFO("My current offense state is {}", current_state_); return state_to_task(intent); } diff --git a/soccer/src/soccer/strategy/agent/position/position.hpp b/soccer/src/soccer/strategy/agent/position/position.hpp index d2715aa583c..adce297fce7 100644 --- a/soccer/src/soccer/strategy/agent/position/position.hpp +++ b/soccer/src/soccer/strategy/agent/position/position.hpp @@ -102,7 +102,7 @@ class Position { * * @return communication::PosAgentRequestWrapper the request to be sent */ - std::optional send_communication_request(); + virtual std::optional send_communication_request(); /** * @brief Receive the response from a sent request. diff --git a/soccer/src/soccer/strategy/agent/position/robot_factory_position.cpp b/soccer/src/soccer/strategy/agent/position/robot_factory_position.cpp index 6bf591bb6a7..7b900967e17 100644 --- a/soccer/src/soccer/strategy/agent/position/robot_factory_position.cpp +++ b/soccer/src/soccer/strategy/agent/position/robot_factory_position.cpp @@ -14,27 +14,25 @@ if (robot_id_ == 0) { } -std::optional RobotFactoryPosition::get_task(WorldState& ws, FieldDimensions& fd) { - return current_position_->get_task(ws, fd); -} - -std::optional RobotFactoryPosition::derived_get_task(RobotIntent intent) { +std::optional RobotFactoryPosition::get_task(WorldState& world_state, FieldDimensions& field_dimensions) { // This is where the current_position can be reassigned based on the // PlayState + return current_position_->get_task(world_state, field_dimensions); +} - - SPDLOG_ERROR("This should not be called"); +std::optional RobotFactoryPosition::derived_get_task(RobotIntent intent) { + SPDLOG_ERROR("RobotFactory derived_get_task() should not be called!"); return std::nullopt; +} - // no longer called - - // return current_position_->get_task(*last_world_state_, field_dimensions_); +std::optional RobotFactoryPosition::send_communication_request() { + // Call to super + return current_position_->send_communication_request(); } void RobotFactoryPosition::receive_communication_response(communication::AgentPosResponseWrapper response) { // Call to super current_position_->receive_communication_response(response); - } communication::PosAgentResponseWrapper RobotFactoryPosition::receive_communication_request( diff --git a/soccer/src/soccer/strategy/agent/position/robot_factory_position.hpp b/soccer/src/soccer/strategy/agent/position/robot_factory_position.hpp index 2cb299ac2b9..2134c22b229 100644 --- a/soccer/src/soccer/strategy/agent/position/robot_factory_position.hpp +++ b/soccer/src/soccer/strategy/agent/position/robot_factory_position.hpp @@ -40,9 +40,12 @@ class RobotFactoryPosition : public Position { std::optional get_task(WorldState&, FieldDimensions&) override; void receive_communication_response(communication::AgentPosResponseWrapper response) override; + communication::PosAgentResponseWrapper receive_communication_request( communication::AgentPosRequestWrapper request) override; + std::optional send_communication_request() override; + void derived_acknowledge_pass() override; void derived_pass_ball() override; void derived_acknowledge_ball_in_transit() override; @@ -56,7 +59,6 @@ class RobotFactoryPosition : public Position { std::unique_ptr current_position_; std::optional derived_get_task(RobotIntent intent) override; - }; } // namespace strategy