Update readme

update svg readme updated with test params change sample rate calculate accurate dt test Signed-off-by: Berkay <[email protected]> fix trajectory size update readme change map loader params Signed-off-by: Berkay <[email protected]> clear unnecessary comment Signed-off-by: Berkay <[email protected]> change the min and max index Signed-off-by: Berkay <[email protected]> ci(pre-commit): autofix removed unnecessary params and comments Signed-off-by: Berkay <[email protected]> ci(pre-commit): autofix all velocities in lookup distance is changed Signed-off-by: Berkay <[email protected]> ci(pre-commit): autofix works ci(pre-commit): autofix changed calculations with const lookupdistance ci(pre-commit): autofix not work peak points written with constant distances added param ci(pre-commit): autofix update ci(pre-commit): autofix update steering angle calculation method ci(pre-commit): autofix changed curvature calculation of steeringAngleLimit func Signed-off-by: Berkay Karaman <[email protected]> changed default parameter values Signed-off-by: Berkay <[email protected]> update readme Signed-off-by: Berkay <[email protected]> update engage velocity parameter Signed-off-by: Berkay <[email protected]>
autowarefoundation · Sep 7, 2022 · 7d73b96 · 7d73b96
1 parent e69d3a6
commit 7d73b96
Show file tree

Hide file tree

Showing 12 changed files with 4,969 additions and 120 deletions.
diff --git a/launch/tier4_control_launch/config/trajectory_follower/lateral_controller.param.yaml b/launch/tier4_control_launch/config/trajectory_follower/lateral_controller.param.yaml
@@ -45,9 +45,8 @@
     vehicle_model_type: "kinematics" # vehicle model type for mpc prediction. option is kinematics, kinematics_no_delay, and dynamics
     input_delay: 0.24                # steering input delay time for delay compensation
     vehicle_model_steer_tau: 0.3     # steering dynamics time constant (1d approximation) [s]
-    steer_lim_deg: 40.0              # steering angle limit [deg]
-    steer_rate_lim_dps: 5.0        # steering angle rate limit [deg/s]
-    acceleration_limit: 0.9          # acceleration limit for trajectory velocity modification [m/ss]
+    steer_rate_lim_dps: 40.0         # steering angle rate limit [deg/s]
+    acceleration_limit: 2.0          # acceleration limit for trajectory velocity modification [m/ss]
     velocity_time_constant: 0.3      # velocity dynamics time constant  for trajectory velocity modification [s]
 
     # -- lowpass filter for noise reduction --

diff --git a/launch/tier4_control_launch/config/trajectory_follower/longitudinal_controller.param.yaml b/launch/tier4_control_launch/config/trajectory_follower/longitudinal_controller.param.yaml
@@ -60,12 +60,12 @@
     emergency_jerk: -3.0
 
     # acceleration limit
-    max_acc: 0.9
-    min_acc: -0.9
+    max_acc: 3.0
+    min_acc: -5.0
 
     # jerk limit
-    max_jerk: 0.6
-    min_jerk: -0.6
+    max_jerk: 2.0
+    min_jerk: -5.0
 
     # pitch
     use_trajectory_for_pitch_calculation: false

diff --git a/launch/tier4_planning_launch/config/scenario_planning/common/common.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/common/common.param.yaml
@@ -3,13 +3,13 @@
     # constraints param for normal driving
     normal:
       min_acc: -0.5         # min deceleration [m/ss]
-      max_acc: 0.5          # max acceleration [m/ss]
-      min_jerk: -0.4        # min jerk [m/sss]
-      max_jerk: 0.4         # max jerk [m/sss]
+      max_acc: 1.0          # max acceleration [m/ss]
+      min_jerk: -0.5        # min jerk [m/sss]
+      max_jerk: 1.0         # max jerk [m/sss]
 
     # constraints to be observed
     limit:
-      min_acc: -0.9         # min deceleration limit [m/ss]
-      max_acc: 0.9          # max acceleration limit [m/ss]
-      min_jerk: -0.6        # min jerk limit [m/sss]
-      max_jerk: 0.6         # max jerk limit [m/sss]
+      min_acc: -2.5         # min deceleration limit [m/ss]
+      max_acc: 1.0          # max acceleration limit [m/ss]
+      min_jerk: -1.5        # min jerk limit [m/sss]
+      max_jerk: 1.5         # max jerk limit [m/sss]
diff --git a/...fig/scenario_planning/common/motion_velocity_smoother/motion_velocity_smoother.param.yaml b/...fig/scenario_planning/common/motion_velocity_smoother/motion_velocity_smoother.param.yaml
@@ -8,8 +8,8 @@
     margin_to_insert_external_velocity_limit: 0.3   #  margin distance to insert external velocity limit [m]
 
     # curve parameters
