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 f12219ffcf4..032c4ba75bf 100644 --- a/soccer/src/soccer/planning/planner/line_kick_path_planner.cpp +++ b/soccer/src/soccer/planning/planner/line_kick_path_planner.cpp @@ -13,6 +13,9 @@ using namespace rj_geometry; namespace planning { Trajectory LineKickPathPlanner::plan(const PlanRequest& plan_request) { + // If we are not allowed to touch the ball, this planner always fails + // This is preferred to simply ending the planner because it is possible (likely) + // that strategy re-requests the same planner anyway. if (plan_request.play_state == PlayState::halt() || plan_request.play_state == PlayState::stop()) { return Trajectory{}; @@ -29,14 +32,10 @@ Trajectory LineKickPathPlanner::plan(const PlanRequest& plan_request) { // // e.g. new_avg_vel = (0.8 * avg_vel) + (0.2 * new_vel) // - // TODO(Kevin): make this gain a ROS param like collect average_ball_vel_ = apply_low_pass_filter(average_ball_vel_, ball.velocity, 0.8); } - // only process state transition if already started the planning - if (has_created_plan) { - process_state_transition(); - } + process_state_transition(); switch (current_state_) { case INITIAL_APPROACH: prev_path_ = initial(plan_request); @@ -46,7 +45,6 @@ Trajectory LineKickPathPlanner::plan(const PlanRequest& plan_request) { break; } prev_path_.stamp(RJ::now()); - has_created_plan = true; return prev_path_; } @@ -54,54 +52,50 @@ Trajectory LineKickPathPlanner::initial(const PlanRequest& plan_request) { // Getting ball info const BallState& ball = plan_request.world_state->ball; - // Creating a modified plan_request to send to PathTargetPlanner - PlanRequest modified_request = plan_request; - - // Where to navigate to + // Distance to stay away from the ball auto distance_from_ball = kBallRadius + kRobotRadius + kAvoidBallBy; + // In case the ball is (slowly) moving auto ball_position = ball.predict_at(RJ::now() + RJ::Seconds{kPredictIn}).position; + // Along the vector from the goal to ball auto goal_to_ball = (plan_request.motion_command.target.position - ball_position); auto offset_from_ball = distance_from_ball * goal_to_ball.normalized(); - // Updating the motion command + // Create an updated MotionCommand and forward to PathTargetPathPlaner + PlanRequest modified_request = plan_request; + LinearMotionInstant target{ball_position - offset_from_ball}; 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 - Trajectory path = path_target_.plan(modified_request); - - return path; + return path_target_.plan(modified_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 is the speed (parameter) times the unit vector in the correct direction auto goal_to_ball = (plan_request.motion_command.target.position - ball.position); auto vel = goal_to_ball.normalized() * kFinalRobotSpeed; + // Create an updated MotionCommand and forward to PathTargetPathPlaner + PlanRequest modified_request = plan_request; + LinearMotionInstant target{ball.position, vel}; + MotionCommand modified_command{"path_target", target, - FacePoint{plan_request.motion_command.target.position}, true}; + FacePoint{plan_request.motion_command.target.position}}; modified_request.motion_command = modified_command; - // Getting the new path from PathTargetPlanner - Trajectory path = path_target_.plan(modified_request); - - return path; + return path_target_.plan(modified_request); } void LineKickPathPlanner::process_state_transition() { + // Let PathTarget decide when the first stage is done + // Possible problem: can PathTarget get stuck and loop infinitely? 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 0e451c79764..900c08b88d0 100644 --- a/soccer/src/soccer/planning/planner/line_kick_path_planner.hpp +++ b/soccer/src/soccer/planning/planner/line_kick_path_planner.hpp @@ -7,19 +7,22 @@ #include "planning/planner/path_target_path_planner.hpp" #include "planning/trajectory.hpp" -class Configuration; -class ConfigDouble; - namespace planning { /** * PathPlanner which plans a path to line kick a ball. - * Uses the System State object to get the position of the ball - * and predict its Path. It chooses the closest intersection point - * with the ball Path it can reach in time and plans a Path so the - * ball and robot intersect at the same time. + * It takes the ball's current trajectory and plans a path for the robot + * to intersect with the ball. + * + * It is in two stages. The first stage drives up to the ball, trying to avoid + * accidentally tapping it out of place. The second stage drives directly + * into the ball, hopefully kicking it if the hardware has been instructed + * to kick. * - * TODO(Kyle): Overhaul this entire planner. It's sketchy right now. + * Because of the two-stage system, this planner is *stateful*. It ought + * not to be destructed and re-constructed during a single execution; + * the best approach is to call plan() on each tick. + * However, it also shouldn't *completely* break if it is reset. * * Params taken from MotionCommand: * target.position - planner will kick to this point @@ -31,40 +34,49 @@ class LineKickPathPlanner : public PathPlanner { void reset() override { prev_path_ = {}; - target_kick_pos_ = std::nullopt; current_state_ = INITIAL_APPROACH; average_ball_vel_initialized_ = false; - has_created_plan = false; } + [[nodiscard]] bool is_done() const override; private: enum State { INITIAL_APPROACH, FINAL_APPROACH }; - State current_state_ = INITIAL_APPROACH; + + State current_state_{INITIAL_APPROACH}; + PathTargetPathPlanner path_target_{}; CollectPathPlanner collect_planner_{}; Trajectory prev_path_; // These constants could be tuned more - static constexpr double kIsDoneBallVel = 1.5; - static constexpr double kFinalRobotSpeed = 1.0; - static constexpr double kPredictIn = 0.5; // seconds - static constexpr double kAvoidBallBy = 0.05; + static constexpr double kIsDoneBallVel{1.5}; + static constexpr double kFinalRobotSpeed{1.0}; + static constexpr double kPredictIn{0.5}; // seconds + static constexpr double kAvoidBallBy{0.05}; + static constexpr double kLowPassFilterGain{0.2}; rj_geometry::Point average_ball_vel_; bool average_ball_vel_initialized_ = false; - bool has_created_plan = false; - std::optional target_kick_pos_; - // Trajectory initial(BallState ball, MotionCommand command, RobotInstant start_instant, - // ShapeSet static_obstacles, std::vector dynamic_obstacles); + /** + * Returns the trajectory during the initial stage. + * Uses PathTargetPathPlanner to draw a path to the spot to kick from. + * Avoids the ball + */ Trajectory initial(const PlanRequest& plan_request); + + /** + * Returns the trajectory during the final stage. + * Uses PathTargetPathPlanner to draw a path directly into the ball. + * Tries to hit the ball with the mouth of the robot. + */ Trajectory final(const PlanRequest& plan_request); - // Trajectory final(BallState ball, MotionCommand command, RobotInstant start_instant, ShapeSet - // static_obstacles, std::vector dynamic_obstacles); - void process_state_transition(); - // PlayState::State current_state_; + /** + * Decides if the intial approach is complete and updates internal state as necessary. + */ + void process_state_transition(); }; } // namespace planning diff --git a/soccer/src/soccer/strategy/agent/position/offense.cpp b/soccer/src/soccer/strategy/agent/position/offense.cpp index 2d97c3d851c..3c1a6ef680c 100644 --- a/soccer/src/soccer/strategy/agent/position/offense.cpp +++ b/soccer/src/soccer/strategy/agent/position/offense.cpp @@ -139,10 +139,7 @@ std::optional Offense::state_to_task(RobotIntent intent) { intent.dribbler_speed = 255.0; return intent; } else if (current_state_ == SHOOTING) { - rj_geometry::Point their_goal_pos = field_dimensions_.their_goal_loc(); - rj_geometry::Point scoring_point = - their_goal_pos + field_dimensions_.goal_width() * 3.0 / 8.0; - planning::LinearMotionInstant target{scoring_point}; + planning::LinearMotionInstant target{calculate_best_shot()}; auto line_kick_cmd = planning::MotionCommand{"line_kick", target}; intent.motion_command = line_kick_cmd; intent.shoot_mode = RobotIntent::ShootMode::KICK; @@ -416,4 +413,28 @@ void Offense::derived_acknowledge_ball_in_transit() { chasing_ball = false; } +rj_geometry::Point Offense::calculate_best_shot() { + // Goal location + rj_geometry::Point their_goal_pos = field_dimensions_.our_goal_loc(); + double goal_width = field_dimensions_.goal_width(); // 1.0 meters + + // Ball location + rj_geometry::Point ball_position = this->last_world_state_->ball.position; + + rj_geometry::Point best_shot = their_goal_pos; + double best_distance = -1.0; + rj_geometry::Point increment(0.05, 0); + rj_geometry::Point curr_point = + their_goal_pos - rj_geometry::Point(goal_width / 2.0, 0) + increment; + for (int i = 0; i < 19; i++) { + double distance = distance_from_their_robots(ball_position, curr_point); + if (distance > best_distance) { + best_distance = distance; + best_shot = curr_point; + } + curr_point = curr_point + increment; + } + return best_shot; +} + } // namespace strategy diff --git a/soccer/src/soccer/strategy/agent/position/offense.hpp b/soccer/src/soccer/strategy/agent/position/offense.hpp index 66b60bea296..705890dc482 100644 --- a/soccer/src/soccer/strategy/agent/position/offense.hpp +++ b/soccer/src/soccer/strategy/agent/position/offense.hpp @@ -71,8 +71,6 @@ class Offense : public Position { bool has_open_shot(); - double distance_from_their_robots(rj_geometry::Point tail, rj_geometry::Point head); - /** * @brief Send request to the other robots to see if this robot should be the scorer * @@ -114,6 +112,16 @@ class Offense : public Position { * status */ communication::Acknowledge receive_reset_scorer_request(); + + /** + * @brief Calcualtes the best location for a shot + */ + rj_geometry::Point calculate_best_shot(); + + /** + * @brief Calculates the distance of vector from other team's closest robot + */ + double distance_from_their_robots(rj_geometry::Point tail, rj_geometry::Point heaad); }; } // namespace strategy