From d263cf8f0c98739d24ca938bf51bfc9b7124b1cc Mon Sep 17 00:00:00 2001 From: "github-actions[bot]" <41898282+github-actions[bot]@users.noreply.github.com> Date: Sun, 21 Jan 2024 20:51:53 -0500 Subject: [PATCH] Fix Code Style On rewrite-linekicker (#2164) automated style fixes Co-authored-by: petergarud --- .../planning/planner/line_kick_path_planner.cpp | 13 +++++++------ .../planning/planner/line_kick_path_planner.hpp | 14 +++++++------- .../src/soccer/planning/planner/plan_request.cpp | 5 +++-- .../src/soccer/planning/planner/plan_request.hpp | 9 ++++----- .../src/soccer/strategy/agent/position/offense.cpp | 5 ++--- 5 files changed, 23 insertions(+), 23 deletions(-) diff --git a/soccer/src/soccer/planning/planner/line_kick_path_planner.cpp b/soccer/src/soccer/planning/planner/line_kick_path_planner.cpp index 7459f0ebafe..8e2b863ed66 100644 --- a/soccer/src/soccer/planning/planner/line_kick_path_planner.cpp +++ b/soccer/src/soccer/planning/planner/line_kick_path_planner.cpp @@ -80,7 +80,8 @@ Trajectory LineKickPathPlanner::initial(const PlanRequest& plan_request) { // Updating the motion command LinearMotionInstant target{ball_position - offset_from_ball}; - MotionCommand modified_command{"path_target", target, FacePoint{plan_request.motion_command.target.position}}; + MotionCommand modified_command{"path_target", target, + FacePoint{plan_request.motion_command.target.position}}; modified_request.motion_command = modified_command; // Getting the new path from PathTargetPlanner @@ -91,18 +92,19 @@ Trajectory LineKickPathPlanner::initial(const PlanRequest& plan_request) { Trajectory LineKickPathPlanner::final(const PlanRequest& plan_request) { // remove ball from obstacles not needed? - + const BallState& ball = plan_request.world_state->ball; // // Creating a modified plan_request to send to PathTargetPlanner PlanRequest modified_request = plan_request; - //velocity + // velocity auto goal_to_ball = (plan_request.motion_command.target.position - ball.position); auto vel = goal_to_ball.normalized() * 1.0; LinearMotionInstant target{ball.position, vel}; - MotionCommand modified_command{"path_target", target, FacePoint{plan_request.motion_command.target.position}, true}; + MotionCommand modified_command{"path_target", target, + FacePoint{plan_request.motion_command.target.position}, true}; modified_request.motion_command = modified_command; // Getting the new path from PathTargetPlanner @@ -112,8 +114,7 @@ Trajectory LineKickPathPlanner::final(const PlanRequest& plan_request) { } void LineKickPathPlanner::state_transition(BallState ball, RobotInstant start_instant) { - if (current_state_ == INITIAL_APPROACH && - (path_target_.is_done())) { + if (current_state_ == INITIAL_APPROACH && (path_target_.is_done())) { current_state_ = FINAL_APPROACH; } } diff --git a/soccer/src/soccer/planning/planner/line_kick_path_planner.hpp b/soccer/src/soccer/planning/planner/line_kick_path_planner.hpp index 4349e43c9f1..75ba5ffdbc7 100644 --- a/soccer/src/soccer/planning/planner/line_kick_path_planner.hpp +++ b/soccer/src/soccer/planning/planner/line_kick_path_planner.hpp @@ -2,10 +2,10 @@ #include +#include "planning/planner/collect_path_planner.hpp" #include "planning/planner/path_planner.hpp" -#include "planning/trajectory.hpp" #include "planning/planner/path_target_path_planner.hpp" -#include "planning/planner/collect_path_planner.hpp" +#include "planning/trajectory.hpp" class Configuration; class ConfigDouble; @@ -38,9 +38,7 @@ class LineKickPathPlanner : public PathPlanner { [[nodiscard]] bool is_done() const override; private: - enum State { - INITIAL_APPROACH, FINAL_APPROACH - }; + enum State { INITIAL_APPROACH, FINAL_APPROACH }; State current_state_ = INITIAL_APPROACH; PathTargetPathPlanner path_target_{}; CollectPathPlanner collect_planner_{}; @@ -51,10 +49,12 @@ class LineKickPathPlanner : public PathPlanner { bool average_ball_vel_initialized_ = false; std::optional target_kick_pos_; - // Trajectory initial(BallState ball, MotionCommand command, RobotInstant start_instant, ShapeSet static_obstacles, std::vector dynamic_obstacles); + // Trajectory initial(BallState ball, MotionCommand command, RobotInstant start_instant, + // ShapeSet static_obstacles, std::vector dynamic_obstacles); Trajectory initial(const PlanRequest& plan_request); Trajectory final(const PlanRequest& plan_request); - // Trajectory final(BallState ball, MotionCommand command, RobotInstant start_instant, ShapeSet static_obstacles, std::vector dynamic_obstacles); + // Trajectory final(BallState ball, MotionCommand command, RobotInstant start_instant, ShapeSet + // static_obstacles, std::vector dynamic_obstacles); void state_transition(BallState ball, RobotInstant start_instant); // PlayState::State current_state_; diff --git a/soccer/src/soccer/planning/planner/plan_request.cpp b/soccer/src/soccer/planning/planner/plan_request.cpp index d838b3b038e..09a7c225ebe 100644 --- a/soccer/src/soccer/planning/planner/plan_request.cpp +++ b/soccer/src/soccer/planning/planner/plan_request.cpp @@ -66,8 +66,9 @@ void fill_obstacles(const PlanRequest& in, rj_geometry::ShapeSet* out_static, // Adding ball as a static obstacle (because dynamic obstacles are not working) // Only added when STOP state is enabled if (in.min_dist_from_ball > 0 || avoid_ball) { - auto ball_obs = make_inflated_static_obs(in.world_state->ball.position, - in.world_state->ball.velocity, kBallRadius + kAvoidBallDistance); + auto ball_obs = + make_inflated_static_obs(in.world_state->ball.position, in.world_state->ball.velocity, + kBallRadius + kAvoidBallDistance); ball_obs.radius(ball_obs.radius() + in.min_dist_from_ball); // Draw ball obstacle in simulator diff --git a/soccer/src/soccer/planning/planner/plan_request.hpp b/soccer/src/soccer/planning/planner/plan_request.hpp index 5bcab63892f..80e0d8a184e 100644 --- a/soccer/src/soccer/planning/planner/plan_request.hpp +++ b/soccer/src/soccer/planning/planner/plan_request.hpp @@ -10,7 +10,6 @@ #include "../global_state.hpp" #include "context.hpp" -#include "../global_state.hpp" #include "planning/dynamic_obstacle.hpp" #include "planning/instant.hpp" #include "planning/robot_constraints.hpp" @@ -30,9 +29,9 @@ struct PlanRequest { PlanRequest(RobotInstant start, MotionCommand command, // NOLINT RobotConstraints constraints, rj_geometry::ShapeSet field_obstacles, rj_geometry::ShapeSet virtual_obstacles, TrajectoryCollection* planned_trajectories, - unsigned shell_id, const WorldState* world_state, PlayState play_state, int8_t priority = 0, - rj_drawing::RosDebugDrawer* debug_drawer = nullptr, bool ball_sense = false, - float min_dist_from_ball = 0, float dribbler_speed = 0) + unsigned shell_id, const WorldState* world_state, PlayState play_state, + int8_t priority = 0, rj_drawing::RosDebugDrawer* debug_drawer = nullptr, + bool ball_sense = false, float min_dist_from_ball = 0, float dribbler_speed = 0) : start(start), motion_command(command), // NOLINT constraints(constraints), @@ -112,6 +111,7 @@ struct PlanRequest { // Whether the robot has a ball bool ball_sense = false; + /** /** * How far away to stay from the ball, if the MotionCommand chooses to avoid the ball. */ @@ -121,7 +121,6 @@ struct PlanRequest { * Dribbler Speed */ float dribbler_speed = 0; - }; /** diff --git a/soccer/src/soccer/strategy/agent/position/offense.cpp b/soccer/src/soccer/strategy/agent/position/offense.cpp index 8d27472e089..09d6583268c 100644 --- a/soccer/src/soccer/strategy/agent/position/offense.cpp +++ b/soccer/src/soccer/strategy/agent/position/offense.cpp @@ -87,7 +87,6 @@ Offense::State Offense::update_state() { } std::optional Offense::state_to_task(RobotIntent intent) { - SPDLOG_INFO(current_state_); if (current_state_ == IDLING) { @@ -140,8 +139,8 @@ std::optional Offense::state_to_task(RobotIntent intent) { intent.is_active = true; // intent.motion_command = planning::MotionCommand{ - // "path_target", planning::LinearMotionInstant{last_world_state_->ball.position, {0.0}}, - // planning::FaceBall{}}; + // "path_target", planning::LinearMotionInstant{last_world_state_->ball.position, + // {0.0}}, planning::FaceBall{}}; return intent; } else if (current_state_ == RECEIVING) {