FSF-6551 | Path Planning working in predictive missions

src/launcher/launch/pacsim-testing.launch.py

@@ -63,6 +63,6 @@ def generate_launch_description():
             # evaluator_launch_description,
             planning_launch_description,
             control_launch_description,
-            mocker_node_launch_description,
+            #mocker_node_launch_description,
         ],
     )

src/planning/CMakeLists.txt

@@ -41,6 +41,7 @@ set(TESTFILES
   test/smoothing_tests.cpp
   test/splines_tests.cpp
   test/test_utils/utils.cpp
+  test/velocity_planning_tests.cpp
 )
 
 set(IMPLEMENTATION_FILES
@@ -53,6 +54,7 @@ set(IMPLEMENTATION_FILES
   src/planning/smoothing.cpp
   src/planning/outliers.cpp
   src/planning/planning.cpp
+  src/planning/velocity_planning.cpp
   src/utils/cone.cpp
   src/utils/files.cpp
 )

src/planning/include/adapter_planning/parameters_factory.hpp

@@ -45,6 +45,11 @@ std::string load_adapter_parameters(PlanningParameters& params) {
       static_cast<double>(adapter_node->declare_parameter("pre_defined_velocity_planning", 0.5));
   params.use_outlier_removal_ = adapter_node->declare_parameter("use_outlier_removal", false);
   params.use_path_smoothing_ = adapter_node->declare_parameter("use_path_smoothing", true);
+  params.minimum_velocity_ = adapter_node->declare_parameter("minimum_velocity", 3.0);
+  params.braking_acceleration_ = adapter_node->declare_parameter("braking_acceleration", -2.0);
+  params.normal_acceleration_ = adapter_node->declare_parameter("normal_acceleration", 6.0);
+  params.use_velocity_planning_ = adapter_node->declare_parameter("use_velocity_planning", true);
+
   std::string adapter_type = adapter_node->declare_parameter("adapter", "vehicle");
   params.map_frame_id_ = adapter_type == "eufs" ? "base_footprint" : "map";
 

src/planning/include/config/planning_config.hpp

@@ -6,6 +6,7 @@
 #include "path_calculation_config.hpp"
 #include "simulation_config.hpp"
 #include "smoothing_config.hpp"
+#include "velocity_config.hpp"
 
 struct PlanningParameters {
   double angle_gain_;
@@ -28,6 +29,10 @@ struct PlanningParameters {
   bool use_outlier_removal_;
   bool use_path_smoothing_;
   double desired_velocity_;
+  double minimum_velocity_;
+  double braking_acceleration_;
+  double normal_acceleration_;
+  bool use_velocity_planning_;
   std::string map_frame_id_;
 };
 
@@ -41,6 +46,7 @@ struct PlanningConfig {
   PathCalculationConfig path_calculation_;
   PathSmoothingConfig smoothing_;
   SimulationConfig simulation_;
+  VelocityPlanningConfig velocity_planning_;
 
   PlanningConfig() = default;
   explicit PlanningConfig(const PlanningParameters &params) {
@@ -68,6 +74,11 @@ struct PlanningConfig {
 
     simulation_.publishing_visualization_msgs_ = params.publishing_visualization_msgs_;
     simulation_.using_simulated_se_ = params.using_simulated_se_;
+
+    velocity_planning_.minimum_velocity_ = params.minimum_velocity_;
+    velocity_planning_.braking_acceleration_ = params.braking_acceleration_;
+    velocity_planning_.normal_acceleration_ = params.normal_acceleration_;
+    velocity_planning_.use_velocity_planning_ = params.use_velocity_planning_;
   }
 };
 

src/planning/include/config/velocity_config.hpp

@@ -0,0 +1,37 @@
+#ifndef SRC_PLANNING_INCLUDE_CONFIG_VELOCITY_CONFIG_HPP_
+#define SRC_PLANNING_INCLUDE_CONFIG_VELOCITY_CONFIG_HPP_
+
+/**
+ * @brief struct for the configuration of the velocity planning algorithm
+ *
+ */
+struct VelocityPlanningConfig {
+  /**
+   * @brief minimum speed the car will consider on the velocity planning lookahead
+   *
+   */
+  double minimum_velocity_ = 3;
+  /**
+   * @brief maximum braking acceleration
+   *
+   */
+  double braking_acceleration_ = -4;
+  /**
+   * @brief the maximum normal acceleration
+   *
+   */
+  double normal_acceleration_ = 7;
+  /**
+   * @brief flag to enable/disable velocity planning
+   *
+   */
+  bool use_velocity_planning_ = true;
+  VelocityPlanningConfig() = default;
+  VelocityPlanningConfig(double minimum_velocity, double braking_acc, double normal_acc, bool use_velocity_planning)
+      : minimum_velocity_(minimum_velocity),
+      braking_acceleration_(braking_acc),
+      normal_acceleration_(normal_acc),
+      use_velocity_planning_(use_velocity_planning) {}
+};
+
+#endif  // SRC_PLANNING_INCLUDE_CONFIG_VELOCITY_CONFIG_HPP_

src/planning/include/planning/path_calculation.hpp

@@ -13,6 +13,7 @@
 #include "common_lib/structures/path_point.hpp"
 #include "config/path_calculation_config.hpp"
 #include "rclcpp/rclcpp.hpp"
+#include "common_lib/structures/pose.hpp" // Add this line to include Pose definition
 
 using K = CGAL::Exact_predicates_inexact_constructions_kernel;
 using DT = CGAL::Delaunay_triangulation_2<K>;
@@ -34,6 +35,10 @@ class PathCalculation {
    */
   PathCalculationConfig config_;
 
+private:
+  bool path_orientation_corrected_ = false; // TODO: Put in Skidpad class
+  std::vector<PathPoint> predefined_path_; // TODO: Put in Skidpad class
+
 public:
   /**
    * @brief Construct a new default PathCalculation object
@@ -62,6 +67,8 @@ class PathCalculation {
    */
   std::vector<PathPoint> process_delaunay_triangulations(
       std::pair<std::vector<Cone>, std::vector<Cone>> refined_cones) const;
+
+  std::vector<PathPoint> skidpad_path(std::vector<Cone>& cone_array, common_lib::structures::Pose pose);
 };
 
 #endif  // SRC_PLANNING_PLANNING_INCLUDE_PLANNING_PATH_CALCULATION_HPP_

src/planning/include/planning/planning.hpp

@@ -22,6 +22,7 @@
 #include "planning/outliers.hpp"
 #include "planning/path_calculation.hpp"
 #include "planning/smoothing.hpp"
+#include "planning/velocity_planning.hpp"
 #include "sensor_msgs/msg/point_cloud2.hpp"
 #include "std_msgs/msg/float64.hpp"
 #include "utils/files.hpp"
@@ -49,6 +50,7 @@ class Planning : public rclcpp::Node {
   Outliers outliers_;
   PathCalculation path_calculation_;
   PathSmoothing path_smoothing_;
+  VelocityPlanning velocity_planning_;
   double desired_velocity_;
   double initial_car_orientation_;
 

src/planning/include/planning/velocity_planning.hpp

@@ -0,0 +1,65 @@
+#ifndef SRC_PLANNING_INCLUDE_PLANNING_VELOCITY_HPP_
+#define SRC_PLANNING_INCLUDE_PLANNING_VELOCITY_HPP_
+
+#include <cmath>
+
+#include "common_lib/structures/path_point.hpp"
+#include "config/velocity_config.hpp"
+
+using PathPoint = common_lib::structures::PathPoint;
+
+/**
+ * @brief class that defines the path smoothing algorithm
+ *
+ */
+class VelocityPlanning {
+private:
+  /**
+   * @brief configuration of the smoothing algorithm
+   *
+   */
+  VelocityPlanningConfig config_;
+
+  /**
+   * @brief function to calculate the radius of the circle that passes through 3 points
+   *
+   * @param point1 first point
+   * @param point2 second point
+   * @param point3 third point
+   * @return radius of the circle
+   */
+  double find_circle_center(PathPoint &point1, PathPoint &point2, PathPoint &point3);
+
+  /**
+   * @brief function to limit the speed of the car according to the curvature of the lookahead path
+   *
+   * @param points path points
+   * @param velocities velocities of the path points
+   * @param brake_acelleration maximum braking acelleration
+   */
+  void speed_limiter(std::vector<PathPoint> &points, std::vector<double> &velocities);
+
+  /**
+   * @brief function to calculate the speed of the car according to the curvature of the path
+   *
+   * @param radiuses radiuses vector of the path points
+   * @param velocities velocities vector of the path points
+   */
+  void point_speed(std::vector<double> &radiuses, std::vector<double> &velocities);
+
+public:
+  /**
+   * @brief Construct a new default Path Smoothing object
+   *
+   */
+  VelocityPlanning() = default;
+  /**
+   * @brief Construct a new Path Smoothing object with a given configuration
+   *
+   */
+  explicit VelocityPlanning(VelocityPlanningConfig config) : config_(config) {}
+
+  void set_velocity(std::vector<PathPoint> &final_path);
+};
+
+#endif  // SRC_PLANNING_INCLUDE_PLANNING_SMOOTHING2_HPP_

src/planning/launch/eufs.launch.py

@@ -115,6 +115,26 @@ def generate_launch_description():
                 description="Whether to use path smoothing or to skip it",
                 default_value="true",
             ),
+            DeclareLaunchArgument(
+                "minimum_velocity",
+                description="Minimum speed for velocity planning",
+                default_value="3.0",
+            ),
+            DeclareLaunchArgument(
+                "braking_acceleration",
+                description="Braking acceleration to consider in velocity planning",
+                default_value="-4.0",
+            ),
+            DeclareLaunchArgument(
+                "normal_acceleration",
+                description="Normal acceleration to consider in velocity planning",
+                default_value="7.0",
+            ),
+            DeclareLaunchArgument(
+                "use_velocity_planning",
+                description="Whether to use velocity planning or to skip it",
+                default_value="true",
+            ),
             Node(
                 package="planning",
                 executable="planning",
@@ -185,6 +205,10 @@ def generate_launch_description():
                     },
                     {"use_outlier_removal": LaunchConfiguration("use_outlier_removal")},
                     {"use_path_smoothing": LaunchConfiguration("use_path_smoothing")},
+                    {"minimum_velocity": LaunchConfiguration("minimum_velocity")},
+                    {"braking_acceleration": LaunchConfiguration("braking_acceleration")},
+                    {"normal_acceleration": LaunchConfiguration("normal_acceleration")},
+                    {"use_velocity_planning": LaunchConfiguration("use_velocity_planning")},
                 ],
                 arguments=["--ros-args", "--log-level", "planning:=info"],
             ),

src/planning/launch/fsds.launch.py

@@ -115,6 +115,26 @@ def generate_launch_description():
                 description="Whether to use path smoothing or to skip it",
                 default_value="true",
             ),
+            DeclareLaunchArgument(
+                "minimum_velocity",
+                description="Minimum speed for velocity planning",
+                default_value="3.0",
+            ),
+            DeclareLaunchArgument(
+                "braking_acceleration",
+                description="Braking acceleration to consider in velocity planning",
+                default_value="-4.0",
+            ),
+            DeclareLaunchArgument(
+                "normal_acceleration",
+                description="Normal acceleration to consider in velocity planning",
+                default_value="7.0",
+            ),
+            DeclareLaunchArgument(
+                "use_velocity_planning",
+                description="Whether to use velocity planning or to skip it",
+                default_value="true",
+            ),
             Node(
                 package="planning",
                 executable="planning",
@@ -185,6 +205,10 @@ def generate_launch_description():
                     }
                     {"use_outlier_removal": LaunchConfiguration("use_outlier_removal")},
                     {"use_path_smoothing": LaunchConfiguration("use_path_smoothing")},
+                    {"minimum_velocity": LaunchConfiguration("minimum_velocity")},
+                    {"braking_acceleration": LaunchConfiguration("braking_acceleration")},
+                    {"normal_acceleration": LaunchConfiguration("normal_acceleration")},
+                    {"use_velocity_planning": LaunchConfiguration("use_velocity_planning")},
                 ],
                 arguments=["--ros-args", "--log-level", "planning:=info"],
             ),

