Intermediate Pathgen (#2266)

* initial test of tigers system

* clean up and add comment

* added line test

* fix line issue

* made comments and removed from settle

* Fix Code Style On intermediate-pathgen (#2267)

automated style fixes

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

* make pr changes

* add cache inter

* start cacching intermediates

* add params

* use abs angle

* Fix Code Style On intermediate-pathgen (#2269)

automated style fixes

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

---------

Co-authored-by: rishiso <rishisoni.5678@gmail.com>
Co-authored-by: github-actions[bot] <41898282+github-actions[bot]@users.noreply.github.com>
Co-authored-by: sanatd33 <sanatd33@users.noreply.github.com>
Co-authored-by: petergarud <peter.garud04@gmail.com>
Co-authored-by: sid-parikh <sid-parikh@users.noreply.github.com>

soccer/src/soccer/CMakeLists.txt

@@ -79,6 +79,7 @@ set(ROBOCUP_LIB_SRC
     strategy/agent/position/position.cpp
     strategy/agent/position/robot_factory_position.cpp
     strategy/agent/position/goalie.cpp
+    strategy/agent/position/line.cpp
     strategy/agent/position/offense.cpp
     strategy/agent/position/idle.cpp
     strategy/agent/position/defense.cpp

soccer/src/soccer/planning/planner/line_kick_path_planner.cpp

@@ -88,6 +88,8 @@ Trajectory LineKickPathPlanner::final(const PlanRequest& plan_request) {
 
     MotionCommand modified_command{"path_target", target,
                                    FacePoint{plan_request.motion_command.target.position}};
+
+    modified_command.ignore_ball = true;
     modified_request.motion_command = modified_command;
 
     return path_target_.plan(modified_request);

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

@@ -221,9 +221,9 @@ Trajectory SettlePathPlanner::intercept(const PlanRequest& plan_request, RobotIn
 
         // Plan a path from our partial path start location to the intercept
         // test location
-        Trajectory path = CreatePath::rrt(start_instant.linear_motion(), target_robot_intersection,
-                                          plan_request.constraints.mot, start_instant.stamp,
-                                          static_obstacles, dynamic_obstacles);
+        Trajectory path = CreatePath::intermediate(
+            start_instant.linear_motion(), target_robot_intersection, plan_request.constraints.mot,
+            start_instant.stamp, static_obstacles);
 
         // Calculate the
         RJ::Seconds buffer_duration = ball_time - path.duration();
@@ -332,9 +332,9 @@ 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::rrt(start_instant.linear_motion(), target, plan_request.constraints.mot,
-                            start_instant.stamp, static_obstacles, dynamic_obstacles);
+        Trajectory shortcut = CreatePath::intermediate(start_instant.linear_motion(), target,
+                                                       plan_request.constraints.mot,
+                                                       start_instant.stamp, static_obstacles);
 
         if (!shortcut.empty()) {
             plan_angles(&shortcut, start_instant, AngleFns::face_point(face_pos),

soccer/src/soccer/planning/planning_params.cpp

@@ -41,6 +41,19 @@ DEFINE_NS_INT64(kPlanningParamModule, rrt, min_iterations, 50,
 DEFINE_NS_INT64(kPlanningParamModule, rrt, max_iterations, 500,
                 "Maximum number of RRT iterations to run before giving up");
 
+DEFINE_NS_FLOAT64(kPlanningParamModule, intermediate, min_scale, 0.5,
+                  "Minimum length for intermediate point (m)");
+DEFINE_NS_FLOAT64(kPlanningParamModule, intermediate, max_scale, 1.5,
+                  "Maximum length for intermediate point (m)");
+DEFINE_NS_FLOAT64(kPlanningParamModule, intermediate, min_angle, 20,
+                  "Minimum angle for intermediate point (deg)");
+DEFINE_NS_FLOAT64(kPlanningParamModule, intermediate, max_angle, 140,
+                  "Maximum angle for intermediate point (deg)");
+DEFINE_NS_INT64(kPlanningParamModule, intermediate, num_intermediates, 5,
+                "Number of intermediate points used (unitless)");
+DEFINE_NS_FLOAT64(kPlanningParamModule, intermediate, step_size, 0.1,
+                  "Step size for testing intermediates (m)");
+
 DEFINE_NS_FLOAT64(
     kPlanningParamModule, escape, step_size, 0.1,
     "Step size for the RRT used to find an unblocked point in find_non_blocked_goal()");

soccer/src/soccer/planning/planning_params.hpp

@@ -25,6 +25,13 @@ DECLARE_NS_FLOAT64(kPlanningParamModule, rrt, waypoint_bias);
 DECLARE_NS_INT64(kPlanningParamModule, rrt, min_iterations);
 DECLARE_NS_INT64(kPlanningParamModule, rrt, max_iterations);
 
+DECLARE_NS_FLOAT64(kPlanningParamModule, intermediate, min_scale);
+DECLARE_NS_FLOAT64(kPlanningParamModule, intermediate, max_scale);
+DECLARE_NS_FLOAT64(kPlanningParamModule, intermediate, min_angle);
+DECLARE_NS_FLOAT64(kPlanningParamModule, intermediate, max_angle);
+DECLARE_NS_INT64(kPlanningParamModule, intermediate, num_intermediates);
+DECLARE_NS_FLOAT64(kPlanningParamModule, intermediate, step_size);
+
 DECLARE_NS_FLOAT64(kPlanningParamModule, escape, step_size);
 DECLARE_NS_FLOAT64(kPlanningParamModule, escape, goal_change_threshold);
 

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

@@ -75,4 +75,105 @@ Trajectory rrt(const LinearMotionInstant& start, const LinearMotionInstant& goal
     return path;
 }
 
+static std::optional<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) {
+    // 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}}};
+    }
+
+    // maybe straight line works
+    Trajectory straight_trajectory =
+        CreatePath::simple(start, goal, motion_constraints, start_time);
+
+    // If we are very close to the goal (i.e. there physically can't be a robot
+    // in our way) or the straight trajectory is feasible, we can use it.
+    if (start.position.dist_to(goal.position) < kRobotRadius ||
+        (!trajectory_hits_static(straight_trajectory, static_obstacles, start_time, nullptr))) {
+        return straight_trajectory;
+    }
+
+    // Generate list of intermediate points
+    std::vector<rj_geometry::Point> intermediates = get_intermediates(start, goal);
+
+    for (int i = 0; i < intermediate::PARAM_num_intermediates; i++) {
+        rj_geometry::Point final_inter = intermediates[i];
+
+        // Step through the path from the robot to the final intermediate point
+        // and test each point on that path as an intermediate point
+        for (double t = intermediate::PARAM_step_size; t < final_inter.dist_to(start.position);
+             t += intermediate::PARAM_step_size) {
+            rj_geometry::Point intermediate =
+                (final_inter - start.position).normalized(t) + start.position;
+            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()};
+                return trajectory;
+            }
+        }
+    }
+
+    // If all else fails, return the straight-line trajectory
+    return straight_trajectory;
+}
+
+std::vector<rj_geometry::Point> get_intermediates(const LinearMotionInstant& start,
+                                                  const LinearMotionInstant& goal) {
+    std::random_device rd;
+    std::mt19937 gen(rd());
+    // Create a random distribution for the distance between the start
+    // and the intermediate points
+    std::uniform_real_distribution<> scale_dist(intermediate::PARAM_min_scale,
+                                                intermediate::PARAM_max_scale);
+
+    double angle_range = intermediate::PARAM_max_angle - intermediate::PARAM_min_angle;
+    // Create a random distribution for the angle between the start
+    // and the intermediate points
+    std::uniform_real_distribution<> angle_dist(-angle_range, angle_range);
+
+    std::vector<rj_geometry::Point> intermediates;
+    std::vector<std::tuple<double, double, double>> inter_tuples;
+
+    for (int i = 0; i < intermediate::PARAM_num_intermediates; i++) {
+        double angle = angle_dist(gen);
+        angle += std::copysign(intermediate::PARAM_min_angle, angle);
+        angle = degrees_to_radians(angle);
+        double scale = scale_dist(gen);
+
+        // Generate random tuples of distances and angles
+        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());
+
+    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]);
+
+        double fin_angle = goal.position.angle_to(start.position) + angle;
+        double fin_length = scale;
+
+        // Convert the distances and angles into a point
+        intermediates.push_back(start.position + rj_geometry::Point{fin_length * cos(fin_angle),
+                                                                    fin_length * sin(fin_angle)});
+    }
+
+    return intermediates;
+}
+
 }  // namespace planning::CreatePath

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

@@ -1,8 +1,11 @@
 #pragma once
 
+#include <optional>
+#include <random>
+
 #include "planning/motion_constraints.hpp"
-#include "planning/trajectory.hpp"
 #include "planning/primitives/path_smoothing.hpp"
+#include "planning/trajectory.hpp"
 
 namespace planning::CreatePath {
 
@@ -24,4 +27,10 @@ Trajectory simple(
     const MotionConstraints& motion_constraints, RJ::Time start_time,
     const std::vector<rj_geometry::Point>& intermediate_points = {});
 
+Trajectory intermediate(const LinearMotionInstant& start, const LinearMotionInstant& goal,
+                        const MotionConstraints& motion_constraints, RJ::Time start_time,
+                        const rj_geometry::ShapeSet& static_obstacles);
+
+std::vector<rj_geometry::Point> get_intermediates(const LinearMotionInstant& start,
+                                                  const LinearMotionInstant& goal);
 }  // namespace planning::CreatePath

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

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

soccer/src/soccer/strategy/agent/position/line.cpp

@@ -0,0 +1,105 @@
+#include "line.hpp"
+
+namespace strategy {
+
+Line::Line(const Position& other) : Position{other} { position_name_ = "Line"; }
+
+Line::Line(int r_id) : Position{r_id, "Line"} {}
+
+std::optional<RobotIntent> Line::derived_get_task(RobotIntent intent) {
+    if (check_is_done()) {
+        forward_ = !forward_;
+    }
+
+    if (vertical_) {
+        if (forward_) {
+            auto motion_command = planning::MotionCommand{
+                "path_target",
+                planning::LinearMotionInstant{
+                    rj_geometry::Point{
+                        field_dimensions_.center_field_loc().x() - (robot_id_ - 3) * 1,
+                        field_dimensions_.center_field_loc().y() - 2.5 + 5 * 0.75,
+                    },
+                    rj_geometry::Point{0.0, 0.0},
+                },
+                planning::FaceTarget(), true};
+
+            intent.motion_command = motion_command;
+        } else {
+            auto motion_command = planning::MotionCommand{
+                "path_target",
+                planning::LinearMotionInstant{
+                    rj_geometry::Point{
+                        field_dimensions_.center_field_loc().x() - (robot_id_ - 3) * 1,
+                        field_dimensions_.center_field_loc().y() - 4.5 + 5 * 0.75,
+                    },
+                    rj_geometry::Point{0.0, 0.0},
+                },
+                planning::FaceTarget(), true};
+
+            intent.motion_command = motion_command;
+        }
+    } else {
+        if (forward_) {
+            if (face_target_) {
+                auto motion_command = planning::MotionCommand{
+                    "path_target",
+                    planning::LinearMotionInstant{
+                        rj_geometry::Point{
+                            field_dimensions_.center_field_loc().x() - 2.5 + 5 * 0.75,
+                            (field_dimensions_.center_field_loc().y() - 1) / 6 * robot_id_ + 1,
+                        },
+                        rj_geometry::Point{0.0, 0.0},
+                    },
+                    planning::FaceTarget(), true};
+
+                intent.motion_command = motion_command;
+            } else {
+                auto motion_command = planning::MotionCommand{
+                    "path_target",
+                    planning::LinearMotionInstant{
+                        rj_geometry::Point{
+                            field_dimensions_.our_defense_area().maxx(),
+                            (field_dimensions_.center_field_loc().y() - 1) / 6 * robot_id_ + 1,
+                        },
+                        rj_geometry::Point{0.0, 0.0},
+                    },
+                    planning::FaceAngle{0}, true};
+
+                intent.motion_command = motion_command;
+            }
+
+        } else {
+            if (face_target_) {
+                auto motion_command = planning::MotionCommand{
+                    "path_target",
+                    planning::LinearMotionInstant{
+                        rj_geometry::Point{
+                            field_dimensions_.our_defense_area().minx(),
+                            (field_dimensions_.center_field_loc().y() - 1) / 6 * robot_id_ + 1,
+                        },
+                        rj_geometry::Point{0.0, 0.0},
+                    },
+                    planning::FaceTarget(), true};
+
+                intent.motion_command = motion_command;
+            } else {
+                auto motion_command = planning::MotionCommand{
+                    "path_target",
+                    planning::LinearMotionInstant{
+                        rj_geometry::Point{
+                            field_dimensions_.our_defense_area().minx(),
+                            (field_dimensions_.center_field_loc().y() - 1) / 6 * robot_id_ + 1,
+                        },
+                        rj_geometry::Point{0.0, 0.0},
+                    },
+                    planning::FaceAngle{0}, true};
+
+                intent.motion_command = motion_command;
+            }
+        }
+    }
+
+    return intent;
+}
+}  // namespace strategy

soccer/src/soccer/strategy/agent/position/line.hpp

@@ -0,0 +1,25 @@
+#pragma once
+
+#include <spdlog/spdlog.h>
+
+#include "position.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+namespace strategy {
+class Line : public Position {
+public:
+    Line(const Position& other);
+    Line(int r_id);
+    ~Line() override = default;
+    Line(const Line& other) = default;
+    Line(Line&& other) = default;
+
+    std::string get_current_state() override { return "Line"; }
+
+private:
+    std::optional<RobotIntent> derived_get_task(RobotIntent intent) override;
+    bool forward_ = true;
+    bool vertical_ = false;
+    bool face_target_ = false;
+};
+}  // namespace strategy

soccer/src/soccer/strategy/agent/position/robot_factory_position.hpp

@@ -20,6 +20,7 @@
 #include "strategy/agent/position/free_kicker.hpp"
 #include "strategy/agent/position/goal_kicker.hpp"
 #include "strategy/agent/position/goalie.hpp"
+#include "strategy/agent/position/line.hpp"
 #include "strategy/agent/position/offense.hpp"
 #include "strategy/agent/position/penalty_non_kicker.hpp"
 #include "strategy/agent/position/penalty_player.hpp"
@@ -147,9 +148,9 @@ class RobotFactoryPosition : public Position {
         //     return;
         // }
         if (dynamic_cast<Pos*>(current_position_.get()) == nullptr) {
-            // SPDLOG_INFO("Robot {}: change {}", robot_id_, current_position_->get_name());
-            //  This line requires Pos to implement the constructor Pos(const
-            //  Position&)
+            // This line requires Pos to implement the constructor Pos(const
+            // Position&)
+            SPDLOG_INFO("Robot {}: change {}", robot_id_, current_position_->get_name());
             current_position_->die();
             current_position_ = std::make_unique<Pos>(*current_position_);
         }