-    max_lateral_accel: 0.3             # max lateral acceleration limit [m/ss]
-    min_curve_velocity: 0.5            # min velocity at lateral acceleration limit [m/ss]
+    max_lateral_accel: 0.8             # max lateral acceleration limit [m/ss]
+    min_curve_velocity: 0.5            # min velocity at lateral acceleration limit and steering angle rate limit [m/s]
     decel_distance_before_curve: 3.5   # slow speed distance before a curve for lateral acceleration limit
     decel_distance_after_curve: 2.0    # slow speed distance after a curve for lateral acceleration limit
 
@@ -47,8 +47,11 @@
     post_sparse_resample_dt: 0.1             # resample time interval for sparse sampling [s]
     post_sparse_min_interval_distance: 1.0   # minimum points-interval length for sparse sampling [m]
 
-    # vehicle model parameters
-    max_steering_angle_rate: 0.0872665             # steering angle rate [rad/s]
+    # steering angle rate limit parameters
+    max_steering_angle_rate: 40.0            # maximum steering angle rate [degree/s]
+    resample_ds: 0.1                         # distance between trajectory points [m]
+    curvature_threshold: 0.02                # if curvature > curvature_threshold, steeringRateLimit is triggered [1/m]
+    curvature_calculation_distance: 1.0      # distance of points while curvature is calculating [m]
 
     # system
     over_stop_velocity_warn_thr: 1.389       # used to check if the optimization exceeds the input velocity on the stop point