src/planning/launch/pacsim.launch.py

@@ -115,6 +115,26 @@ def generate_launch_description():
                 description="Whether to use path smoothing or to skip it",
                 default_value="true",
             ),
+            DeclareLaunchArgument(
+                "minimum_velocity",
+                description="Minimum speed for velocity planning",
+                default_value="2.0",
+            ),
+            DeclareLaunchArgument(
+                "braking_acceleration",
+                description="Braking acceleration to consider in velocity planning",
+                default_value="-1.0",
+            ),
+            DeclareLaunchArgument(
+                "normal_acceleration",
+                description="Normal acceleration to consider in velocity planning",
+                default_value="4.0",
+            ),
+            DeclareLaunchArgument(
+                "use_velocity_planning",
+                description="Whether to use velocity planning or to skip it",
+                default_value="true",
+            ),
             Node(
                 package="planning",
                 executable="planning",
@@ -185,6 +205,10 @@ def generate_launch_description():
                     },
                     {"use_outlier_removal": LaunchConfiguration("use_outlier_removal")},
                     {"use_path_smoothing": LaunchConfiguration("use_path_smoothing")},
+                    {"minimum_velocity": LaunchConfiguration("minimum_velocity")},
+                    {"braking_acceleration": LaunchConfiguration("braking_acceleration")},
+                    {"normal_acceleration": LaunchConfiguration("normal_acceleration")},
+                    {"use_velocity_planning": LaunchConfiguration("use_velocity_planning")},
                 ],
                 arguments=["--ros-args", "--log-level", "planning:=info"],
             ),

