feat(pure_pursuit): add maximum steering angle limit (autowarefoundat…

…ion#1654) * feat(pure_pursuit): add maximum steering angle limit Signed-off-by: Berkay Karaman <[email protected]> * ci(pre-commit): autofix Signed-off-by: Berkay Karaman <[email protected]> Co-authored-by: Berkay Karaman <[email protected]> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
Interplai · Aug 29, 2022 · 4e1ff9b · 4e1ff9b
1 parent 89bc5d2
commit 4e1ff9b
Show file tree

Hide file tree

Showing 2 changed files with 7 additions and 2 deletions.
diff --git a/control/pure_pursuit/include/pure_pursuit/pure_pursuit_lateral_controller.hpp b/control/pure_pursuit/include/pure_pursuit/pure_pursuit_lateral_controller.hpp
@@ -60,6 +60,7 @@ struct Param
 {
   // Global Parameters
   double wheel_base;
+  double max_steering_angle;  // [rad]
 
   // Algorithm Parameters
   double lookahead_distance_ratio;

diff --git a/control/pure_pursuit/src/pure_pursuit/pure_pursuit_lateral_controller.cpp b/control/pure_pursuit/src/pure_pursuit/pure_pursuit_lateral_controller.cpp
@@ -61,6 +61,7 @@ PurePursuitLateralController::PurePursuitLateralController(rclcpp::Node & node)
   // Vehicle Parameters
   const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(*node_).getVehicleInfo();
   param_.wheel_base = vehicle_info.wheel_base_m;
+  param_.max_steering_angle = vehicle_info.max_steer_angle_rad;
 
   // Algorithm Parameters
   param_.lookahead_distance_ratio =
@@ -137,10 +138,13 @@ boost::optional<LateralOutput> PurePursuitLateralController::run()
 AckermannLateralCommand PurePursuitLateralController::generateCtrlCmdMsg(
   const double target_curvature)
 {
+  const double tmp_steering =
+    planning_utils::convertCurvatureToSteeringAngle(param_.wheel_base, target_curvature);
   AckermannLateralCommand cmd;
   cmd.stamp = node_->get_clock()->now();
-  cmd.steering_tire_angle =
-    planning_utils::convertCurvatureToSteeringAngle(param_.wheel_base, target_curvature);
+  cmd.steering_tire_angle = static_cast<float>(
+    std::min(std::max(tmp_steering, -param_.max_steering_angle), param_.max_steering_angle));
+
   // pub_ctrl_cmd_->publish(cmd);
   return cmd;
 }