[RPP] Jerk limited trajectory generation

nav2_regulated_pure_pursuit_controller/CMakeLists.txt

@@ -11,6 +11,7 @@ find_package(geometry_msgs REQUIRED)
 find_package(nav_msgs REQUIRED)
 find_package(pluginlib REQUIRED)
 find_package(tf2 REQUIRED)
+find_package(ruckig REQUIRED)
 
 nav2_package()
 set(CMAKE_CXX_STANDARD 17)
@@ -28,13 +29,14 @@ set(dependencies
   nav2_util
   nav2_core
   tf2
+  ruckig
 )
 
 set(library_name nav2_regulated_pure_pursuit_controller)
 
 add_library(${library_name} SHARED
         src/regulated_pure_pursuit_controller.cpp)
-
+target_link_libraries(${library_name} ruckig::ruckig)
 ament_target_dependencies(${library_name}
   ${dependencies}
 )

nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp

@@ -30,6 +30,7 @@
 #include "nav2_util/odometry_utils.hpp"
 #include "nav2_util/geometry_utils.hpp"
 #include "geometry_msgs/msg/pose2_d.hpp"
+#include "ruckig/ruckig.hpp"
 
 namespace nav2_regulated_pure_pursuit_controller
 {
@@ -157,9 +158,11 @@ class RegulatedPurePursuitController : public nav2_core::Controller
   /**
    * @brief Whether robot should rotate to final goal orientation
    * @param carrot_pose current lookahead point
+   * @param lookahead_dist current lookahead distance
    * @return Whether should rotate to goal heading
    */
-  bool shouldRotateToGoalHeading(const geometry_msgs::msg::PoseStamped & carrot_pose);
+  bool shouldRotateToGoalHeading(
+    const geometry_msgs::msg::PoseStamped & carrot_pose, const double & lookahead_dist);
 
   /**
    * @brief Create a smooth and kinematically smoothed rotation command
@@ -223,9 +226,11 @@ class RegulatedPurePursuitController : public nav2_core::Controller
    * @brief Get lookahead point
    * @param lookahead_dist Optimal lookahead distance
    * @param path Current global path
+   * @param remaining_path_length path length from lookahead point to the end of pruned path
    * @return Lookahead point
    */
-  geometry_msgs::msg::PoseStamped getLookAheadPoint(const double &, const nav_msgs::msg::Path &);
+  geometry_msgs::msg::PoseStamped getLookAheadPoint(
+    const double &, const nav_msgs::msg::Path &, double & remaining_path_length);
 
   /**
    * @brief checks for the cusp position
@@ -234,6 +239,13 @@ class RegulatedPurePursuitController : public nav2_core::Controller
    */
   double findDirectionChange(const geometry_msgs::msg::PoseStamped & pose);
 
+  /**
+   * @brief Normalizes angle in range [-pi, pi]
+   * @param angle Angle to normalize
+   * @return Normalized angle
+   */
+  double angleNormalize(double angle);
+
   /**
    * @brief Callback executed when a parameter change is detected
    * @param event ParameterEvent message
@@ -250,6 +262,8 @@ class RegulatedPurePursuitController : public nav2_core::Controller
   rclcpp::Clock::SharedPtr clock_;
 
   double desired_linear_vel_, base_desired_linear_vel_;
+  double max_linear_accel_;
+  double max_linear_decel_;
   double lookahead_dist_;
   double rotate_to_heading_angular_vel_;
   double max_lookahead_dist_;
@@ -272,6 +286,12 @@ class RegulatedPurePursuitController : public nav2_core::Controller
   double rotate_to_heading_min_angle_;
   double goal_dist_tol_;
   bool allow_reversing_;
+  double robot_angle_;
+  bool rotating_;
+  double kp_angle_;
+  double max_linear_jerk_;
+  double max_angular_jerk_;
+  rclcpp::Time system_time_;
 
   nav_msgs::msg::Path global_plan_;
   std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::Path>> global_path_pub_;
@@ -281,6 +301,16 @@ class RegulatedPurePursuitController : public nav2_core::Controller
   std::unique_ptr<nav2_costmap_2d::FootprintCollisionChecker<nav2_costmap_2d::Costmap2D *>>
   collision_checker_;
 
+  ruckig::Ruckig<1> * distance_profile_;
+  ruckig::InputParameter<1> distance_profile_input_;
+  ruckig::OutputParameter<1> distance_profile_output_;
+  ruckig::Result distance_profile_result_;
+
+  ruckig::Ruckig<1> * angle_profile_;
+  ruckig::InputParameter<1> angle_profile_input_;
+  ruckig::OutputParameter<1> angle_profile_output_;
+  ruckig::Result angle_profile_result_;
+
   // Dynamic parameters handler
   std::mutex mutex_;
   rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_;

nav2_regulated_pure_pursuit_controller/package.xml

@@ -19,6 +19,7 @@
   <depend>nav2_msgs</depend>
   <depend>pluginlib</depend>
   <depend>tf2</depend>
+  <depend>ruckig</depend>
 
   <test_depend>ament_cmake_gtest</test_depend>
   <test_depend>ament_lint_common</test_depend>