diff --git a/map/map_loader/config/lanelet2_map_loader.param.yaml b/map/map_loader/config/lanelet2_map_loader.param.yaml
@@ -1,7 +1,7 @@
 /**:
   ros__parameters:
-    lanelet2_map_projector_type: UTM   # Options: MGRS, UTM
-    latitude: 40.81187906        # Latitude of map_origin, using in UTM
-    longitude: 29.35810110       # Longitude of map_origin, using in UTM
+    lanelet2_map_projector_type: MGRS   # Options: MGRS, UTM
+    latitude: 40.816617984672746        # Latitude of map_origin, using in UTM
+    longitude: 29.360491808334285       # Longitude of map_origin, using in UTM
 
     center_line_resolution: 5.0         # [m]
diff --git a/planning/motion_velocity_smoother/README.md b/planning/motion_velocity_smoother/README.md
@@ -34,6 +34,10 @@ It calculates the velocity limit from the curvature of the reference trajectory
 The velocity limit is set as not to fall under `min_curve_velocity`.
 
 Note: velocity limit that requests larger than `nominal.jerk` is not applied. In other words, even if a sharp curve is planned just in front of the ego, no deceleration is performed.
+#### Apply steering rate limit
+
+It calculates the desired steering angles of trajectory points. and it applies the steering rate limit. If the (`steering_angle_rate` > `max_steering_angle_rate`), it decreases the velocity of the trajectory point to acceptable velocity.
+
 
 #### Resample trajectory
 
@@ -108,6 +112,7 @@ After the optimization, a resampling called `post resampling` is performed befor
 | `~/debug/trajectory_raw`                           | `autoware_auto_planning_msgs/Trajectory` | Extracted trajectory (for debug)                                                                          |
 | `~/debug/trajectory_external_velocity_limited`     | `autoware_auto_planning_msgs/Trajectory` | External velocity limited trajectory (for debug)                                                          |
 | `~/debug/trajectory_lateral_acc_filtered`          | `autoware_auto_planning_msgs/Trajectory` | Lateral acceleration limit filtered trajectory (for debug)                                                |
+| `~/debug/trajectory_steering_rate_limited`         | `autoware_auto_planning_msgs/Trajectory` | Steering angle rate limit filtered trajectory (for debug)                                                 |
 | `~/debug/trajectory_time_resampled`                | `autoware_auto_planning_msgs/Trajectory` | Time resampled trajectory (for debug)                                                                     |
 | `~/distance_to_stopline`                           | `std_msgs/Float32`                       | Distance to stop line from current ego pose (max 50 m) (for debug)                                        |
 | `~/stop_speed_exceeded`                            | `std_msgs/Bool`                          | It publishes `true` if planned velocity on the point which the maximum velocity is zero is over threshold |
@@ -189,6 +194,16 @@ After the optimization, a resampling called `post resampling` is performed befor
 | `post_sparse_dt`                    | `double` | resample time interval for sparse sampling [s]         | 0.1           |
 | `post_sparse_min_interval_distance` | `double` | minimum points-interval length for sparse sampling [m] | 1.0           |
 
+### Limit steering angle rate parameters
+
+| Name                                 | Type     | Description                                                              | Default value |
+|:-------------------------------------| :------- |:-------------------------------------------------------------------------|:--------------|
+| `max_steering_angle_rate`            | `double` | Maximum steering angle rate [degree/s]                                   | 40.0          |
+| `resample_ds`                        | `double` | Distance between trajectory points [m]                                   | 0.1           |
+| `curvature_threshold`                | `double` | If curvature > curvature_threshold, steeringRateLimit is triggered [1/m] | 0.02          |
+| `curvature_calculation_distance`     | `double` | Distance of points while curvature is calculating [m]                    | 1.0           |
+
+
 ### Weights for optimization
 
 #### JerkFiltered

diff --git a/...tion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp b/...tion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp
@@ -86,10 +86,6 @@ class MotionVelocitySmootherNode : public rclcpp::Node
   double external_velocity_limit_dist_{0.0};       // distance to set external velocity limit
   double wheelbase_;                               // wheelbase
 
-  // maximum velocity with deceleration for external velocity limit
-  double max_velocity_with_deceleration_;
-  double external_velocity_limit_dist_{0.0};  // distance to set external velocity limit
-
   TrajectoryPoints prev_output_;  // previously published trajectory
 
   // previous trajectory point closest to ego vehicle

diff --git a/...ning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/smoother_base.hpp b/...ning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/smoother_base.hpp
@@ -20,9 +20,9 @@
 #include "motion_velocity_smoother/trajectory_utils.hpp"
 #include "rclcpp/rclcpp.hpp"
 #include "tier4_autoware_utils/geometry/geometry.hpp"
+#include "vehicle_info_util/vehicle_info_util.hpp"
 
 #include "autoware_auto_planning_msgs/msg/trajectory_point.hpp"
-#include "vehicle_info_util/vehicle_info_util.hpp"
 
 #include "boost/optional.hpp"
 
@@ -49,9 +49,14 @@ class SmootherBase
     double min_curve_velocity;           // min velocity at curve [m/s]
     double decel_distance_before_curve;  // distance before slow down for lateral acc at a curve
     double decel_distance_after_curve;   // distance after slow down for lateral acc at a curve
-    double max_steering_angle_rate;       // max steering angle rate [degree/s]
+    double max_steering_angle_rate;      // max steering angle rate [degree/s]
     double wheel_base;                   // wheel base [m]
-      resampling::ResampleParam resample_param;
+    double sample_ds;                    // distance between trajectory points [m]
+    double curvature_threshold;  // look-up distance of Trajectory point for calculation of steering
+                                 // angle limit [m]
+    double curvature_calculation_distance;  // threshold steering degree limit to trigger
+                                            // steeringRateLimit [degree]
+    resampling::ResampleParam resample_param;
   };
 
   explicit SmootherBase(rclcpp::Node & node);
@@ -69,9 +74,7 @@ class SmootherBase
     [[maybe_unused]] const double a0 = 0.0,
     [[maybe_unused]] const bool enable_smooth_limit = false) const;
 
-  boost::optional<TrajectoryPoints> applySteeringRateLimit(
-    const TrajectoryPoints & input) const;
-
+  boost::optional<TrajectoryPoints> applySteeringRateLimit(const TrajectoryPoints & input) const;
 
   double getMaxAccel() const;
   double getMinDecel() const;
@@ -83,10 +86,6 @@ class SmootherBase
 
 protected:
   BaseParam base_param_;
-  const double points_interval = 0.1; // constant interval distance of trajectory
-                                      // for lateral acceleration calculation and
-                                      // limit steering rate. [m]
-
 };
 }  // namespace motion_velocity_smoother
 

diff --git a/planning/motion_velocity_smoother/media/motion_velocity_smoother_flow.drawio.svg b/planning/motion_velocity_smoother/media/motion_velocity_smoother_flow.drawio.svg
diff --git a/planning/motion_velocity_smoother/package.xml b/planning/motion_velocity_smoother/package.xml
@@ -22,6 +22,7 @@
   <depend>geometry_msgs</depend>
   <depend>interpolation</depend>
   <depend>libboost-dev</depend>
+  <depend>motion_common</depend>
   <depend>motion_utils</depend>
   <depend>nav_msgs</depend>
   <depend>osqp_interface</depend>

diff --git a/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp b/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp
@@ -18,6 +18,7 @@
 #include "motion_velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp"
 #include "motion_velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp"
 #include "tier4_autoware_utils/ros/update_param.hpp"
+
 #include <vehicle_info_util/vehicle_info_util.hpp>
 
 #include <algorithm>
@@ -75,12 +76,12 @@ MotionVelocitySmootherNode::MotionVelocitySmootherNode(const rclcpp::NodeOptions
     default:
       throw std::domain_error("[MotionVelocitySmootherNode] invalid algorithm");
   }
-  //Initialize the wheelbase
+  // Initialize the wheelbase
   auto p = smoother_->getBaseParam();
   p.wheel_base = wheelbase_;
   smoother_->setParam(p);
 
-    // publishers, subscribers
+  // publishers, subscribers
   pub_trajectory_ = create_publisher<Trajectory>("~/output/trajectory", 1);
   pub_velocity_limit_ = create_publisher<VelocityLimit>(
     "~/output/current_velocity_limit_mps", rclcpp::QoS{1}.transient_local());
@@ -179,7 +180,10 @@ rcl_interfaces::msg::SetParametersResult MotionVelocitySmootherNode::onParameter
     update_param("min_interval_distance", p.resample_param.dense_min_interval_distance);
     update_param("sparse_resample_dt", p.resample_param.sparse_resample_dt);
     update_param("sparse_min_interval_distance", p.resample_param.sparse_min_interval_distance);
+    update_param("resample_ds", p.sample_ds);
+    update_param("curvature_threshold", p.curvature_threshold);
     update_param("max_steering_angle_rate", p.max_steering_angle_rate);
+    update_param("curvature_calculation_distance", p.curvature_calculation_distance);
     smoother_->setParam(p);
   }
 
@@ -496,24 +500,23 @@ bool MotionVelocitySmootherNode::smoothVelocity(
   const auto traj_lateral_acc_filtered =
     smoother_->applyLateralAccelerationFilter(input, initial_motion.vel, initial_motion.acc, true);
   if (!traj_lateral_acc_filtered) {
-    RCLCPP_WARN(get_logger(), "Fail to do traj_lateral_acc_filtered");
+    RCLCPP_ERROR(get_logger(), "Fail to do traj_lateral_acc_filtered");
 
     return false;
   }
 
   // Steering angle rate limit
-  const auto traj_steering_rate_limited = smoother_->applySteeringRateLimit(*traj_lateral_acc_filtered);
+  const auto traj_steering_rate_limited =
+    smoother_->applySteeringRateLimit(*traj_lateral_acc_filtered);
   if (!traj_steering_rate_limited) {
-    RCLCPP_WARN(get_logger(), "Fail to do traj_steering_rate_limited");
+    RCLCPP_ERROR(get_logger(), "Fail to do traj_steering_rate_limited");
 
     return false;
   }
-  RCLCPP_WARN(get_logger(), "AAAAAAAAA CREATED");
-
 
   // Resample trajectory with ego-velocity based interval distance
   auto traj_resampled = smoother_->resampleTrajectory(
-    *traj_lateral_acc_filtered, current_odometry_ptr_->twist.twist.linear.x,
+    *traj_steering_rate_limited, current_odometry_ptr_->twist.twist.linear.x,
     current_pose_ptr_->pose, node_param_.ego_nearest_dist_threshold,
     node_param_.ego_nearest_yaw_threshold);
   if (!traj_resampled) {
@@ -573,9 +576,9 @@ bool MotionVelocitySmootherNode::smoothVelocity(
       pub_trajectory_resampled_->publish(toTrajectoryMsg(tmp));
     }
     {
-      auto tmp = *traj_resampled;
+      auto tmp = *traj_steering_rate_limited;
       if (is_reverse_) flipVelocity(tmp);
-      pub_trajectory_resampled_->publish(toTrajectoryMsg(tmp));
+      pub_trajectory_steering_rate_limited_->publish(toTrajectoryMsg(tmp));
     }
 
     if (!debug_trajectories.empty()) {