Valid intermediate Points (#2307)

* enforce points to be in range by giving field dimensions

* Fix Code Style On valid-intermediate-points (#2308)

automated style fixes

Co-authored-by: sanatd33 <sanatd33@users.noreply.github.com>

* pass a reference

---------

Co-authored-by: github-actions[bot] <41898282+github-actions[bot]@users.noreply.github.com>
Co-authored-by: sanatd33 <sanatd33@users.noreply.github.com>

soccer/src/soccer/planning/global_state.cpp

@@ -74,6 +74,10 @@ GlobalState::GlobalState(rclcpp::Node* node) {
     auto lock = std::lock_guard(last_world_state_mutex_);
     return &last_world_state_;
 }
+[[nodiscard]] const FieldDimensions* GlobalState::field_dimensions() const {
+    auto lock = std::lock_guard(last_field_dimensions_mutex_);
+    return &last_field_dimensions_;
+}
 
 rj_geometry::ShapeSet GlobalState::create_defense_area_obstacles() {
     // need field dimensions and to be initialized for this to

soccer/src/soccer/planning/global_state.hpp

@@ -34,6 +34,7 @@ class GlobalState {
     [[nodiscard]] rj_geometry::ShapeSet global_obstacles() const;
     [[nodiscard]] rj_geometry::ShapeSet def_area_obstacles() const;
     [[nodiscard]] const WorldState* world_state() const;
+    [[nodiscard]] const FieldDimensions* field_dimensions() const;
 
 private:
     rclcpp::Subscription<rj_msgs::msg::PlayState>::SharedPtr play_state_sub_;

soccer/src/soccer/planning/planner/collect_path_planner.cpp

@@ -224,8 +224,10 @@ Trajectory CollectPathPlanner::coarse_approach(
                                  target_slow,
                                  static_obstacles,
                                  dynamic_obstacles,
+                                 plan_request.field_dimensions,
                                  plan_request.constraints,
-                                 AngleFns::face_point(ball.position)};
+                                 AngleFns::face_point(ball.position),
+                                 plan_request.shell_id};
     Trajectory coarse_path = Replanner::create_plan(params, previous_);
 
     if (plan_request.debug_drawer != nullptr) {
@@ -282,8 +284,10 @@ Trajectory CollectPathPlanner::fine_approach(
                                  target_hit,
                                  static_obstacles,
                                  dynamic_obstacles,
+                                 plan_request.field_dimensions,
                                  plan_request.constraints,
-                                 AngleFns::face_point(ball.position)};
+                                 AngleFns::face_point(ball.position),
+                                 plan_request.shell_id};
     Trajectory path_hit = Replanner::create_plan(params, previous_);
 
     path_hit.set_debug_text("fine");
@@ -376,8 +380,10 @@ Trajectory CollectPathPlanner::control(const PlanRequest& plan_request, RobotIns
                                  target,
                                  static_obstacles,
                                  dynamic_obstacles,
+                                 plan_request.field_dimensions,
                                  plan_request.constraints,
-                                 AngleFns::face_point(ball.position)};
+                                 AngleFns::face_point(ball.position),
+                                 plan_request.shell_id};
 
     Trajectory path = Replanner::create_plan(params, previous_);
 
@@ -419,10 +425,14 @@ Trajectory CollectPathPlanner::invalid(const PlanRequest& plan_request,
     // programmatically
     LinearMotionInstant target{plan_request.start.position(), Point()};
 
-    Replanner::PlanParams params{
-        plan_request.start,       target,
-        static_obstacles,         dynamic_obstacles,
-        plan_request.constraints, AngleFns::face_point(plan_request.world_state->ball.position)};
+    Replanner::PlanParams params{plan_request.start,
+                                 target,
+                                 static_obstacles,
+                                 dynamic_obstacles,
+                                 plan_request.field_dimensions,
+                                 plan_request.constraints,
+                                 AngleFns::face_point(plan_request.world_state->ball.position),
+                                 plan_request.shell_id};
     Trajectory path = Replanner::create_plan(params, previous_);
     path.set_debug_text("Invalid state in collect");
 

soccer/src/soccer/planning/planner/goalie_idle_path_planner.cpp

@@ -31,7 +31,8 @@ Trajectory GoalieIdlePathPlanner::plan(const PlanRequest& plan_request) {
     // call Replanner to generate a Trajectory
     Trajectory trajectory = Replanner::create_plan(
         Replanner::PlanParams{plan_request.start, target, static_obstacles, dynamic_obstacles,
-                              plan_request.constraints, angle_function, RJ::Seconds(3.0)},
+                              plan_request.field_dimensions, plan_request.constraints,
+                              angle_function, plan_request.shell_id, RJ::Seconds(3.0)},
         std::move(previous_));
 
     // Debug drawing

soccer/src/soccer/planning/planner/path_target_path_planner.cpp

@@ -41,7 +41,8 @@ Trajectory PathTargetPathPlanner::plan(const PlanRequest& request) {
     // Call into the sub-object to actually execute the plan.
     Trajectory trajectory = Replanner::create_plan(
         Replanner::PlanParams{request.start, target_instant, static_obstacles, dynamic_obstacles,
-                              request.constraints, angle_function, RJ::Seconds(3.0)},
+                              request.field_dimensions, request.constraints, angle_function,
+                              request.shell_id, RJ::Seconds(3.0)},
         std::move(previous_));
 
     previous_ = trajectory;

soccer/src/soccer/planning/planner/plan_request.hpp

@@ -30,8 +30,9 @@ struct PlanRequest {
                 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)
+                const FieldDimensions* field_dimensions, 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),
@@ -42,6 +43,7 @@ struct PlanRequest {
           world_state(world_state),
           priority(priority),
           play_state(play_state),
+          field_dimensions(field_dimensions),
           debug_drawer(debug_drawer),
           ball_sense(ball_sense),
           min_dist_from_ball(min_dist_from_ball),
@@ -102,6 +104,11 @@ struct PlanRequest {
      */
     PlayState play_state;
 
+    /**
+     * the Field Dimensions
+     */
+    const FieldDimensions* field_dimensions;
+
     /**
      * Allows debug drawing in the world. If this is nullptr, no debug drawing
      * should be performed.

soccer/src/soccer/planning/planner/settle_path_planner.cpp

@@ -223,7 +223,8 @@ Trajectory SettlePathPlanner::intercept(const PlanRequest& plan_request, RobotIn
         // test location
         Trajectory path = CreatePath::intermediate(
             start_instant.linear_motion(), target_robot_intersection, plan_request.constraints.mot,
-            start_instant.stamp, static_obstacles);
+            start_instant.stamp, static_obstacles, dynamic_obstacles, plan_request.field_dimensions,
+            plan_request.shell_id);
 
         // Calculate the
         RJ::Seconds buffer_duration = ball_time - path.duration();
@@ -332,9 +333,10 @@ Trajectory SettlePathPlanner::intercept(const PlanRequest& plan_request, RobotIn
         LinearMotionInstant target{closest_pt,
                                    settle::PARAM_ball_speed_percent_for_dampen * average_ball_vel_};
 
-        Trajectory shortcut = CreatePath::intermediate(start_instant.linear_motion(), target,
-                                                       plan_request.constraints.mot,
-                                                       start_instant.stamp, static_obstacles);
+        Trajectory shortcut = CreatePath::intermediate(
+            start_instant.linear_motion(), target, plan_request.constraints.mot,
+            start_instant.stamp, static_obstacles, dynamic_obstacles, plan_request.field_dimensions,
+            plan_request.shell_id);
 
         if (!shortcut.empty()) {
             plan_angles(&shortcut, start_instant, AngleFns::face_point(face_pos),
@@ -361,9 +363,14 @@ Trajectory SettlePathPlanner::intercept(const PlanRequest& plan_request, RobotIn
     LinearMotionInstant target_robot_intersection{
         path_intercept_target_, settle::PARAM_ball_speed_percent_for_dampen * average_ball_vel_};
 
-    Replanner::PlanParams params{
-        start_instant,     target_robot_intersection, static_obstacles,
-        dynamic_obstacles, plan_request.constraints,  AngleFns::face_point(face_pos)};
+    Replanner::PlanParams params{start_instant,
+                                 target_robot_intersection,
+                                 static_obstacles,
+                                 dynamic_obstacles,
+                                 plan_request.field_dimensions,
+                                 plan_request.constraints,
+                                 AngleFns::face_point(face_pos),
+                                 plan_request.shell_id};
     Trajectory new_target_path = Replanner::create_plan(params, previous_);
 
     RJ::Seconds time_of_arrival = new_target_path.duration();
@@ -485,10 +492,14 @@ Trajectory SettlePathPlanner::invalid(const PlanRequest& plan_request,
     // programmatically
     LinearMotionInstant target{plan_request.start.position(), Point()};
 
-    Replanner::PlanParams params{
-        plan_request.start,       target,
-        static_obstacles,         dynamic_obstacles,
-        plan_request.constraints, AngleFns::face_point(plan_request.world_state->ball.position)};
+    Replanner::PlanParams params{plan_request.start,
+                                 target,
+                                 static_obstacles,
+                                 dynamic_obstacles,
+                                 plan_request.field_dimensions,
+                                 plan_request.constraints,
+                                 AngleFns::face_point(plan_request.world_state->ball.position),
+                                 plan_request.shell_id};
     Trajectory path = Replanner::create_plan(params, previous_);
     path.set_debug_text("Invalid state in settle");
     return path;

soccer/src/soccer/planning/planner_for_robot.cpp

@@ -217,6 +217,7 @@ PlanRequest PlannerForRobot::make_request(const RobotIntent& intent) {
                        static_cast<unsigned int>(robot_id_),
                        world_state,
                        play_state,
+                       global_state_.field_dimensions(),
                        intent.priority,
                        &debug_draw_,
                        had_break_beam_,

soccer/src/soccer/planning/primitives/create_path.cpp

@@ -75,11 +75,13 @@ Trajectory rrt(const LinearMotionInstant& start, const LinearMotionInstant& goal
     return path;
 }
 
-static std::optional<std::tuple<double, double, double>> cached_intermediate_tuple_{};
+static std::unordered_map<uint8_t, std::tuple<double, double, double>> cached_intermediate_tuple_{};
 
 Trajectory intermediate(const LinearMotionInstant& start, const LinearMotionInstant& goal,
                         const MotionConstraints& motion_constraints, RJ::Time start_time,
-                        const rj_geometry::ShapeSet& static_obstacles) {
+                        const rj_geometry::ShapeSet& static_obstacles,
+                        const std::vector<DynamicObstacle>& dynamic_obstacles,
+                        const FieldDimensions* field_dimensions, unsigned int robot_id) {
     // if already on goal, no need to move
     if (start.position.dist_to(goal.position) < 1e-6) {
         return Trajectory{{RobotInstant{Pose(start.position, 0), Twist(), start_time}}};
@@ -97,7 +99,7 @@ Trajectory intermediate(const LinearMotionInstant& start, const LinearMotionInst
     }
 
     // Generate list of intermediate points
-    std::vector<rj_geometry::Point> intermediates = get_intermediates(start, goal);
+    std::vector<rj_geometry::Point> intermediates = get_intermediates(start, goal, robot_id);
 
     for (int i = 0; i < intermediate::PARAM_num_intermediates; i++) {
         rj_geometry::Point final_inter = intermediates[i];
@@ -108,25 +110,38 @@ Trajectory intermediate(const LinearMotionInstant& start, const LinearMotionInst
              t += intermediate::PARAM_step_size) {
             rj_geometry::Point intermediate =
                 (final_inter - start.position).normalized(t) + start.position;
+
+            auto offset = intermediate - field_dimensions->center_point();
+
+            // Ignore out-of-bounds intermediate points
+            // The offset 0.2m is chosen because the sim prevents you from moving
+            // more than 0.2m away from the border lines
+            if (abs(offset.x()) > field_dimensions->width() / 2 + 0.2 ||
+                abs(offset.y()) > field_dimensions->length() / 2 + 0.2) {
+                continue;
+            }
+
             Trajectory trajectory =
                 CreatePath::simple(start, goal, motion_constraints, start_time, {intermediate});
 
             // If the trajectory does not hit an obstacle, it is valid
             if ((!trajectory_hits_static(trajectory, static_obstacles, start_time, nullptr))) {
                 auto angle = (final_inter - start.position).angle();
-                cached_intermediate_tuple_ = {abs(angle), signbit(angle) ? -1 : 1,
-                                              (final_inter - start.position).mag()};
+                cached_intermediate_tuple_[robot_id] = {
+                    abs(angle), (final_inter - start.position).mag(), signbit(angle) ? -1 : 1};
                 return trajectory;
             }
         }
     }
 
-    // If all else fails, return the straight-line trajectory
-    return straight_trajectory;
+    // If all else fails, use rrt to ensure obstacle avoidance
+    return CreatePath::rrt(start, goal, motion_constraints, start_time, static_obstacles,
+                           dynamic_obstacles);
 }
 
 std::vector<rj_geometry::Point> get_intermediates(const LinearMotionInstant& start,
-                                                  const LinearMotionInstant& goal) {
+                                                  const LinearMotionInstant& goal,
+                                                  unsigned int robot_id) {
     std::random_device rd;
     std::mt19937 gen(rd());
     // Create a random distribution for the distance between the start
@@ -152,15 +167,16 @@ std::vector<rj_geometry::Point> get_intermediates(const LinearMotionInstant& sta
         inter_tuples.emplace_back(abs(angle), signbit(angle) ? -1 : 1, scale);
     }
 
-    if (cached_intermediate_tuple_) {
-        inter_tuples.push_back(*cached_intermediate_tuple_);
-    }
 
     // Sort the list of tuples by the magnitude of angle
     // This ensures that we take paths with
     // smaller offsets from the simple path
     sort(inter_tuples.begin(), inter_tuples.end());
 
+    if (cached_intermediate_tuple_.find(robot_id) != cached_intermediate_tuple_.end()) {
+        inter_tuples.emplace(inter_tuples.begin(), cached_intermediate_tuple_[robot_id]);
+    }
+
     for (int i = 0; i < intermediate::PARAM_num_intermediates; i++) {
         double angle = std::get<0>(inter_tuples[i]) * std::get<1>(inter_tuples[i]);
         double scale = std::get<2>(inter_tuples[i]);

soccer/src/soccer/planning/primitives/create_path.hpp

@@ -6,6 +6,7 @@
 #include "planning/motion_constraints.hpp"
 #include "planning/primitives/path_smoothing.hpp"
 #include "planning/trajectory.hpp"
+#include "rj_common/field_dimensions.hpp"
 
 namespace planning::CreatePath {
 
@@ -29,8 +30,11 @@ Trajectory simple(
 
 Trajectory intermediate(const LinearMotionInstant& start, const LinearMotionInstant& goal,
                         const MotionConstraints& motion_constraints, RJ::Time start_time,
-                        const rj_geometry::ShapeSet& static_obstacles);
+                        const rj_geometry::ShapeSet& static_obstacles,
+                        const std::vector<DynamicObstacle>& dynamic_obstacles,
+                        const FieldDimensions* field_dimensions, unsigned int robot_id);
 
 std::vector<rj_geometry::Point> get_intermediates(const LinearMotionInstant& start,
-                                                  const LinearMotionInstant& goal);
+                                                  const LinearMotionInstant& goal,
+                                                  unsigned int robot_id);
 }  // namespace planning::CreatePath

soccer/src/soccer/planning/primitives/replanner.cpp

@@ -33,7 +33,8 @@ Trajectory Replanner::partial_replan(const PlanParams& params, const Trajectory&
     Trajectory pre_trajectory = partial_path(previous, params.start.stamp);
     Trajectory post_trajectory = CreatePath::intermediate(
         pre_trajectory.last().linear_motion(), params.goal, params.constraints.mot,
-        pre_trajectory.end_time(), params.static_obstacles);
+        pre_trajectory.end_time(), params.static_obstacles, params.dynamic_obstacles,
+        params.field_dimensions, params.robot_id);
 
     // If we couldn't profile such that velocity at the end of the partial replan period is valid,
     // do a full replan.
@@ -55,9 +56,10 @@ Trajectory Replanner::partial_replan(const PlanParams& params, const Trajectory&
 }
 
 Trajectory Replanner::full_replan(const Replanner::PlanParams& params) {
-    Trajectory path =
-        CreatePath::intermediate(params.start.linear_motion(), params.goal, params.constraints.mot,
-                                 params.start.stamp, params.static_obstacles);
+    Trajectory path = CreatePath::intermediate(params.start.linear_motion(), params.goal,
+                                               params.constraints.mot, params.start.stamp,
+                                               params.static_obstacles, params.dynamic_obstacles,
+                                               params.field_dimensions, params.robot_id);
 
     // if the initial path is empty, the goal must be blocked
     // try to shift the goal_point until it is no longer blocked
@@ -77,7 +79,8 @@ Trajectory Replanner::full_replan(const Replanner::PlanParams& params) {
 
         path = CreatePath::intermediate(params.start.linear_motion(), almost_goal,
                                         params.constraints.mot, params.start.stamp,
-                                        params.static_obstacles);
+                                        params.static_obstacles, params.dynamic_obstacles,
+                                        params.field_dimensions, params.robot_id);
     }
 
     if (!path.empty()) {

soccer/src/soccer/planning/primitives/replanner.hpp

@@ -7,6 +7,7 @@
 #include "planning/primitives/angle_planning.hpp"
 #include "planning/robot_constraints.hpp"
 #include "planning/trajectory.hpp"
+#include "rj_common/field_dimensions.hpp"
 #include "velocity_profiling.hpp"
 
 namespace planning {
@@ -27,8 +28,10 @@ class Replanner {
         LinearMotionInstant goal;
         const rj_geometry::ShapeSet& static_obstacles;
         const std::vector<DynamicObstacle>& dynamic_obstacles;
+        const FieldDimensions* field_dimensions;
         RobotConstraints constraints;
         const AngleFunction& angle_function;
+        unsigned int robot_id;
         std::optional<RJ::Seconds> hold_time = std::nullopt;
     };