diff --git a/soccer/src/soccer/strategy/agent/agent_action_client.cpp b/soccer/src/soccer/strategy/agent/agent_action_client.cpp index b03bec8a867..5a7188eca22 100644 --- a/soccer/src/soccer/strategy/agent/agent_action_client.cpp +++ b/soccer/src/soccer/strategy/agent/agent_action_client.cpp @@ -48,7 +48,7 @@ AgentActionClient::AgentActionClient(int r_id) receive_communication_callback(request, response); }); - //Default Positions with the Position class + // Default Positions with the Position class current_position_ = std::make_unique(robot_id_); // Create clients @@ -139,7 +139,8 @@ void AgentActionClient::get_task() { // 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); + SPDLOG_INFO("New task: {} Last task: {}", task.motion_command.name, + last_task_.motion_command.name); } last_task_ = task; send_new_goal(); @@ -171,19 +172,16 @@ void AgentActionClient::send_new_goal() { client_ptr_->async_send_goal(goal_msg, send_goal_options); } - [[nodiscard]] WorldState* AgentActionClient::world_state() { // thread-safe getter for world_state auto lock = std::lock_guard(world_state_mutex_); return &last_world_state_; } - // With the get_task function deleted, we need to initialize the new class once // in the initializer. This will call the class which will analyze the // situation based on the current tick. - void AgentActionClient::goal_response_callback( std::shared_future future) { @@ -198,7 +196,7 @@ void AgentActionClient::feedback_callback( double time_left = rj_convert::convert_from_ros(feedback->time_left).count(); if (current_position_ == nullptr) { - current_position_->set_time_left(time_left); + current_position_->set_time_left(time_left); } } diff --git a/soccer/src/soccer/strategy/agent/agent_action_client.hpp b/soccer/src/soccer/strategy/agent/agent_action_client.hpp index 60ef47c6495..85b868bb7e6 100644 --- a/soccer/src/soccer/strategy/agent/agent_action_client.hpp +++ b/soccer/src/soccer/strategy/agent/agent_action_client.hpp @@ -19,10 +19,8 @@ #include "rclcpp/rclcpp.hpp" #include "rclcpp_action/rclcpp_action.hpp" #include "rj_msgs/action/robot_move.hpp" - #include "strategy/agent/position/position.hpp" #include "strategy/agent/position/robot_factory_position.hpp" - #include "world_state.hpp" #include "game_state.hpp" @@ -86,7 +84,6 @@ class AgentActionClient : public rclcpp::Node { void get_task(); rclcpp::TimerBase::SharedPtr get_task_timer_; - // note that this is our RobotIntent struct (robot_intent.hpp), not a // pre-generated ROS msg type RobotIntent last_task_; diff --git a/soccer/src/soccer/strategy/agent/position/offense.cpp b/soccer/src/soccer/strategy/agent/position/offense.cpp index 41137100e3f..812f01bdb83 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); } @@ -216,7 +216,8 @@ void Offense::send_scorer_request() { scorer_request.robot_id = robot_id_; // Calculate distance to ball - rj_geometry::Point robot_position = last_world_state_->get_robot(true, robot_id_).pose.position(); + rj_geometry::Point robot_position = + last_world_state_->get_robot(true, robot_id_).pose.position(); rj_geometry::Point ball_position = last_world_state_->ball.position; double ball_distance = robot_position.dist_to(ball_position); scorer_request.ball_distance = ball_distance; @@ -248,7 +249,8 @@ communication::ScorerResponse Offense::receive_scorer_request( scorer_response.robot_id = robot_id_; // Calculate distance to ball - rj_geometry::Point robot_position = last_world_state_->get_robot(true, robot_id_).pose.position(); + rj_geometry::Point robot_position = + last_world_state_->get_robot(true, robot_id_).pose.position(); rj_geometry::Point ball_position = last_world_state_->ball.position; double ball_distance = robot_position.dist_to(ball_position); scorer_response.ball_distance = ball_distance; diff --git a/soccer/src/soccer/strategy/agent/position/position.cpp b/soccer/src/soccer/strategy/agent/position/position.cpp index 4dae10e5d87..70571fc274d 100644 --- a/soccer/src/soccer/strategy/agent/position/position.cpp +++ b/soccer/src/soccer/strategy/agent/position/position.cpp @@ -5,7 +5,8 @@ namespace strategy { Position::Position(int r_id) : robot_id_(r_id) {} -std::optional Position::get_task(WorldState& world_state, FieldDimensions& field_dimensions) { +std::optional Position::get_task(WorldState& world_state, + FieldDimensions& field_dimensions) { // Point class variables to parameter references // TODO (Prabhanjan): Don't copy references into local vars field_dimensions_ = field_dimensions; diff --git a/soccer/src/soccer/strategy/agent/position/position.hpp b/soccer/src/soccer/strategy/agent/position/position.hpp index 9e32a08d767..a42a9f8be71 100644 --- a/soccer/src/soccer/strategy/agent/position/position.hpp +++ b/soccer/src/soccer/strategy/agent/position/position.hpp @@ -66,7 +66,8 @@ class Position { * https://www.sandordargo.com/blog/2022/08/24/tmp-and-nvi */ - virtual std::optional get_task(WorldState& world_state, FieldDimensions& field_dimensions); + virtual std::optional get_task(WorldState& world_state, + FieldDimensions& field_dimensions); // communication with AC void update_play_state(PlayState play_state); 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 7b900967e17..77e6169e2e7 100644 --- a/soccer/src/soccer/strategy/agent/position/robot_factory_position.cpp +++ b/soccer/src/soccer/strategy/agent/position/robot_factory_position.cpp @@ -2,19 +2,20 @@ namespace strategy { -RobotFactoryPosition::RobotFactoryPosition(int r_id) : Position(r_id) { position_name_ = "RobotFactoryPosition"; +RobotFactoryPosition::RobotFactoryPosition(int r_id) : Position(r_id) { + position_name_ = "RobotFactoryPosition"; -if (robot_id_ == 0) { + if (robot_id_ == 0) { current_position_ = std::make_unique(robot_id_); } else if (robot_id_ == 1) { current_position_ = std::make_unique(robot_id_); } else { current_position_ = std::make_unique(robot_id_); } - } -std::optional RobotFactoryPosition::get_task(WorldState& world_state, FieldDimensions& field_dimensions) { +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); @@ -25,19 +26,20 @@ std::optional RobotFactoryPosition::derived_get_task(RobotIntent in return std::nullopt; } -std::optional RobotFactoryPosition::send_communication_request() { +std::optional +RobotFactoryPosition::send_communication_request() { // Call to super return current_position_->send_communication_request(); } -void RobotFactoryPosition::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 RobotFactoryPosition::receive_communication_request( communication::AgentPosRequestWrapper request) { - // Return the response return current_position_->receive_communication_request(request); } @@ -46,24 +48,16 @@ void RobotFactoryPosition::derived_acknowledge_pass() { current_position_->derived_acknowledge_pass(); } -void RobotFactoryPosition::derived_pass_ball() { - current_position_->derived_pass_ball(); -} +void RobotFactoryPosition::derived_pass_ball() { current_position_->derived_pass_ball(); } void RobotFactoryPosition::derived_acknowledge_ball_in_transit() { current_position_->derived_acknowledge_ball_in_transit(); } -void RobotFactoryPosition::set_is_done() { - current_position_->set_is_done(); -} +void RobotFactoryPosition::set_is_done() { current_position_->set_is_done(); } -void RobotFactoryPosition::die() { - current_position_->die(); -} +void RobotFactoryPosition::die() { current_position_->die(); } -void RobotFactoryPosition::revive() { - current_position_->revive(); -} +void RobotFactoryPosition::revive() { current_position_->revive(); } } // namespace strategy 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 f4b9ac2efed..3f82338a447 100644 --- a/soccer/src/soccer/strategy/agent/position/robot_factory_position.hpp +++ b/soccer/src/soccer/strategy/agent/position/robot_factory_position.hpp @@ -7,24 +7,20 @@ #include #include -#include - - +#include "game_state.hpp" #include "planning/instant.hpp" #include "position.hpp" -#include "game_state.hpp" #include "rj_common/field_dimensions.hpp" #include "rj_common/time.hpp" #include "rj_constants/constants.hpp" #include "rj_geometry/geometry_conversions.hpp" - -#include "strategy/agent/position/position.hpp" #include "strategy/agent/position/defense.hpp" #include "strategy/agent/position/goal_kicker.hpp" #include "strategy/agent/position/goalie.hpp" #include "strategy/agent/position/offense.hpp" #include "strategy/agent/position/penalty_player.hpp" +#include "strategy/agent/position/position.hpp" namespace strategy { @@ -55,7 +51,6 @@ class RobotFactoryPosition : public Position { void revive() override; private: - std::unique_ptr current_position_; std::optional derived_get_task(RobotIntent intent) override;