Merge branch 'main' into test/add_unit_test_to_ndt_scan_matcher

Author: SakodaShintaro

.github/workflows/build-and-test-differential.yaml

@@ -74,3 +74,40 @@ jobs:
 
       - name: Show disk space after the tasks
         run: df -h
+
+  clang-tidy-differential:
+    runs-on: ubuntu-latest
+    container: ghcr.io/autowarefoundation/autoware-openadk:latest-prebuilt-cuda
+    steps:
+      - name: Check out repository
+        uses: actions/checkout@v3
+        with:
+          fetch-depth: 0
+
+      - name: Show disk space before the tasks
+        run: df -h
+
+      - name: Remove exec_depend
+        uses: autowarefoundation/autoware-github-actions/remove-exec-depend@v1
+
+      - name: Get modified packages
+        id: get-modified-packages
+        uses: autowarefoundation/autoware-github-actions/get-modified-packages@v1
+
+      - name: Get modified files
+        id: get-modified-files
+        uses: tj-actions/changed-files@v35
+        with:
+          files: |
+            **/*.cpp
+            **/*.hpp
+
+      - name: Run clang-tidy
+        if: ${{ steps.get-modified-files.outputs.all_changed_files != '' }}
+        uses: autowarefoundation/autoware-github-actions/clang-tidy@v1
+        with:
+          rosdistro: humble
+          target-packages: ${{ steps.get-modified-packages.outputs.modified-packages }}
+          target-files: ${{ steps.get-modified-files.outputs.all_changed_files }}
+          clang-tidy-config-url: https://raw.githubusercontent.com/autowarefoundation/autoware/main/.clang-tidy
+          build-depends-repos: build_depends.repos

common/tier4_autoware_utils/include/tier4_autoware_utils/ros/published_time_publisher.hpp

@@ -20,9 +20,11 @@
 #include <autoware_internal_msgs/msg/published_time.hpp>
 #include <std_msgs/msg/header.hpp>
 
+#include <cstring>
 #include <functional>
 #include <map>
 #include <string>
