Skip to content

Commit

Permalink
Fix Code Style On rewrite-linekicker (#2164)
Browse files Browse the repository at this point in the history
automated style fixes

Co-authored-by: petergarud <[email protected]>
  • Loading branch information
github-actions[bot] and petergarud authored Jan 22, 2024
1 parent 35fe2d9 commit b7574c9
Show file tree
Hide file tree
Showing 5 changed files with 24 additions and 24 deletions.
13 changes: 7 additions & 6 deletions soccer/src/soccer/planning/planner/line_kick_path_planner.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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
Expand All @@ -112,8 +114,7 @@ Trajectory LineKickPathPlanner::final(const PlanRequest& plan_request) {
}

void LineKickPathPlanner::state_transition(BallState ball, RobotInstant start_instant) {

Check warning on line 116 in soccer/src/soccer/planning/planner/line_kick_path_planner.cpp

View workflow job for this annotation

GitHub Actions / build-and-test

parameter 'ball' is unused

Check warning on line 116 in soccer/src/soccer/planning/planner/line_kick_path_planner.cpp

View workflow job for this annotation

GitHub Actions / build-and-test

parameter 'start_instant' is unused
if (current_state_ == INITIAL_APPROACH &&
(path_target_.is_done())) {
if (current_state_ == INITIAL_APPROACH && (path_target_.is_done())) {
current_state_ = FINAL_APPROACH;
}
}
Expand Down
14 changes: 7 additions & 7 deletions soccer/src/soccer/planning/planner/line_kick_path_planner.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -2,10 +2,10 @@

#include <optional>

#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;
Expand Down Expand Up @@ -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_{};
Expand All @@ -51,10 +49,12 @@ class LineKickPathPlanner : public PathPlanner {
bool average_ball_vel_initialized_ = false;
std::optional<rj_geometry::Point> target_kick_pos_;

// Trajectory initial(BallState ball, MotionCommand command, RobotInstant start_instant, ShapeSet static_obstacles, std::vector<DynamicObstacle> dynamic_obstacles);
// Trajectory initial(BallState ball, MotionCommand command, RobotInstant start_instant,
// ShapeSet static_obstacles, std::vector<DynamicObstacle> 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<DynamicObstacle> dynamic_obstacles);
// Trajectory final(BallState ball, MotionCommand command, RobotInstant start_instant, ShapeSet
// static_obstacles, std::vector<DynamicObstacle> dynamic_obstacles);
void state_transition(BallState ball, RobotInstant start_instant);

// PlayState::State current_state_;
Expand Down
5 changes: 3 additions & 2 deletions soccer/src/soccer/planning/planner/plan_request.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
11 changes: 5 additions & 6 deletions soccer/src/soccer/planning/planner/plan_request.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -8,8 +8,8 @@
#include <planning/planner/motion_command.hpp>
#include <planning/trajectory.hpp>

#include "context.hpp"
#include "../global_state.hpp"
#include "context.hpp"
#include "planning/dynamic_obstacle.hpp"
#include "planning/instant.hpp"
#include "planning/robot_constraints.hpp"
Expand All @@ -29,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),
Expand Down Expand Up @@ -110,7 +110,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.
*/
float min_dist_from_ball = 0;
Expand All @@ -119,7 +119,6 @@ struct PlanRequest {
* Dribbler Speed
*/
float dribbler_speed = 0;

};

/**
Expand Down
5 changes: 2 additions & 3 deletions soccer/src/soccer/strategy/agent/position/offense.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -83,7 +83,6 @@ Offense::State Offense::update_state() {
}

std::optional<RobotIntent> Offense::state_to_task(RobotIntent intent) {

SPDLOG_INFO(current_state_);

if (current_state_ == IDLING) {
Expand Down Expand Up @@ -137,8 +136,8 @@ std::optional<RobotIntent> 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) {
Expand Down

0 comments on commit b7574c9

Please sign in to comment.