Skip to content

Commit

Permalink
Fix Code Style On position_rewrite (#2108)
Browse files Browse the repository at this point in the history
automated style fixes

Co-authored-by: p-nayak11 <[email protected]>
Co-authored-by: Prabhanjan Nayak <[email protected]>
  • Loading branch information
3 people authored Nov 13, 2023
1 parent 9bd8204 commit c027010
Show file tree
Hide file tree
Showing 7 changed files with 29 additions and 41 deletions.
10 changes: 4 additions & 6 deletions soccer/src/soccer/strategy/agent/agent_action_client.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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<RobotFactoryPosition>(robot_id_);

// Create clients
Expand Down Expand Up @@ -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();
Expand Down Expand Up @@ -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<GoalHandleRobotMove::SharedPtr> future) {

Expand All @@ -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);
}
}

Expand Down
3 changes: 0 additions & 3 deletions soccer/src/soccer/strategy/agent/agent_action_client.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -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"

Expand Down Expand Up @@ -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_;
Expand Down
10 changes: 6 additions & 4 deletions soccer/src/soccer/strategy/agent/position/offense.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -8,9 +8,9 @@ Offense::Offense(int r_id) : Position(r_id) {
}

std::optional<RobotIntent> 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);
}

Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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;
Expand Down
3 changes: 2 additions & 1 deletion soccer/src/soccer/strategy/agent/position/position.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -5,7 +5,8 @@ namespace strategy {

Position::Position(int r_id) : robot_id_(r_id) {}

std::optional<RobotIntent> Position::get_task(WorldState& world_state, FieldDimensions& field_dimensions) {
std::optional<RobotIntent> 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;
Expand Down
3 changes: 2 additions & 1 deletion soccer/src/soccer/strategy/agent/position/position.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -66,7 +66,8 @@ class Position {
* https://www.sandordargo.com/blog/2022/08/24/tmp-and-nvi
*/

virtual std::optional<RobotIntent> get_task(WorldState& world_state, FieldDimensions& field_dimensions);
virtual std::optional<RobotIntent> get_task(WorldState& world_state,
FieldDimensions& field_dimensions);

// communication with AC
void update_play_state(PlayState play_state);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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<Goalie>(robot_id_);
} else if (robot_id_ == 1) {
current_position_ = std::make_unique<Offense>(robot_id_);
} else {
current_position_ = std::make_unique<Defense>(robot_id_);
}

}

std::optional<RobotIntent> RobotFactoryPosition::get_task(WorldState& world_state, FieldDimensions& field_dimensions) {
std::optional<RobotIntent> 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);
Expand All @@ -25,19 +26,20 @@ std::optional<RobotIntent> RobotFactoryPosition::derived_get_task(RobotIntent in
return std::nullopt;
}

std::optional<communication::PosAgentRequestWrapper> RobotFactoryPosition::send_communication_request() {
std::optional<communication::PosAgentRequestWrapper>
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);
}
Expand All @@ -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
Original file line number Diff line number Diff line change
Expand Up @@ -7,24 +7,20 @@
#include <spdlog/spdlog.h>

#include <rj_msgs/action/robot_move.hpp>
#include <spdlog/spdlog.h>



#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 {

Expand Down Expand Up @@ -55,7 +51,6 @@ class RobotFactoryPosition : public Position {
void revive() override;

private:

std::unique_ptr<Position> current_position_;

std::optional<RobotIntent> derived_get_task(RobotIntent intent) override;
Expand Down

0 comments on commit c027010

Please sign in to comment.