squash all the modifications

fix ndt topic name


yolo multiple camera


fix lanenet


three lidar test


max iteration increased


ndt vehicle_test


controller test  day - 1


controller test day 2


purepursuit param changed


mpc-test-day-3


perception related changes


obstacle detection for planning offset changing


add vehicle_velocity_converter


control test day 4


control test 5


lateral controller tune


motionvel smoother param update


max lateral acc changed


update vehicle velocity controller launch


fix densification param 


update ekf params


Added traffic light 


add lidar_detection_model param


fix ekf init


update map altitude


add gnss twist topic to ekf


ndt max distance set 200


make clustering use non-ground points instead of outlier remover


practically disable object_lanelet_filter


avoid unknowns and pedestrians?


disable occlusion_spot in behavior_velocity_planner


change planner related params


tune mpc parameters for better tracking


change path planner/avoidance module params


tune controllers


feat(motion_velocity_smoother): add steering rate limit while planning velocity (autowarefoundation#1071)


function added,


not turning


fix the always positive curvature problem


added lower velocity limit


added vehicle parameters


functions created

Signed-off-by: Berkay <berkay@leodrive.ai>

Update readme

update svg


readme updated


with test params
change sample rate


calculate accurate dt


test

Signed-off-by: Berkay <berkay@leodrive.ai>

fix trajectory size


update readme


change map loader params
Signed-off-by: Berkay <berkay@leodrive.ai>

clear unnecessary comment

Signed-off-by: Berkay <berkay@leodrive.ai>

change the min and max index

Signed-off-by: Berkay <berkay@leodrive.ai>

ci(pre-commit): autofix

removed unnecessary params and comments

Signed-off-by: Berkay <berkay@leodrive.ai>

ci(pre-commit): autofix

all velocities in lookup distance is changed


Signed-off-by: Berkay <berkay@leodrive.ai>


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

change controller related params


change controller related params


control pure pursuit longitudinal tune


tune
change some planning params


(temp) add speed_bump vis to lanelet map (temp)


acc std distance -> 7.0 m


Revert "(temp) add speed_bump vis to lanelet map (temp)"

This reverts commit 1c51e37.

modify hdd_reader params


change NTP server


added camera component container


yolo changed gpu variable


added rectifier node 


changed rectifier node on container


configure localization topics as sensordataqos


steering offset topic changed


Revert "feat(motion_velocity_smoother): add steering rate limit while planning velocity (autowarefoundation#1071)"

This reverts commit 88efc4c.

control/pure_pursuit/config/pure_pursuit.param.yaml

@@ -2,6 +2,6 @@
   ros__parameters:
     # -- algorithm --
     lookahead_distance_ratio: 2.2
-    min_lookahead_distance: 2.5
+    min_lookahead_distance: 6.0
     reverse_min_lookahead_distance: 7.0
     converged_steer_rad: 0.1

control/pure_pursuit/src/pure_pursuit/pure_pursuit_lateral_controller.cpp

@@ -66,7 +66,7 @@ PurePursuitLateralController::PurePursuitLateralController(rclcpp::Node & node)
   // Algorithm Parameters
   param_.lookahead_distance_ratio =
     node_->declare_parameter<double>("lookahead_distance_ratio", 2.2);
-  param_.min_lookahead_distance = node_->declare_parameter<double>("min_lookahead_distance", 2.5);
+  param_.min_lookahead_distance = node_->declare_parameter<double>("min_lookahead_distance", 4.0);
   param_.reverse_min_lookahead_distance =
     node_->declare_parameter<double>("reverse_min_lookahead_distance", 7.0);
   param_.converged_steer_rad_ = node_->declare_parameter<double>("converged_steer_rad", 0.1);

launch/tier4_control_launch/config/trajectory_follower/lateral_controller.param.yaml

@@ -8,58 +8,61 @@
     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: true   # 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)
 
     # -- 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.5 # lateral error weight in matrix Q
+    mpc_weight_heading_error: 1.0 # heading error weight in matrix Q
+    mpc_weight_heading_error_squared_vel: 5.0 # heading error * velocity weight in matrix Q
+    mpc_weight_steering_input: 1.5 # steering error weight in matrix R
+    mpc_weight_steering_input_squared_vel: 3.0 # 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: 2.5 # 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: 5.0 # heading error * velocity weight in matrix Q in low curvature point
+    mpc_low_curvature_weight_steering_input: 1.5 # steering error weight in matrix R in low curvature point
+    mpc_low_curvature_weight_steering_input_squared_vel: 5.0 # 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.01 # threshold of curvature to use "low_curvature" parameter (recommend: 0.01~0.03)
+    mpc_weight_terminal_lat_error: 4.0 # terminal lateral error weight in matrix Q to improve mpc stability
+    mpc_weight_terminal_heading_error: 1.0 # 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: 0.9 # limit on the vehicle's acceleration
+    mpc_velocity_time_constant: 0.4 # time constant used for velocity smoothing
+    mpc_min_prediction_length: 4.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: 40.0         # steering angle rate limit [deg/s]
-    acceleration_limit: 2.0          # acceleration limit for trajectory velocity modification [m/ss]
+    vehicle_model_steer_tau: 0.5     # steering dynamics time constant (1d approximation) [s]
+    steer_rate_lim_dps: 19.0        # steering angle rate limit [deg/s]
+    acceleration_limit: 0.9          # 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]
-    error_deriv_lpf_cutoff_hz: 5.0
+    error_deriv_lpf_cutoff_hz: 19.0
 
     # stop state: steering command is kept in the previous value in the stop state.
     stop_state_entry_ego_speed: 0.001
     stop_state_entry_target_speed: 0.001
-    converged_steer_rad: 0.1
+    converged_steer_rad: 0.5
     keep_steer_control_until_converged: true
     new_traj_duration_time: 1.0
     new_traj_end_dist: 0.3
+    lookahead_distance_ratio: 2.7
+    min_lookahead_distance: 4.2
+    reverse_min_lookahead_distance: 7.0
 
     # vehicle parameters
     vehicle:

launch/tier4_control_launch/config/trajectory_follower/longitudinal_controller.param.yaml

@@ -1,17 +1,18 @@
 /**:
   ros__parameters:
-    delay_compensation_time: 0.17
+    longitudinal_ctrl_period: 0.03  # control period [s]
+    delay_compensation_time: 0.90  # delay compensation time [s]
 
-    enable_smooth_stop: true
+    enable_smooth_stop: false
     enable_overshoot_emergency: true
     enable_large_tracking_error_emergency: true
     enable_slope_compensation: false
     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
+    drive_state_offset_stop_dist: 5.0
+    stopping_state_stop_dist: 5.5
     stopped_state_entry_duration_time: 0.1
     stopped_state_entry_vel: 0.1
     stopped_state_entry_acc: 0.1
@@ -20,33 +21,33 @@
     emergency_state_traj_rot_dev: 0.7
 
     # drive state
-    kp: 1.0
-    ki: 0.1
+    kp: 0.7
+    ki: 0.25
     kd: 0.0
-    max_out: 1.0
-    min_out: -1.0
-    max_p_effort: 1.0
-    min_p_effort: -1.0
+    max_out: 0.9
+    min_out: -0.9
+    max_p_effort: 0.9
+    min_p_effort: -0.9
     max_i_effort: 0.3
-    min_i_effort: -0.3
+    min_i_effort: -0.9
     max_d_effort: 0.0
     min_d_effort: 0.0
     lpf_vel_error_gain: 0.9
     current_vel_threshold_pid_integration: 0.5
-    enable_brake_keeping_before_stop: false
+    enable_brake_keeping_before_stop: true
     brake_keeping_acc: -0.2
 
     # smooth stop state
-    smooth_stop_max_strong_acc: -0.5
-    smooth_stop_min_strong_acc: -1.0
-    smooth_stop_weak_acc: -0.3
-    smooth_stop_weak_stop_acc: -0.8
+    smooth_stop_max_strong_acc: -0.6
+    smooth_stop_min_strong_acc: -0.9
+    smooth_stop_weak_acc: -0.6
+    smooth_stop_weak_stop_acc: -0.9
     smooth_stop_strong_stop_acc: -3.4
-    smooth_stop_max_fast_vel: 0.5
+    smooth_stop_max_fast_vel: 1.0
     smooth_stop_min_running_vel: 0.01
     smooth_stop_min_running_acc: 0.01
     smooth_stop_weak_stop_time: 0.8
-    smooth_stop_weak_stop_dist: -0.3
+    smooth_stop_weak_stop_dist: -2.0
     smooth_stop_strong_stop_dist: -0.5
 
     # stopped state
@@ -60,12 +61,12 @@
     emergency_jerk: -3.0
 
     # acceleration limit
-    max_acc: 3.0
-    min_acc: -5.0
+    max_acc: 0.9
+    min_acc: -1.5
 
     # jerk limit
-    max_jerk: 2.0
-    min_jerk: -5.0
+    max_jerk: 0.6
+    min_jerk: -0.6
 
     # pitch
     use_trajectory_for_pitch_calculation: false

launch/tier4_localization_launch/config/crop_box_filter_measurement_range.param.yaml

@@ -2,10 +2,10 @@
   ros__parameters:
     input_frame: "base_link"
     output_frame: "base_link"
-    min_x: -60.0
-    max_x: 60.0
-    min_y: -60.0
-    max_y: 60.0
-    min_z: -30.0
+    min_x: -200.0
+    max_x: 200.0
+    min_y: -200.0
+    max_y: 200.0
+    min_z: -50.0
     max_z: 50.0
     negative: False

launch/tier4_localization_launch/config/ndt_scan_matcher.param.yaml

@@ -21,7 +21,7 @@
     resolution: 2.0
 
     # The number of iterations required to calculate alignment
-    max_iterations: 30
+    max_iterations: 50
 
     # Converged param type
     # 0=TRANSFORM_PROBABILITY, 1=NEAREST_VOXEL_TRANSFORMATION_LIKELIHOOD
@@ -34,7 +34,7 @@
 
     # If converged_param_type is 1
     # Threshold for deciding whether to trust the estimation result
-    converged_param_nearest_voxel_transformation_likelihood: 2.3
+    converged_param_nearest_voxel_transformation_likelihood: 1.5
 
     # The number of particles to estimate initial pose
     initial_estimate_particles_num: 100

launch/tier4_localization_launch/config/voxel_grid_filter.param.yaml

@@ -1,5 +1,5 @@
 /**:
   ros__parameters:
-    voxel_size_x: 3.0
-    voxel_size_y: 3.0
-    voxel_size_z: 3.0
+    voxel_size_x: 0.5
+    voxel_size_y: 0.5
+    voxel_size_z: 0.5

launch/tier4_localization_launch/launch/localization.launch.xml

@@ -1,6 +1,6 @@
 <?xml version="1.0"?>
 <launch>
-  <arg name="input/pointcloud" default="/sensing/lidar/top/outlier_filtered/pointcloud" description="The topic will be used in the localization util module"/>
+  <arg name="input/pointcloud" default="/sensing/lidar/concatenated/pointcloud" description="The topic will be used in the localization util module"/>
 
   <arg name="tier4_localization_launch_param_path" default="$(find-pkg-share tier4_localization_launch)/config" description="tier4_localization_launch parameter path"/>
 

launch/tier4_localization_launch/launch/pose_twist_fusion_filter/pose_twist_fusion_filter.launch.xml

@@ -7,7 +7,7 @@
       <arg name="twist_smoothing_steps" value="2"/>
       <arg name="input_initial_pose_name" value="/initialpose3d"/>
       <arg name="input_pose_with_cov_name" value="/localization/pose_estimator/pose_with_covariance"/>
-      <arg name="input_twist_with_cov_name" value="/localization/twist_estimator/twist_with_covariance"/>
+      <arg name="input_twist_with_cov_name" value="/applanix/lvx_client/twist_with_covariance"/>
       <arg name="output_odom_name" value="kinematic_state"/>
       <arg name="output_pose_name" value="pose"/>
       <arg name="output_pose_with_covariance_name" value="/localization/pose_with_covariance"/>

launch/tier4_localization_launch/launch/twist_estimator/twist_estimator.launch.xml

@@ -1,10 +1,10 @@
 <?xml version="1.0"?>
 <launch>
-  <group>
-    <include file="$(find-pkg-share gyro_odometer)/launch/gyro_odometer.launch.xml">
-      <arg name="input_vehicle_twist_with_covariance_topic" value="/sensing/vehicle_velocity_converter/twist_with_covariance"/>
-      <arg name="output_twist_with_covariance_topic" value="/localization/twist_estimator/twist_with_covariance"/>
-      <arg name="output_twist_with_covariance_raw_topic" value="/localization/twist_estimator/twist_with_covariance_raw"/>
-    </include>
-  </group>
+  <include file="$(find-pkg-share gyro_odometer)/launch/gyro_odometer.launch.xml">
+    <arg name="input_vehicle_twist_with_covariance_topic" value="/sensing/vehicle_velocity_converter/twist_with_covariance"/>
+    <arg name="output_twist_with_covariance_topic" value="/localization/twist_estimator/twist_with_covariance"/>
+    <arg name="output_twist_with_covariance_raw_topic" value="/localization/twist_estimator/twist_with_covariance_raw"/>
+  </include>
+
+<!--  <include file="$(find-pkg-share vehicle_velocity_converter)/launch/vehicle_velocity_converter.launch.xml"/>-->
 </launch>

launch/tier4_localization_launch/launch/util/util.launch.py

@@ -119,9 +119,14 @@ def add_launch_arg(name: str, default_value=None, description=None):
     add_launch_arg("use_intra_process", "true", "use ROS2 component container communication")
     add_launch_arg(
         "container",
-        "/sensing/lidar/top/pointcloud_preprocessor/velodyne_node_container",
+        "/sensing/lidar/pointcloud_preprocessor/pointcloud_preprocessor_container",
         "container name",
     )
+    add_launch_arg(
+        "input/pointcloud",
+        "/sensing/lidar/right/rectified/pointcloud",
+        "input topic name for raw pointcloud",
+    )
     add_launch_arg(
         "output/pointcloud",
         "downsample/pointcloud",

launch/tier4_localization_launch/launch/util/util.launch.xml

@@ -5,7 +5,7 @@
   <arg name="tier4_localization_launch_param_path" description="tier4_localization_launch param path"/>
   <arg name="output/pointcloud" default="downsample/pointcloud" description="final output topic name"/>
   <!-- container -->
-  <arg name="container" default="/sensing/lidar/top/pointcloud_preprocessor/velodyne_node_container" description="container name"/>
+  <arg name="container" default="/sensing/lidar/right/pointcloud_preprocessor/velodyne_node_container" description="container name"/>
 
   <!-- option -->
   <arg name="use_intra_process" default="true" description="use ROS2 component container communication"/>

launch/tier4_perception_launch/config/object_recognition/detection/object_lanelet_filter.param.yaml

@@ -1,7 +1,7 @@
 /**:
   ros__parameters:
     filter_target_label:
-      UNKNOWN : true
+      UNKNOWN : false
       CAR : false
       TRUCK : false
       BUS : false

launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml

@@ -28,18 +28,27 @@
   <arg name="container_name" default="pointcloud_container"/>
   <arg name="use_validator" default="true" description="use obstacle_pointcloud based validator"/>
 
-  <!-- Jetson AGX -->
-  <!-- <include file="$(find-pkg-share tensorrt_yolo)/launch/yolo.launch.xml">
-    <arg name="image_raw0" value="$(var image_raw0)"/>
-    <arg name="image_raw1" value="$(var image_raw1)"/>
-    <arg name="image_raw2" value="$(var image_raw2)"/>
-    <arg name="image_raw3" value="$(var image_raw3)"/>
-    <arg name="image_raw4" value="$(var image_raw4)"/>
-    <arg name="image_raw5" value="$(var image_raw5)"/>
-    <arg name="image_raw6" value="$(var image_raw6)"/>
-    <arg name="image_raw7" value="$(var image_raw7)"/>
-    <arg name="image_number" value="$(var image_number)"/>
-  </include> -->
+
+<!--  &lt;!&ndash; ISUZU Decompress Cameras &ndash;&gt;-->
+<!--  <include file="$(find-pkg-share image_transport_decompressor)/launch/isuzu_image_decompressor.launch.xml"/>-->
+
+  <!-- ISUZU -->
+<!--  <include file="$(find-pkg-share tensorrt_yolo)/launch/isuzu_yolo.launch.xml">-->
+<!--    <arg name="image_raw0" value="$(var image_raw0)"/>-->
+<!--    <arg name="image_raw1" value="$(var image_raw1)"/>-->
+<!--    <arg name="image_raw2" value="$(var image_raw2)"/>-->
+<!--    <arg name="image_raw3" value="$(var image_raw3)"/>-->
+<!--    <arg name="image_raw4" value="$(var image_raw4)"/>-->
+<!--    <arg name="image_raw5" value="$(var image_raw5)"/>-->
+<!--    <arg name="image_raw6" value="$(var image_raw6)"/>-->
+<!--    <arg name="image_raw7" value="$(var image_raw7)"/>-->
+<!--    <arg name="image_number" value="$(var image_number)"/>-->
+<!--  </include>-->
+
+
+  <include file="$(find-pkg-share tier4_perception_launch)/launch/object_recognition/detection/front_camera_node_container.launch.py"/>
+  <include file="$(find-pkg-share tier4_perception_launch)/launch/object_recognition/detection/left_camera_node_container.launch.py"/>
+  <include file="$(find-pkg-share tier4_perception_launch)/launch/object_recognition/detection/right_camera_node_container.launch.py"/>
 
   <!-- Pointcloud map filter -->
   <group>
@@ -154,6 +163,8 @@
     <include file="$(find-pkg-share lidar_centerpoint)/launch/lidar_centerpoint.launch.xml">
       <arg name="input/pointcloud" value="$(var input/pointcloud)"/>
       <arg name="output/objects" value="objects"/>
+      <arg name="score_threshold" value="0.75"/>
+      <arg name="densification_num_past_frames" value="1"/>
     </include>
   </group>
 

launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml

@@ -5,6 +5,7 @@
 
   <!-- lidar param -->
   <arg name="input/pointcloud"/>
+  <arg name="mode" description="options: `camera_lidar_fusion`, `lidar` or `camera`"/>
   <arg name="lidar_detection_model" default="centerpoint" description="options: `centerpoint`, `apollo`"/>
   <arg name="use_pointcloud_map" default="true" description="use pointcloud map in detection"/>
   <arg name="use_object_filter" default="true" description="use object filter"/>
@@ -28,7 +29,8 @@
   <arg name="camera_info6" default=""/>
   <arg name="image_raw7" default=""/>
   <arg name="camera_info7" default=""/>
-  <arg name="image_number" default="1" description="choose image raw number(0-7)"/>
+  <arg name="image_number" default="3" description="choose image raw number(0-7)"/>
+  <arg name="use_pointcloud_map" default="true" description="use pointcloud map in detection"/>
 
   <!-- radar param -->