From 00e6ef9e69d223bf5e9ca6782fec322c39f49494 Mon Sep 17 00:00:00 2001 From: "github-actions[bot]" <41898282+github-actions[bot]@users.noreply.github.com> Date: Sun, 19 Jan 2025 21:49:29 -0500 Subject: [PATCH] Fix Code Style On settle-collect (#2321) automated style fixes Co-authored-by: sanatd33 --- .../planning/planner/collect_path_planner.cpp | 48 +++++++++---------- .../planning/planner/collect_path_planner.hpp | 10 ++-- .../planning/planner/settle_path_planner.cpp | 9 ++-- .../strategy/agent/position/solo_offense.cpp | 3 +- 4 files changed, 35 insertions(+), 35 deletions(-) diff --git a/soccer/src/soccer/planning/planner/collect_path_planner.cpp b/soccer/src/soccer/planning/planner/collect_path_planner.cpp index 0a99c04303..2e5cedf1d7 100644 --- a/soccer/src/soccer/planning/planner/collect_path_planner.cpp +++ b/soccer/src/soccer/planning/planner/collect_path_planner.cpp @@ -142,9 +142,10 @@ Trajectory CollectPathPlanner::plan(const PlanRequest& plan_request) { case Intercept: { Point delta_pos; Point face_pos; - + calc_delta_pos_for_dir(ball, start_instant, &delta_pos, &face_pos); - previous_ = intercept(plan_request, start_instant, static_obstacles, dynamic_obstacles, delta_pos, face_pos); + previous_ = intercept(plan_request, start_instant, static_obstacles, dynamic_obstacles, + delta_pos, face_pos); break; } default: @@ -152,7 +153,6 @@ Trajectory CollectPathPlanner::plan(const PlanRequest& plan_request) { break; } - return previous_; } @@ -170,8 +170,8 @@ void CollectPathPlanner::check_solution_validity(BallState ball, RobotInstant st } } -void CollectPathPlanner::process_state_transition(const PlanRequest& request, BallState ball, RobotInstant start_instant) { - +void CollectPathPlanner::process_state_transition(const PlanRequest& request, BallState ball, + RobotInstant start_instant) { // If the ball is moving, intercept // if not, regularly approach if (current_state_ == CoarseApproach && average_ball_vel_.mag() > 0.2) { @@ -243,14 +243,14 @@ Trajectory CollectPathPlanner::coarse_approach( LinearMotionInstant target_slow{path_coarse_target_, target_slow_vel}; Replanner::PlanParams params{start, - target_slow, - static_obstacles, - dynamic_obstacles, - plan_request.field_dimensions, - plan_request.constraints, - AngleFns::face_point(ball.position), - plan_request.shell_id}; - + target_slow, + static_obstacles, + dynamic_obstacles, + plan_request.field_dimensions, + plan_request.constraints, + AngleFns::face_point(ball.position), + plan_request.shell_id}; + Trajectory coarse_path = Replanner::create_plan(params, previous_); if (plan_request.debug_drawer != nullptr) { @@ -267,8 +267,8 @@ Trajectory CollectPathPlanner::coarse_approach( } void CollectPathPlanner::calc_delta_pos_for_dir(BallState ball, RobotInstant start_instant, - rj_geometry::Point* delta_robot_pos, - rj_geometry::Point* face_pos) { + rj_geometry::Point* delta_robot_pos, + rj_geometry::Point* face_pos) { // Get angle between target and normal hit Point normal_face_vector = ball.position - start_instant.position(); @@ -276,18 +276,18 @@ void CollectPathPlanner::calc_delta_pos_for_dir(BallState ball, RobotInstant sta // possibilities *delta_robot_pos = Point(0, 0); - *face_pos = start_instant.position() + - Point::direction(normal_face_vector.angle()) * 10; + *face_pos = start_instant.position() + Point::direction(normal_face_vector.angle()) * 10; } -Trajectory CollectPathPlanner::intercept(const PlanRequest& plan_request, RobotInstant start_instant, - const rj_geometry::ShapeSet& static_obstacles, - const std::vector& dynamic_obstacles, - rj_geometry::Point delta_pos, rj_geometry::Point face_pos) { - +Trajectory CollectPathPlanner::intercept(const PlanRequest& plan_request, + RobotInstant start_instant, + const rj_geometry::ShapeSet& static_obstacles, + const std::vector& dynamic_obstacles, + rj_geometry::Point delta_pos, + rj_geometry::Point face_pos) { const double max_ball_angle_change_for_path_reset = settle::PARAM_max_ball_angle_for_reset * M_PI / 180.0f; - + BallState ball = plan_request.world_state->ball; // If the ball changed directions or magnitude really quickly, do a reset of @@ -297,7 +297,7 @@ Trajectory CollectPathPlanner::intercept(const PlanRequest& plan_request, RobotI first_intercept_target_found_ = false; average_ball_vel_initialized_ = false; } - + // Try find best point to intercept using brute force method // where we check ever X distance along the ball velocity vector // diff --git a/soccer/src/soccer/planning/planner/collect_path_planner.hpp b/soccer/src/soccer/planning/planner/collect_path_planner.hpp index baa6809d5f..eddb405fb6 100644 --- a/soccer/src/soccer/planning/planner/collect_path_planner.hpp +++ b/soccer/src/soccer/planning/planner/collect_path_planner.hpp @@ -41,15 +41,15 @@ class CollectPathPlanner : public PathPlanner { // and won't intercept ball correctly anymore void check_solution_validity(BallState ball, RobotInstant start); - void process_state_transition(const PlanRequest& request, BallState ball, RobotInstant start_instant); + void process_state_transition(const PlanRequest& request, BallState ball, + RobotInstant start_instant); Trajectory coarse_approach( const PlanRequest& plan_request, RobotInstant start, const rj_geometry::ShapeSet& static_obstacles, const std::vector& dynamic_obstacles); - Trajectory intercept(const PlanRequest& plan_request, - RobotInstant start_instant, + Trajectory intercept(const PlanRequest& plan_request, RobotInstant start_instant, const rj_geometry::ShapeSet& static_obstacles, const std::vector& dynamic_obstacles, rj_geometry::Point delta_pos, rj_geometry::Point face_pos); @@ -70,8 +70,8 @@ class CollectPathPlanner : public PathPlanner { // Calculate the delta position to get the robot in the correct location // And the face point to get the bounce right towards their goal - void calc_delta_pos_for_dir(BallState ball, RobotInstant start_instant, rj_geometry::Point* delta_robot_pos, - rj_geometry::Point* face_pos); + void calc_delta_pos_for_dir(BallState ball, RobotInstant start_instant, + rj_geometry::Point* delta_robot_pos, rj_geometry::Point* face_pos); Trajectory previous_; diff --git a/soccer/src/soccer/planning/planner/settle_path_planner.cpp b/soccer/src/soccer/planning/planner/settle_path_planner.cpp index 2c454d576b..2bde6e3501 100644 --- a/soccer/src/soccer/planning/planner/settle_path_planner.cpp +++ b/soccer/src/soccer/planning/planner/settle_path_planner.cpp @@ -468,13 +468,14 @@ Trajectory SettlePathPlanner::dampen(const PlanRequest& plan_request, RobotInsta LinearMotionInstant final_stopping_motion{final_stopping_point}; Trajectory dampen_end; - + if (previous_.empty()) { dampen_end = CreatePath::simple(start_instant.linear_motion(), final_stopping_motion, - plan_request.constraints.mot, start_instant.stamp); + plan_request.constraints.mot, start_instant.stamp); } else { - dampen_end = CreatePath::simple(previous_.instant_at(previous_.num_instants() - 1).linear_motion(), final_stopping_motion, - plan_request.constraints.mot, start_instant.stamp); + dampen_end = CreatePath::simple( + previous_.instant_at(previous_.num_instants() - 1).linear_motion(), + final_stopping_motion, plan_request.constraints.mot, start_instant.stamp); } dampen_end.set_debug_text("Damping"); diff --git a/soccer/src/soccer/strategy/agent/position/solo_offense.cpp b/soccer/src/soccer/strategy/agent/position/solo_offense.cpp index 872994b4d4..de80ddd568 100644 --- a/soccer/src/soccer/strategy/agent/position/solo_offense.cpp +++ b/soccer/src/soccer/strategy/agent/position/solo_offense.cpp @@ -99,8 +99,7 @@ std::optional SoloOffense::state_to_task(RobotIntent intent) { robotToBall = robotToBall.normalized(length); planning::LinearMotionInstant target{ last_world_state_->get_robot(true, robot_id_).pose.position() + robotToBall}; - auto pivot_cmd = - planning::MotionCommand{"collect"}; + auto pivot_cmd = planning::MotionCommand{"collect"}; intent.motion_command = pivot_cmd; intent.dribbler_speed = 255; return intent;