src/planning/launch/vehicle.launch.py

@@ -115,6 +115,26 @@ def generate_launch_description():
                 description="Whether to use path smoothing or to skip it",
                 default_value="true",
             ),
+            DeclareLaunchArgument(
+                "minimum_velocity",
+                description="Minimum speed for velocity planning",
+                default_value="3.0",
+            ),
+            DeclareLaunchArgument(
+                "braking_acceleration",
+                description="Braking acceleration to consider in velocity planning",
+                default_value="-4.0",
+            ),
+            DeclareLaunchArgument(
+                "normal_acceleration",
+                description="Normal acceleration to consider in velocity planning",
+                default_value="7.0",
+            ),
+            DeclareLaunchArgument(
+                "use_velocity_planning",
+                description="Whether to use velocity planning or to skip it",
+                default_value="true",
+            ),
             Node(
                 package="planning",
                 executable="planning",
@@ -185,6 +205,10 @@ def generate_launch_description():
                     },
                     {"use_outlier_removal": LaunchConfiguration("use_outlier_removal")},
                     {"use_path_smoothing": LaunchConfiguration("use_path_smoothing")},
+                    {"minimum_velocity": LaunchConfiguration("minimum_velocity")},
+                    {"braking_acceleration": LaunchConfiguration("braking_acceleration")},
+                    {"normal_acceleration": LaunchConfiguration("normal_acceleration")},
+                    {"use_velocity_planning": LaunchConfiguration("use_velocity_planning")},
                 ],
                 arguments=["--ros-args", "--log-level", "planning:=info"],
             ),