From d931867ae5dd3b1b742347e18bf965baca0a23f2 Mon Sep 17 00:00:00 2001 From: jvogt23 <91633361+jvogt23@users.noreply.github.com> Date: Sun, 3 Mar 2024 21:34:30 -0500 Subject: [PATCH] Fixed conversations for code style --- .../strategy/agent/position/defense.cpp | 9 +++--- .../strategy/agent/position/defense.hpp | 2 +- .../soccer/strategy/agent/position/marker.cpp | 31 +++++++++---------- .../soccer/strategy/agent/position/marker.hpp | 19 ++++++++---- 4 files changed, 34 insertions(+), 27 deletions(-) diff --git a/soccer/src/soccer/strategy/agent/position/defense.cpp b/soccer/src/soccer/strategy/agent/position/defense.cpp index db103996bec..247a41166d3 100644 --- a/soccer/src/soccer/strategy/agent/position/defense.cpp +++ b/soccer/src/soccer/strategy/agent/position/defense.cpp @@ -2,7 +2,7 @@ namespace strategy { -Defense::Defense(int r_id) : Position(r_id), marker_{static_cast(r_id)} { +Defense::Defense(int r_id) : Position(r_id), marker_{} { position_name_ = "Defense"; } @@ -38,8 +38,8 @@ Defense::State Defense::update_state() { // If a wall is already full, // Remove the robot with the highest ID from a wall // and make them a marker instead. - if (this->robot_id_ == *max_element(walling_robots_.begin(), walling_robots_.end()) && - walling_robots_.size() > MAX_WALLERS) { + if (walling_robots_.size() > kMaxWallers && + this->robot_id_ == *max_element(walling_robots_.begin(), walling_robots_.end())) { send_leave_wall_request(); next_state = ENTERING_MARKING; } @@ -73,7 +73,8 @@ Defense::State Defense::update_state() { } break; case ENTERING_MARKING: - int target_id = marker_.choose_target(world_state); + marker_.choose_target(world_state); + int target_id = marker_.get_target(); if (target_id == -1) { next_state = ENTERING_MARKING; } else { diff --git a/soccer/src/soccer/strategy/agent/position/defense.hpp b/soccer/src/soccer/strategy/agent/position/defense.hpp index c2eb3da3ccd..a23dfe056c7 100644 --- a/soccer/src/soccer/strategy/agent/position/defense.hpp +++ b/soccer/src/soccer/strategy/agent/position/defense.hpp @@ -43,7 +43,7 @@ class Defense : public Position { private: int move_ct_ = 0; - static constexpr int MAX_WALLERS{3}; + static constexpr int kMaxWallers {3}; /** * @brief The derived_get_task method returns the task for the defensive robot diff --git a/soccer/src/soccer/strategy/agent/position/marker.cpp b/soccer/src/soccer/strategy/agent/position/marker.cpp index d313e5badb7..582145e2dc4 100644 --- a/soccer/src/soccer/strategy/agent/position/marker.cpp +++ b/soccer/src/soccer/strategy/agent/position/marker.cpp @@ -1,11 +1,11 @@ #include "marker.hpp" namespace strategy { -Marker::Marker(u_int8_t robot_id) {} +Marker::Marker() {} std::optional Marker::get_task(RobotIntent intent, const WorldState* world_state, - FieldDimensions field_dimensions) { - rj_geometry::Point targetPoint = world_state->get_robot(false, target).pose.position(); + [[maybe_unused]] FieldDimensions field_dimensions) { + rj_geometry::Point targetPoint = world_state->get_robot(false, target_).pose.position(); rj_geometry::Point ballPoint = world_state->ball.position; rj_geometry::Point targetToBall = (ballPoint - targetPoint).normalized(0.55f); planning::LinearMotionInstant goal{targetPoint + targetToBall, rj_geometry::Point{0.0, 0.0}}; @@ -14,31 +14,30 @@ std::optional Marker::get_task(RobotIntent intent, const WorldState return intent; } -int Marker::choose_target(WorldState* ws) { +void Marker::choose_target(const WorldState* ws) { // TODO: (James Vogt, github: jvogt23) // If we ever use multiple Markers, they should choose different // robots to track from each other. Logic for this operation must be // added because multiple markers currently mark the same robot. - for (int i = 0; i < 11; i++) { - if (std::fabs(ws->get_robot(false, i).pose.position().x()) < 2.5 && - ws->get_robot(false, i).pose.position().y() < Y_BOUND && + for (int i = 0; i < kNumShells; i++) { + if (std::fabs(ws->get_robot(false, i).pose.position().x()) < kMarkerFollowCutoff && + ws->get_robot(false, i).pose.position().y() < kYBound && (ws->ball.position - ws->get_robot(false, i).pose.position()).mag() > .25) { - target = i; - return i; + target_ = i; + return; } } - target = -1; - return -1; + target_ = -1; } -bool Marker::target_out_of_bounds(WorldState* ws) { - if (target == -1) return true; - if (ws->get_robot(false, target).pose.position().y() > Y_BOUND) { - target = -1; +bool Marker::target_out_of_bounds(const WorldState* ws) { + if (target_ == -1) return true; + if (ws->get_robot(false, target_).pose.position().y() > kYBound) { + target_ = -1; return true; } return false; } -int Marker::get_target() { return target; } +int Marker::get_target() { return target_; } } // namespace strategy \ No newline at end of file diff --git a/soccer/src/soccer/strategy/agent/position/marker.hpp b/soccer/src/soccer/strategy/agent/position/marker.hpp index 519936cbe71..c4d23aa2668 100644 --- a/soccer/src/soccer/strategy/agent/position/marker.hpp +++ b/soccer/src/soccer/strategy/agent/position/marker.hpp @@ -17,18 +17,25 @@ namespace strategy { */ class Marker : public RoleInterface { private: - int target{-1}; + int target_{-1}; - static constexpr double Y_BOUND{4.5}; + static constexpr double kYBound{4.5}; + //Constant - Avoid marking robots that are right on the edge of the field. + static constexpr double kMarkerFollowCutoff {2.5}; public: - Marker(u_int8_t robot_id); + Marker(); ~Marker() = default; - std::optional get_task(RobotIntent intent, const WorldState* const world_state, + Marker(const Marker& other) = default; + Marker(Marker&& other) = default; + Marker& operator=(const Marker& other) = default; + Marker& operator=(Marker&& other) = default; + + std::optional get_task(RobotIntent intent, const WorldState* world_state, FieldDimensions field_dimensions) override; - int choose_target(WorldState* ws); + void choose_target(const WorldState* ws); int get_target(); - bool target_out_of_bounds(WorldState* ws); + bool target_out_of_bounds(const WorldState* ws); }; } // namespace strategy