+#include <utility>
 
 namespace tier4_autoware_utils
 {
@@ -31,9 +33,9 @@ class PublishedTimePublisher
 {
 public:
   explicit PublishedTimePublisher(
-    rclcpp::Node * node, const std::string & publisher_topic_suffix = "/debug/published_time",
+    rclcpp::Node * node, std::string publisher_topic_suffix = "/debug/published_time",
     const rclcpp::QoS & qos = rclcpp::QoS(1))
-  : node_(node), publisher_topic_suffix_(publisher_topic_suffix), qos_(qos)
+  : node_(node), publisher_topic_suffix_(std::move(publisher_topic_suffix)), qos_(qos)
   {
   }
 
@@ -45,16 +47,16 @@ class PublishedTimePublisher
     // if the publisher is not in the map, create a new publisher for published time
     ensure_publisher_exists(gid_key, publisher->get_topic_name());
 
-    const auto & pub_published_time_ = publishers_[gid_key];
+    const auto & pub_published_time = publishers_[gid_key];
 
     // Check if there are any subscribers, otherwise don't do anything
-    if (pub_published_time_->get_subscription_count() > 0) {
+    if (pub_published_time->get_subscription_count() > 0) {
       PublishedTime published_time;
 
       published_time.header.stamp = stamp;
       published_time.published_stamp = rclcpp::Clock().now();
 
-      pub_published_time_->publish(published_time);
+      pub_published_time->publish(published_time);
     }
   }
 
@@ -66,16 +68,16 @@ class PublishedTimePublisher
     // if the publisher is not in the map, create a new publisher for published time
     ensure_publisher_exists(gid_key, publisher->get_topic_name());
 
-    const auto & pub_published_time_ = publishers_[gid_key];
+    const auto & pub_published_time = publishers_[gid_key];
 
     // Check if there are any subscribers, otherwise don't do anything
-    if (pub_published_time_->get_subscription_count() > 0) {
+    if (pub_published_time->get_subscription_count() > 0) {
       PublishedTime published_time;
 
       published_time.header = header;
       published_time.published_stamp = rclcpp::Clock().now();
 
-      pub_published_time_->publish(published_time);
+      pub_published_time->publish(published_time);
     }
   }
 

control/mpc_lateral_controller/param/lateral_controller_defaults.param.yaml

@@ -1,58 +1,58 @@
 /**:
   ros__parameters:
     # -- system --
-    traj_resample_dist: 0.1 # path resampling interval [m]
-    use_steer_prediction: false # flag for using steer prediction (do not use steer measurement)
-    admissible_position_error: 5.0 # stop mpc calculation when error is larger than the following value
-    admissible_yaw_error_rad: 1.57 # stop mpc calculation when error is larger than the following value
+    traj_resample_dist: 0.1         # path resampling interval [m]
+    use_steer_prediction: false     # flag for using steer prediction (do not use steer measurement)
+    admissible_position_error: 5.0  # stop mpc calculation when error is larger than the following value
+    admissible_yaw_error_rad: 1.57  # stop mpc calculation when error is larger than the following value
 
     # -- path smoothing --
-    enable_path_smoothing: false # flag for path smoothing
+    enable_path_smoothing: false   # flag for path smoothing
     path_filter_moving_ave_num: 25 # param of moving average filter for path smoothing
-    curvature_smoothing_num_traj: 15 # point-to-point index distance used in curvature calculation (for trajectory): curvature is calculated from three points p(i-num), p(i), p(i+num)
-    curvature_smoothing_num_ref_steer: 15 # point-to-point index distance used in curvature calculation (for steer command reference): curvature is calculated from three points p(i-num), p(i), p(i+num)
+    curvature_smoothing_num_traj: 15        # point-to-point index distance used in curvature calculation (for trajectory): curvature is calculated from three points p(i-num), p(i), p(i+num)
+    curvature_smoothing_num_ref_steer: 15   # point-to-point index distance used in curvature calculation (for steer command reference): curvature is calculated from three points p(i-num), p(i), p(i+num)
 
     # -- trajectory extending --
-    extend_trajectory_for_end_yaw_control: true  # flag of trajectory extending for terminal yaw control
+    extend_trajectory_for_end_yaw_control: false  # flag of trajectory extending for terminal yaw control
 
     # -- mpc optimization --
-    qp_solver_type: "osqp" # optimization solver option (unconstraint_fast or osqp)
-    mpc_prediction_horizon: 50 # prediction horizon step
-    mpc_prediction_dt: 0.1 # prediction horizon period [s]
-    mpc_weight_lat_error: 0.1 # lateral error weight in matrix Q
-    mpc_weight_heading_error: 0.0 # heading error weight in matrix Q
-    mpc_weight_heading_error_squared_vel: 0.3 # heading error * velocity weight in matrix Q
-    mpc_weight_steering_input: 1.0 # steering error weight in matrix R
-    mpc_weight_steering_input_squared_vel: 0.25 # steering error * velocity weight in matrix R
-    mpc_weight_lat_jerk: 0.0 # lateral jerk weight in matrix R
-    mpc_weight_steer_rate: 0.0 # steering rate weight in matrix R
-    mpc_weight_steer_acc: 0.000001 # steering angular acceleration weight in matrix R
-    mpc_low_curvature_weight_lat_error: 0.1 # lateral error weight in matrix Q in low curvature point
-    mpc_low_curvature_weight_heading_error: 0.0 # heading error weight in matrix Q in low curvature point
-    mpc_low_curvature_weight_heading_error_squared_vel: 0.3 # heading error * velocity weight in matrix Q in low curvature point
-    mpc_low_curvature_weight_steering_input: 1.0 # steering error weight in matrix R in low curvature point
-    mpc_low_curvature_weight_steering_input_squared_vel: 0.25 # steering error * velocity weight in matrix R in low curvature point
-    mpc_low_curvature_weight_lat_jerk: 0.0 # lateral jerk weight in matrix R in low curvature point
-    mpc_low_curvature_weight_steer_rate: 0.0 # steering rate weight in matrix R in low curvature point
-    mpc_low_curvature_weight_steer_acc: 0.000001 # steering angular acceleration weight in matrix R in low curvature point
-    mpc_low_curvature_thresh_curvature: 0.0 # threshold of curvature to use "low_curvature" parameter (recommend: 0.01~0.03)
-    mpc_weight_terminal_lat_error: 1.0 # terminal lateral error weight in matrix Q to improve mpc stability
-    mpc_weight_terminal_heading_error: 0.1 # terminal heading error weight in matrix Q to improve mpc stability
-    mpc_zero_ff_steer_deg: 0.5 # threshold that feed-forward angle becomes zero
-    mpc_acceleration_limit: 2.0 # limit on the vehicle's acceleration
-    mpc_velocity_time_constant: 0.3 # time constant used for velocity smoothing
-    mpc_min_prediction_length: 5.0 # minimum prediction length
+    qp_solver_type: "osqp"                       # optimization solver option (unconstraint_fast or osqp)
+    mpc_prediction_horizon: 50                   # prediction horizon step
+    mpc_prediction_dt: 0.1                       # prediction horizon period [s]
+    mpc_weight_lat_error: 1.0                    # lateral error weight in matrix Q
+    mpc_weight_heading_error: 0.0                # heading error weight in matrix Q
+    mpc_weight_heading_error_squared_vel: 0.3    # heading error * velocity weight in matrix Q
+    mpc_weight_steering_input: 1.0               # steering error weight in matrix R
+    mpc_weight_steering_input_squared_vel: 0.25  # steering error * velocity weight in matrix R
+    mpc_weight_lat_jerk: 0.1                     # lateral jerk weight in matrix R
+    mpc_weight_steer_rate: 0.0                   # steering rate weight in matrix R
+    mpc_weight_steer_acc: 0.000001               # steering angular acceleration weight in matrix R
+    mpc_low_curvature_weight_lat_error: 0.1                    # lateral error weight in matrix Q in low curvature point
+    mpc_low_curvature_weight_heading_error: 0.0                # heading error weight in matrix Q in low curvature point
+    mpc_low_curvature_weight_heading_error_squared_vel: 0.3    # heading error * velocity weight in matrix Q in low curvature point
+    mpc_low_curvature_weight_steering_input: 1.0               # steering error weight in matrix R in low curvature point
+    mpc_low_curvature_weight_steering_input_squared_vel: 0.25  # steering error * velocity weight in matrix R in low curvature point
+    mpc_low_curvature_weight_lat_jerk: 0.0                     # lateral jerk weight in matrix R in low curvature point
+    mpc_low_curvature_weight_steer_rate: 0.0                   # steering rate weight in matrix R in low curvature point
+    mpc_low_curvature_weight_steer_acc: 0.000001               # steering angular acceleration weight in matrix R in low curvature point
+    mpc_low_curvature_thresh_curvature: 0.0                    # threshold of curvature to use "low_curvature" parameter (recommend: 0.01~0.03)
+    mpc_weight_terminal_lat_error: 1.0           # terminal lateral error weight in matrix Q to improve mpc stability
+    mpc_weight_terminal_heading_error: 0.1       # terminal heading error weight in matrix Q to improve mpc stability
+    mpc_zero_ff_steer_deg: 0.5                   # threshold that feed-forward angle becomes zero
+    mpc_acceleration_limit: 2.0                  # limit on the vehicle's acceleration
+    mpc_velocity_time_constant: 0.3              # time constant used for velocity smoothing
+    mpc_min_prediction_length: 5.0               # minimum prediction length
 
     # -- vehicle model --
     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_rate_lim_dps_list_by_curvature: [10.0, 20.0, 30.0]            # steering angle rate limit list depending on curvature [deg/s]
+    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_rate_lim_dps_list_by_curvature: [40.0, 50.0, 60.0]            # steering angle rate limit list depending on curvature [deg/s]
     curvature_list_for_steer_rate_lim: [0.001, 0.002, 0.01]              # curvature list for steering angle rate limit interpolation in ascending order [/m]
-    steer_rate_lim_dps_list_by_velocity: [40.0, 30.0, 20.0]             # steering angle rate limit list depending on velocity [deg/s]
+    steer_rate_lim_dps_list_by_velocity: [60.0, 50.0, 40.0]             # steering angle rate limit list depending on velocity [deg/s]
     velocity_list_for_steer_rate_lim: [10.0, 15.0, 20.0]                  # velocity list for steering angle rate limit interpolation in ascending order [m/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]
+    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 --
     steering_lpf_cutoff_hz: 3.0 # cutoff frequency of lowpass filter for steering command [Hz]
@@ -65,7 +65,7 @@
     keep_steer_control_until_converged: true
     new_traj_duration_time: 1.0
     new_traj_end_dist: 0.3
-    mpc_converged_threshold_rps: 0.01 # threshold of mpc convergence check [rad/s]
+    mpc_converged_threshold_rps:  0.01 # threshold of mpc convergence check [rad/s]
 
     # steer offset
     steering_offset:

control/pid_longitudinal_controller/param/longitudinal_controller_defaults.param.yaml

@@ -5,19 +5,19 @@
     enable_smooth_stop: true
     enable_overshoot_emergency: true
     enable_large_tracking_error_emergency: true
-    enable_slope_compensation: false
+    enable_slope_compensation: true
     enable_keep_stopped_until_steer_convergence: true
 
     # state transition
     drive_state_stop_dist: 0.5
     drive_state_offset_stop_dist: 1.0
-    stopping_state_stop_dist: 0.49
+    stopping_state_stop_dist: 0.5
     stopped_state_entry_duration_time: 0.1
-    stopped_state_entry_vel: 0.1
+    stopped_state_entry_vel: 0.01
     stopped_state_entry_acc: 0.1
     emergency_state_overshoot_stop_dist: 1.5
     emergency_state_traj_trans_dev: 3.0
-    emergency_state_traj_rot_dev: 0.7
+    emergency_state_traj_rot_dev: 0.7854
 
     # drive state
     kp: 1.0
@@ -40,7 +40,7 @@
 
     # smooth stop state
     smooth_stop_max_strong_acc: -0.5
-    smooth_stop_min_strong_acc: -1.0
+    smooth_stop_min_strong_acc: -0.8
     smooth_stop_weak_acc: -0.3
     smooth_stop_weak_stop_acc: -0.8
     smooth_stop_strong_stop_acc: -3.4

control/trajectory_follower_node/test/test_controller_node.cpp

@@ -56,8 +56,6 @@ rclcpp::NodeOptions makeNodeOptions(const bool enable_keep_stopped_until_steer_c
   const auto lateral_share_dir =
     ament_index_cpp::get_package_share_directory("mpc_lateral_controller");
   rclcpp::NodeOptions node_options;
-  node_options.append_parameter_override("ctrl_period", 0.03);
-  node_options.append_parameter_override("timeout_thr_sec", 0.5);
   node_options.append_parameter_override("lateral_controller_mode", "mpc");
   node_options.append_parameter_override("longitudinal_controller_mode", "pid");
   node_options.append_parameter_override(
@@ -254,8 +252,10 @@ TEST_F(FakeNodeFixture, straight_trajectory)
 
   test_utils::waitForMessage(tester.node, this, tester.received_control_command);
   ASSERT_TRUE(tester.received_control_command);
+  // following conditions will pass even if the MPC solution does not converge
   EXPECT_EQ(tester.cmd_msg->lateral.steering_tire_angle, 0.0f);
   EXPECT_EQ(tester.cmd_msg->lateral.steering_tire_rotation_rate, 0.0f);
+  EXPECT_GT(tester.cmd_msg->longitudinal.speed, 0.0f);
   EXPECT_GT(rclcpp::Time(tester.cmd_msg->stamp), rclcpp::Time(traj_msg.header.stamp));
 }