diff --git a/.github/workflows/build-and-test-differential.yaml b/.github/workflows/build-and-test-differential.yaml index f25c2fe07aab5..e7e53cacaee3b 100644 --- a/.github/workflows/build-and-test-differential.yaml +++ b/.github/workflows/build-and-test-differential.yaml @@ -2,20 +2,9 @@ name: build-and-test-differential on: pull_request: - types: - - opened - - synchronize - - labeled jobs: - prevent-no-label-execution: - uses: autowarefoundation/autoware-github-actions/.github/workflows/prevent-no-label-execution.yaml@v1 - with: - label: tag:run-build-and-test-differential - build-and-test-differential: - needs: prevent-no-label-execution - if: ${{ needs.prevent-no-label-execution.outputs.run == 'true' }} runs-on: ubuntu-latest container: ${{ matrix.container }}${{ matrix.container-suffix }} strategy: @@ -36,6 +25,9 @@ jobs: with: fetch-depth: 0 + - name: Check disk space before build + run: df -h + - name: Remove exec_depend uses: autowarefoundation/autoware-github-actions/remove-exec-depend@v1 @@ -69,6 +61,9 @@ jobs: verbose: true flags: differential + - name: Check disk space after build + run: df -h + clang-tidy-differential: runs-on: ubuntu-latest container: ghcr.io/autowarefoundation/autoware-universe:humble-latest-cuda diff --git a/launch/tier4_localization_launch/launch/pose_twist_fusion_filter/pose_twist_fusion_filter.launch.xml b/launch/tier4_localization_launch/launch/pose_twist_fusion_filter/pose_twist_fusion_filter.launch.xml index 23a1201a84958..bdea584bd58ae 100644 --- a/launch/tier4_localization_launch/launch/pose_twist_fusion_filter/pose_twist_fusion_filter.launch.xml +++ b/launch/tier4_localization_launch/launch/pose_twist_fusion_filter/pose_twist_fusion_filter.launch.xml @@ -27,10 +27,10 @@ - + diff --git a/launch/tier4_map_launch/package.xml b/launch/tier4_map_launch/package.xml index f75a181bfb659..8fb41076204a8 100644 --- a/launch/tier4_map_launch/package.xml +++ b/launch/tier4_map_launch/package.xml @@ -8,6 +8,11 @@ Ryu Yamamoto Koji Minoda Kento Yabuuchi + Yamato Ando + NGUYEN Viet Anh + Taiki Yamada + Shintaro Sakoda + Masahiro Sakamoto Apache License 2.0 ament_cmake_auto diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/detector/camera_lidar_detector.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/camera_lidar_detector.launch.xml index 1c9201a9af8b3..2232feb6d7c67 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/detector/camera_lidar_detector.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/camera_lidar_detector.launch.xml @@ -85,6 +85,9 @@ + + + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/detector/lidar_dnn_detector.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/lidar_dnn_detector.launch.xml index 1a97659b357d8..5b5fabd4dd886 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/detector/lidar_dnn_detector.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/lidar_dnn_detector.launch.xml @@ -22,6 +22,9 @@ + + + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/detector/lidar_rule_detector.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/lidar_rule_detector.launch.xml index ce34640bd3179..cec0c3bc05aac 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/detector/lidar_rule_detector.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/lidar_rule_detector.launch.xml @@ -26,6 +26,9 @@ + + + diff --git a/localization/twist2accel/CMakeLists.txt b/localization/twist2accel/CMakeLists.txt index 93b9546be0a36..59f23eacb2fb3 100644 --- a/localization/twist2accel/CMakeLists.txt +++ b/localization/twist2accel/CMakeLists.txt @@ -13,4 +13,5 @@ ament_target_dependencies(twist2accel) ament_auto_package( INSTALL_TO_SHARE launch + config ) diff --git a/localization/twist2accel/README.md b/localization/twist2accel/README.md index ec73c34052f99..5a540dca895d4 100644 --- a/localization/twist2accel/README.md +++ b/localization/twist2accel/README.md @@ -21,10 +21,7 @@ This package is responsible for estimating acceleration using the output of `ekf ## Parameters -| Name | Type | Description | -| -------------------- | ------ | ------------------------------------------------------------------------- | -| `use_odom` | bool | use odometry if true, else use twist input (default: true) | -| `accel_lowpass_gain` | double | lowpass gain for lowpass filter in estimating acceleration (default: 0.9) | +{{ json_to_markdown("localization/twist2accel/schema/twist2accel.schema.json") }} ## Future work diff --git a/localization/twist2accel/config/twist2accel.param.yaml b/localization/twist2accel/config/twist2accel.param.yaml new file mode 100644 index 0000000000000..e58e029248253 --- /dev/null +++ b/localization/twist2accel/config/twist2accel.param.yaml @@ -0,0 +1,4 @@ +/**: + ros__parameters: + use_odom: true + accel_lowpass_gain: 0.9 diff --git a/localization/twist2accel/launch/twist2accel.launch.xml b/localization/twist2accel/launch/twist2accel.launch.xml index 47b8a95d13a08..c4c9a3ed50bfc 100644 --- a/localization/twist2accel/launch/twist2accel.launch.xml +++ b/localization/twist2accel/launch/twist2accel.launch.xml @@ -1,6 +1,6 @@ - - + + @@ -8,7 +8,6 @@ - - + diff --git a/localization/twist2accel/schema/twist2accel.schema.json b/localization/twist2accel/schema/twist2accel.schema.json new file mode 100644 index 0000000000000..6b3c2bd094166 --- /dev/null +++ b/localization/twist2accel/schema/twist2accel.schema.json @@ -0,0 +1,36 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for twist2accel Nodes", + "type": "object", + "definitions": { + "twist2accel": { + "type": "object", + "properties": { + "use_odom": { + "type": "boolean", + "default": true, + "description": "use odometry if true, else use twist input." + }, + "accel_lowpass_gain": { + "type": "number", + "default": 0.9, + "minimum": 0.0, + "description": "lowpass gain for lowpass filter in estimating acceleration." + } + }, + "required": ["use_odom", "accel_lowpass_gain"] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/twist2accel" + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] +} diff --git a/localization/twist2accel/src/twist2accel.cpp b/localization/twist2accel/src/twist2accel.cpp index a8a7d46f57056..68019f27e95fe 100644 --- a/localization/twist2accel/src/twist2accel.cpp +++ b/localization/twist2accel/src/twist2accel.cpp @@ -35,8 +35,8 @@ Twist2Accel::Twist2Accel(const std::string & node_name, const rclcpp::NodeOption pub_accel_ = create_publisher("output/accel", 1); prev_twist_ptr_ = nullptr; - accel_lowpass_gain_ = declare_parameter("accel_lowpass_gain", 0.5); - use_odom_ = declare_parameter("use_odom", true); + accel_lowpass_gain_ = declare_parameter("accel_lowpass_gain"); + use_odom_ = declare_parameter("use_odom"); lpf_alx_ptr_ = std::make_shared(accel_lowpass_gain_); lpf_aly_ptr_ = std::make_shared(accel_lowpass_gain_); diff --git a/perception/euclidean_cluster/launch/euclidean_cluster.launch.py b/perception/euclidean_cluster/launch/euclidean_cluster.launch.py index 66c25396a656e..b8d4f61a9cf28 100644 --- a/perception/euclidean_cluster/launch/euclidean_cluster.launch.py +++ b/perception/euclidean_cluster/launch/euclidean_cluster.launch.py @@ -76,6 +76,13 @@ def load_composable_node_param(param_path): executable="component_container", composable_node_descriptions=[], output="screen", + condition=UnlessCondition(LaunchConfiguration("use_pointcloud_container")), + ) + + target_container = ( + LaunchConfiguration("pointcloud_container_name") + if IfCondition(LaunchConfiguration("use_pointcloud_container")).evaluate(context) + else container ) use_low_height_pointcloud_loader = LoadComposableNodes( @@ -83,13 +90,13 @@ def load_composable_node_param(param_path): low_height_cropbox_filter_component, use_low_height_euclidean_component, ], - target_container=container, + target_container=target_container, condition=IfCondition(LaunchConfiguration("use_low_height_cropbox")), ) disuse_low_height_pointcloud_loader = LoadComposableNodes( composable_node_descriptions=[disuse_low_height_euclidean_component], - target_container=container, + target_container=target_container, condition=UnlessCondition(LaunchConfiguration("use_low_height_cropbox")), ) @@ -106,6 +113,8 @@ def add_launch_arg(name: str, default_value=None): add_launch_arg("input_map", "/map/pointcloud_map"), add_launch_arg("output_clusters", "clusters"), add_launch_arg("use_low_height_cropbox", "false"), + add_launch_arg("use_pointcloud_container", "false"), + add_launch_arg("pointcloud_container_name", "pointcloud_container"), add_launch_arg( "euclidean_param_path", [ diff --git a/perception/euclidean_cluster/launch/euclidean_cluster.launch.xml b/perception/euclidean_cluster/launch/euclidean_cluster.launch.xml index fd1ea82befae0..f4deeccf7b76c 100644 --- a/perception/euclidean_cluster/launch/euclidean_cluster.launch.xml +++ b/perception/euclidean_cluster/launch/euclidean_cluster.launch.xml @@ -5,6 +5,8 @@ + + @@ -12,5 +14,8 @@ + + + diff --git a/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.py b/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.py index 00bcd4422bd3c..607e1bf30ccaa 100644 --- a/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.py +++ b/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.py @@ -77,6 +77,13 @@ def load_composable_node_param(param_path): executable="component_container", composable_node_descriptions=[], output="screen", + condition=UnlessCondition(LaunchConfiguration("use_pointcloud_container")), + ) + + target_container = ( + LaunchConfiguration("pointcloud_container_name") + if IfCondition(LaunchConfiguration("use_pointcloud_container")).evaluate(context) + else container ) use_low_height_pointcloud_loader = LoadComposableNodes( @@ -84,13 +91,13 @@ def load_composable_node_param(param_path): low_height_cropbox_filter_component, use_low_height_euclidean_component, ], - target_container=container, + target_container=target_container, condition=IfCondition(LaunchConfiguration("use_low_height_cropbox")), ) disuse_low_height_pointcloud_loader = LoadComposableNodes( composable_node_descriptions=[disuse_low_height_euclidean_component], - target_container=container, + target_container=target_container, condition=UnlessCondition(LaunchConfiguration("use_low_height_cropbox")), ) return [ @@ -110,6 +117,8 @@ def add_launch_arg(name: str, default_value=None): add_launch_arg("input_map", "/map/pointcloud_map"), add_launch_arg("output_clusters", "clusters"), add_launch_arg("use_low_height_cropbox", "false"), + add_launch_arg("use_pointcloud_container", "false"), + add_launch_arg("pointcloud_container_name", "pointcloud_container"), add_launch_arg( "voxel_grid_based_euclidean_param_path", [ diff --git a/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.xml b/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.xml index b6a426dabfd12..3a7c685d8f449 100644 --- a/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.xml +++ b/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.xml @@ -5,6 +5,8 @@ + + @@ -12,5 +14,8 @@ + + + diff --git a/perception/image_projection_based_fusion/README.md b/perception/image_projection_based_fusion/README.md index dba64a27232b1..e83cf92400a60 100644 --- a/perception/image_projection_based_fusion/README.md +++ b/perception/image_projection_based_fusion/README.md @@ -53,6 +53,15 @@ The timeout threshold should be set according to the postprocessing time. E.g, if the postprocessing time is around 50ms, the timeout threshold should be set smaller than 50ms, so that the whole processing time could be less than 100ms. current default value at autoware.universe for XX1: - timeout_ms: 50.0 +#### The `build_only` option + +The `pointpainting_fusion` node has `build_only` option to build the TensorRT engine file from the ONNX file. +Although it is preferred to move all the ROS parameters in `.param.yaml` file in Autoware Universe, the `build_only` option is not moved to the `.param.yaml` file for now, because it may be used as a flag to execute the build as a pre-task. You can execute with the following command: + +```bash +ros2 launch image_projection_based_fusion pointpainting_fusion.launch.xml model_name:=pointpainting model_path:=/home/autoware/autoware_data/image_projection_based_fusion model_param_path:=$(ros2 pkg prefix image_projection_based_fusion --share)/config/pointpainting.param.yaml build_only:=true +``` + #### Known Limits The rclcpp::TimerBase timer could not break a for loop, therefore even if time is out when fusing a roi msg at the middle, the program will run until all msgs are fused. diff --git a/perception/image_projection_based_fusion/config/pointpainting.param.yaml b/perception/image_projection_based_fusion/config/pointpainting.param.yaml index 21d31f216373b..8ccd46978630d 100755 --- a/perception/image_projection_based_fusion/config/pointpainting.param.yaml +++ b/perception/image_projection_based_fusion/config/pointpainting.param.yaml @@ -1,5 +1,11 @@ /**: ros__parameters: + trt_precision: fp16 + encoder_onnx_path: "$(var model_path)/pts_voxel_encoder_$(var model_name).onnx" + encoder_engine_path: "$(var model_path)/pts_voxel_encoder_$(var model_name).engine" + head_onnx_path: "$(var model_path)/pts_backbone_neck_head_$(var model_name).onnx" + head_engine_path: "$(var model_path)/pts_backbone_neck_head_$(var model_name).engine" + model_params: class_names: ["CAR", "TRUCK", "BUS", "BICYCLE", "PEDESTRIAN"] paint_class_names: ["CAR", "BICYCLE", "PEDESTRIAN"] diff --git a/perception/image_projection_based_fusion/config/segmentation_pointcloud_fusion.param.yaml b/perception/image_projection_based_fusion/config/segmentation_pointcloud_fusion.param.yaml index 914ad13519807..c2752710a54f1 100644 --- a/perception/image_projection_based_fusion/config/segmentation_pointcloud_fusion.param.yaml +++ b/perception/image_projection_based_fusion/config/segmentation_pointcloud_fusion.param.yaml @@ -28,3 +28,13 @@ # this is selected based on semantic segmentation model accuracy, # calibration accuracy and unknown reaction distance filter_distance_threshold: 60.0 + + # debug + debug_mode: false + filter_scope_minx: -100 + filter_scope_maxx: 100 + filter_scope_miny: -100 + filter_scope_maxy: 100 + filter_scope_minz: -100 + filter_scope_maxz: 100 + image_buffer_size: 15 diff --git a/perception/image_projection_based_fusion/launch/pointpainting_fusion.launch.xml b/perception/image_projection_based_fusion/launch/pointpainting_fusion.launch.xml index 33781461fa1cc..f336eb16e94a1 100644 --- a/perception/image_projection_based_fusion/launch/pointpainting_fusion.launch.xml +++ b/perception/image_projection_based_fusion/launch/pointpainting_fusion.launch.xml @@ -26,6 +26,9 @@ + + + @@ -38,19 +41,59 @@ - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - - - - - - + + + diff --git a/perception/image_projection_based_fusion/launch/segmentation_pointcloud_fusion.launch.xml b/perception/image_projection_based_fusion/launch/segmentation_pointcloud_fusion.launch.xml index 96bf47594bfe8..1db2bb20793ac 100644 --- a/perception/image_projection_based_fusion/launch/segmentation_pointcloud_fusion.launch.xml +++ b/perception/image_projection_based_fusion/launch/segmentation_pointcloud_fusion.launch.xml @@ -20,17 +20,6 @@ - - - - - - - - - - - @@ -72,16 +61,6 @@ - - - - - - - - - - diff --git a/perception/lidar_centerpoint/config/detection_class_remapper.param.yaml b/perception/lidar_centerpoint/config/detection_class_remapper.param.yaml index ed378ffa44a70..baea087c96bca 100644 --- a/perception/lidar_centerpoint/config/detection_class_remapper.param.yaml +++ b/perception/lidar_centerpoint/config/detection_class_remapper.param.yaml @@ -29,9 +29,9 @@ max_area_matrix: #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN [ 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, #UNKNOWN - 0.000, 0.000, 36.000, 0.000, inf, 0.000, 0.000, 0.000, #CAR - 0.000, 0.000, 0.000, 0.000, inf, 0.000, 0.000, 0.000, #TRUCK - 0.000, 0.000, 0.000, 0.000, inf, 0.000, 0.000, 0.000, #BUS + 0.000, 0.000, 36.000, 0.000, 999.999, 0.000, 0.000, 0.000, #CAR + 0.000, 0.000, 0.000, 0.000, 999.999, 0.000, 0.000, 0.000, #TRUCK + 0.000, 0.000, 0.000, 0.000, 999.999, 0.000, 0.000, 0.000, #BUS 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, #TRAILER 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, #MOTORBIKE 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, #BICYCLE diff --git a/perception/lidar_centerpoint/launch/lidar_centerpoint.launch.xml b/perception/lidar_centerpoint/launch/lidar_centerpoint.launch.xml index a7f181ab78ac6..1210875770510 100644 --- a/perception/lidar_centerpoint/launch/lidar_centerpoint.launch.xml +++ b/perception/lidar_centerpoint/launch/lidar_centerpoint.launch.xml @@ -11,20 +11,45 @@ - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/bicycle_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/bicycle_tracker.hpp index ad268283d5890..ba50933eddaec 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/bicycle_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/bicycle_tracker.hpp @@ -32,16 +32,18 @@ class BicycleTracker : public Tracker private: KalmanFilter ekf_; rclcpp::Time last_update_time_; - enum IDX { X = 0, Y = 1, YAW = 2, VX = 3, SLIP = 4 }; + enum IDX { X = 0, Y = 1, YAW = 2, VEL = 3, SLIP = 4 }; struct EkfParams { char dim_x = 5; - float q_cov_x; - float q_cov_y; - float q_cov_yaw; - float q_cov_slip; - float q_cov_vx; - float p0_cov_vx; + float q_stddev_acc_long; + float q_stddev_acc_lat; + float q_stddev_yaw_rate_min; + float q_stddev_yaw_rate_max; + float q_cov_slip_rate_min; + float q_cov_slip_rate_max; + float q_max_slip_angle; + float p0_cov_vel; float p0_cov_slip; float r_cov_x; float r_cov_y; @@ -51,7 +53,7 @@ class BicycleTracker : public Tracker float p0_cov_yaw; } ekf_params_; - double max_vx_; + double max_vel_; double max_slip_; double lf_; double lr_; diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp index 8a9b6adfc9cd5..e19b6382ad952 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp @@ -32,26 +32,28 @@ class BigVehicleTracker : public Tracker private: KalmanFilter ekf_; rclcpp::Time last_update_time_; - enum IDX { X = 0, Y = 1, YAW = 2, VX = 3, SLIP = 4 }; + enum IDX { X = 0, Y = 1, YAW = 2, VEL = 3, SLIP = 4 }; struct EkfParams { char dim_x = 5; - float q_cov_x; - float q_cov_y; - float q_cov_yaw; - float q_cov_slip; - float q_cov_vx; - float p0_cov_vx; + float q_stddev_acc_long; + float q_stddev_acc_lat; + float q_stddev_yaw_rate_min; + float q_stddev_yaw_rate_max; + float q_cov_slip_rate_min; + float q_cov_slip_rate_max; + float q_max_slip_angle; + float p0_cov_vel; float p0_cov_slip; float r_cov_x; float r_cov_y; float r_cov_yaw; - float r_cov_vx; + float r_cov_vel; float p0_cov_x; float p0_cov_y; float p0_cov_yaw; } ekf_params_; - double max_vx_; + double max_vel_; double max_slip_; double lf_; double lr_; diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp index 79e9a20a10421..05fa0a5ef01ba 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp @@ -33,28 +33,30 @@ class NormalVehicleTracker : public Tracker private: KalmanFilter ekf_; rclcpp::Time last_update_time_; - enum IDX { X = 0, Y = 1, YAW = 2, VX = 3, SLIP = 4 }; + enum IDX { X = 0, Y = 1, YAW = 2, VEL = 3, SLIP = 4 }; struct EkfParams { char dim_x = 5; - float q_cov_x; - float q_cov_y; - float q_cov_yaw; - float q_cov_slip; - float q_cov_vx; - float p0_cov_vx; + float q_stddev_acc_long; + float q_stddev_acc_lat; + float q_stddev_yaw_rate_min; + float q_stddev_yaw_rate_max; + float q_cov_slip_rate_min; + float q_cov_slip_rate_max; + float q_max_slip_angle; + float p0_cov_vel; float p0_cov_slip; float r_cov_x; float r_cov_y; float r_cov_yaw; - float r_cov_vx; + float r_cov_vel; float p0_cov_x; float p0_cov_y; float p0_cov_yaw; } ekf_params_; - double max_vx_; + double max_vel_; double max_slip_; double lf_; double lr_; @@ -88,6 +90,7 @@ class NormalVehicleTracker : public Tracker const rclcpp::Time & time, autoware_auto_perception_msgs::msg::TrackedObject & object) const override; void setNearestCornerOrSurfaceIndex(const geometry_msgs::msg::Transform & self_transform); + double getMeasurementYaw(const autoware_auto_perception_msgs::msg::DetectedObject & object); virtual ~NormalVehicleTracker() {} }; diff --git a/perception/multi_object_tracker/models.md b/perception/multi_object_tracker/models.md index f74967943be32..f1f53c7398c0a 100644 --- a/perception/multi_object_tracker/models.md +++ b/perception/multi_object_tracker/models.md @@ -11,21 +11,21 @@ CTRV model is a model that assumes constant turn rate and velocity magnitude. - state transition equation $$ -\begin{align*} -x_{k+1} &= x_k + v_{x_k} \cos(\psi_k) \cdot dt \\ -y_{k+1} &= y_k + v_{x_k} \sin(\psi_k) \cdot dt \\ -\psi_{k+1} &= \psi_k + \dot{\psi}_k \cdot dt \\ -v_{x_{k+1}} &= v_{x_k} \\ -\dot{\psi}_{k+1} &= \dot{\psi}_k \\ -\end{align*} +\begin{aligned} +x_{k+1} & = x_{k} + v_{k} \cos(\psi_k) \cdot {d t} \\ +y_{k+1} & = y_{k} + v_{k} \sin(\psi_k) \cdot {d t} \\ +\psi_{k+1} & = \psi_k + \dot\psi_{k} \cdot {d t} \\ +v_{k+1} & = v_{k} \\ +\dot\psi_{k+1} & = \dot\psi_{k} \\ +\end{aligned} $$ - jacobian $$ A = \begin{bmatrix} -1 & 0 & -v_x \sin(\psi) \cdot dt & \cos(\psi) \cdot dt & 0 \\ -0 & 1 & v_x \cos(\psi) \cdot dt & \sin(\psi) \cdot dt & 0 \\ +1 & 0 & -v \sin(\psi) \cdot dt & \cos(\psi) \cdot dt & 0 \\ +0 & 1 & v \cos(\psi) \cdot dt & \sin(\psi) \cdot dt & 0 \\ 0 & 0 & 1 & 0 & dt \\ 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 1 \\ @@ -40,17 +40,20 @@ The merit of using this model is that it can prevent unintended yaw rotation whe ![kinematic_bicycle_model](image/kinematic_bicycle_model.png) - **state variable** - - pose( $x,y$ ), velocity( $v$ ), yaw( $\psi$ ), and slip angle ( $\beta$ ) - - $[x_{k} ,y_{k} , v_{k} , \psi_{k} , \beta_{k} ]^\mathrm{T}$ + - pose( $x,y$ ), yaw( $\psi$ ), velocity( $v$ ), and slip angle ( $\beta$ ) + - $[x_{k}, y_{k}, \psi_{k}, v_{k}, \beta_{k} ]^\mathrm{T}$ - **Prediction Equation** - $dt$: sampling time + - $w_{k} = \dot\psi_{k} = \frac{ v_{k} \sin \left( \beta_{k} \right) }{ l_r }$ : angular velocity $$ \begin{aligned} -x_{k+1} & =x_{k}+v_{k} \cos \left(\psi_{k}+\beta_{k}\right) d t \\ -y_{k+1} & =y_{k}+v_{k} \sin \left(\psi_{k}+\beta_{k}\right) d t \\ -v_{k+1} & =v_{k} \\ -\psi_{k+1} & =\psi_{k}+\frac{v_{k}}{l_{r}} \sin \beta_{k} d t \\ +x_{k+1} & = x_{k} + v_{k} \cos \left( \psi_{k}+\beta_{k} \right) {d t} + -\frac{1}{2} \left\lbrace w_k v_k \sin \left(\psi_{k}+\beta_{k} \right) \right\rbrace {d t}^2\\ +y_{k+1} & = y_{k} + v_{k} \sin \left( \psi_{k}+\beta_{k} \right) {d t} + +\frac{1}{2} \left\lbrace w_k v_k \cos \left(\psi_{k}+\beta_{k} \right) \right\rbrace {d t}^2\\ +\psi_{k+1} & =\psi_{k} + w_k {d t} \\ +v_{k+1} & = v_{k} \\ \beta_{k+1} & =\beta_{k} \end{aligned} $$ @@ -59,9 +62,15 @@ $$ $$ \frac{\partial f}{\partial \mathrm x}=\left[\begin{array}{ccccc} -1 & 0 & -v \sin (\psi+\beta) d t & v \cos (\psi+\beta) & -v \sin (\psi+\beta) d t \\ -0 & 1 & v \cos (\psi+\beta) d t & v \sin (\psi+\beta) & v \cos (\psi+\beta) d t \\ -0 & 0 & 1 & \frac{1}{l_r} \sin \beta d t & \frac{v}{l_r} \cos \beta d t \\ +1 & 0 + & v \cos (\psi+\beta) {d t} - \frac{1}{2} \left\lbrace w v \cos \left( \psi+\beta \right) \right\rbrace {d t}^2 + & \sin (\psi+\beta) {d t} - \left\lbrace w \sin \left( \psi+\beta \right) \right\rbrace {d t}^2 + & -v \sin (\psi+\beta) {d t} - \frac{v^2}{2l_r} \left\lbrace \cos(\beta)\sin(\psi+\beta)+\sin(\beta)\cos(\psi+\beta) \right\rbrace {d t}^2 \\ +0 & 1 + & v \sin (\psi+\beta) {d t} - \frac{1}{2} \left\lbrace w v \sin \left( \psi+\beta \right) \right\rbrace {d t}^2 + & \cos (\psi+\beta) {d t} + \left\lbrace w \cos \left( \psi+\beta \right) \right\rbrace {d t}^2 + & v \cos (\psi+\beta) {d t} + \frac{v^2}{2l_r} \left\lbrace \cos(\beta)\cos(\psi+\beta)-\sin(\beta)\sin(\psi+\beta) \right\rbrace {d t}^2 \\ +0 & 0 & 1 & \frac{1}{l_r} \sin \beta {d t} & \frac{v}{l_r} \cos \beta d t \\ 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 1 \end{array}\right] diff --git a/perception/multi_object_tracker/src/data_association/data_association.cpp b/perception/multi_object_tracker/src/data_association/data_association.cpp index 5e7cadc423216..f367e19c11f2a 100644 --- a/perception/multi_object_tracker/src/data_association/data_association.cpp +++ b/perception/multi_object_tracker/src/data_association/data_association.cpp @@ -201,7 +201,7 @@ Eigen::MatrixXd DataAssociation::calcScoreMatrix( measurement_object.kinematics.pose_with_covariance.pose.position, tracked_object.kinematics.pose_with_covariance.pose.position, getXYCovariance(tracked_object.kinematics.pose_with_covariance)); - if (2.448 /*95%*/ <= mahalanobis_dist) passed_gate = false; + if (3.035 /*99%*/ <= mahalanobis_dist) passed_gate = false; } // 2d iou gate if (passed_gate) { diff --git a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp index 6571e70b8c123..0b56b11451ad3 100644 --- a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp @@ -52,51 +52,57 @@ BicycleTracker::BicycleTracker( { object_ = object; - // initialize params - float q_stddev_x = 0.6; // [m/s] - float q_stddev_y = 0.6; // [m/s] - float q_stddev_yaw = tier4_autoware_utils::deg2rad(10); // [rad/s] - float q_stddev_vx = tier4_autoware_utils::kmph2mps(10); // [m/(s*s)] - float q_stddev_slip = tier4_autoware_utils::deg2rad(15); // [rad/(s*s)] - float r_stddev_x = 0.6; // [m] - float r_stddev_y = 0.4; // [m] - float r_stddev_yaw = tier4_autoware_utils::deg2rad(30); // [rad] - float p0_stddev_x = 0.8; // [m/s] - float p0_stddev_y = 0.5; // [m/s] - float p0_stddev_yaw = tier4_autoware_utils::deg2rad(1000); // [rad/s] - float p0_stddev_vx = tier4_autoware_utils::kmph2mps(1000); // [m/(s*s)] - float p0_stddev_slip = tier4_autoware_utils::deg2rad(10); // [rad/(s*s)] - ekf_params_.q_cov_x = std::pow(q_stddev_x, 2.0); - ekf_params_.q_cov_y = std::pow(q_stddev_y, 2.0); - ekf_params_.q_cov_yaw = std::pow(q_stddev_yaw, 2.0); - ekf_params_.q_cov_vx = std::pow(q_stddev_vx, 2.0); - ekf_params_.q_cov_slip = std::pow(q_stddev_slip, 2.0); + // Initialize parameters + // measurement noise covariance: detector uncertainty + ego vehicle motion uncertainty + float r_stddev_x = 0.5; // in vehicle coordinate [m] + float r_stddev_y = 0.4; // in vehicle coordinate [m] + float r_stddev_yaw = tier4_autoware_utils::deg2rad(30); // [rad] ekf_params_.r_cov_x = std::pow(r_stddev_x, 2.0); ekf_params_.r_cov_y = std::pow(r_stddev_y, 2.0); ekf_params_.r_cov_yaw = std::pow(r_stddev_yaw, 2.0); + // initial state covariance + float p0_stddev_x = 0.8; // [m] + float p0_stddev_y = 0.5; // [m] + float p0_stddev_yaw = tier4_autoware_utils::deg2rad(25); // in map coordinate [rad] + float p0_stddev_vel = tier4_autoware_utils::kmph2mps(1000); // in object coordinate [m/s] + float p0_stddev_slip = tier4_autoware_utils::deg2rad(5); // in object coordinate [rad/s] ekf_params_.p0_cov_x = std::pow(p0_stddev_x, 2.0); ekf_params_.p0_cov_y = std::pow(p0_stddev_y, 2.0); ekf_params_.p0_cov_yaw = std::pow(p0_stddev_yaw, 2.0); - ekf_params_.p0_cov_vx = std::pow(p0_stddev_vx, 2.0); + ekf_params_.p0_cov_vel = std::pow(p0_stddev_vel, 2.0); ekf_params_.p0_cov_slip = std::pow(p0_stddev_slip, 2.0); - max_vx_ = tier4_autoware_utils::kmph2mps(100); // [m/s] - max_slip_ = tier4_autoware_utils::deg2rad(30); // [rad/s] - - // initialize X matrix + // process noise covariance + ekf_params_.q_stddev_acc_long = 9.8 * 0.35; // [m/(s*s)] uncertain longitudinal acceleration + ekf_params_.q_stddev_acc_lat = 9.8 * 0.15; // [m/(s*s)] uncertain lateral acceleration + ekf_params_.q_stddev_yaw_rate_min = + tier4_autoware_utils::deg2rad(5.0); // [rad/s] uncertain yaw change rate + ekf_params_.q_stddev_yaw_rate_max = + tier4_autoware_utils::deg2rad(15.0); // [rad/s] uncertain yaw change rate + float q_stddev_slip_rate_min = + tier4_autoware_utils::deg2rad(1); // [rad/s] uncertain slip angle change rate + float q_stddev_slip_rate_max = + tier4_autoware_utils::deg2rad(10.0); // [rad/s] uncertain slip angle change rate + ekf_params_.q_cov_slip_rate_min = std::pow(q_stddev_slip_rate_min, 2.0); + ekf_params_.q_cov_slip_rate_max = std::pow(q_stddev_slip_rate_max, 2.0); + ekf_params_.q_max_slip_angle = tier4_autoware_utils::deg2rad(30); // [rad] max slip angle + // limitations + max_vel_ = tier4_autoware_utils::kmph2mps(100); // [m/s] + max_slip_ = tier4_autoware_utils::deg2rad(30); // [rad/s] + + // initialize state vector X Eigen::MatrixXd X(ekf_params_.dim_x, 1); X(IDX::X) = object.kinematics.pose_with_covariance.pose.position.x; X(IDX::Y) = object.kinematics.pose_with_covariance.pose.position.y; X(IDX::YAW) = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); + X(IDX::SLIP) = 0.0; if (object.kinematics.has_twist) { - X(IDX::VX) = object.kinematics.twist_with_covariance.twist.linear.x; + X(IDX::VEL) = object.kinematics.twist_with_covariance.twist.linear.x; } else { - X(IDX::VX) = 0.0; + X(IDX::VEL) = 0.0; } - X(IDX::SLIP) = 0.0; - // initialize P matrix + // initialize state covariance matrix P Eigen::MatrixXd P = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); - if (!object.kinematics.has_position_covariance) { const double cos_yaw = std::cos(X(IDX::YAW)); const double sin_yaw = std::sin(X(IDX::YAW)); @@ -110,7 +116,7 @@ BicycleTracker::BicycleTracker( ekf_params_.p0_cov_x * sin_yaw * sin_yaw + ekf_params_.p0_cov_y * cos_yaw * cos_yaw; P(IDX::Y, IDX::X) = P(IDX::X, IDX::Y); P(IDX::YAW, IDX::YAW) = ekf_params_.p0_cov_yaw; - P(IDX::VX, IDX::VX) = ekf_params_.p0_cov_vx; + P(IDX::VEL, IDX::VEL) = ekf_params_.p0_cov_vel; P(IDX::SLIP, IDX::SLIP) = ekf_params_.p0_cov_slip; } else { P(IDX::X, IDX::X) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; @@ -120,10 +126,10 @@ BicycleTracker::BicycleTracker( P(IDX::YAW, IDX::YAW) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_YAW]; if (object.kinematics.has_twist_covariance) { - P(IDX::VX, IDX::VX) = + P(IDX::VEL, IDX::VEL) = object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; } else { - P(IDX::VX, IDX::VX) = ekf_params_.p0_cov_vx; + P(IDX::VEL, IDX::VEL) = ekf_params_.p0_cov_vel; } P(IDX::SLIP, IDX::SLIP) = ekf_params_.p0_cov_slip; } @@ -137,15 +143,28 @@ BicycleTracker::BicycleTracker( ekf_.init(X, P); // Set lf, lr - double point_ratio = 0.2; // under steered if smaller than 0.5 - lf_ = bounding_box_.length * point_ratio; - lr_ = bounding_box_.length * (1.0 - point_ratio); + lf_ = bounding_box_.length * 0.3; // 30% front from the center + lr_ = bounding_box_.length * 0.3; // 30% rear from the center } bool BicycleTracker::predict(const rclcpp::Time & time) { const double dt = (time - last_update_time_).seconds(); - bool ret = predict(dt, ekf_); + if (dt < 0.0) { + RCLCPP_WARN(logger_, "dt is negative. (%f)", dt); + return false; + } + // if dt is too large, shorten dt and repeat prediction + const double dt_max = 0.11; + const uint32_t repeat = std::ceil(dt / dt_max); + const double dt_ = dt / repeat; + bool ret = false; + for (uint32_t i = 0; i < repeat; ++i) { + ret = predict(dt_, ekf_); + if (!ret) { + return false; + } + } if (ret) { last_update_time_ = time; } @@ -154,69 +173,116 @@ bool BicycleTracker::predict(const rclcpp::Time & time) bool BicycleTracker::predict(const double dt, KalmanFilter & ekf) const { - /* == Nonlinear model == static bicycle model + /* static bicycle model (constant slip angle, constant velocity) * - * x_{k+1} = x_k + vx_k * cos(yaw_k + slip_k) * dt - * y_{k+1} = y_k + vx_k * sin(yaw_k + slip_k) * dt - * yaw_{k+1} = yaw_k + vx_k / l_r * sin(slip_k) * dt - * vx_{k+1} = vx_k + * w_k = vel_k * sin(slip_k) / l_r + * x_{k+1} = x_k + vel_k*cos(yaw_k+slip_k)*dt - 0.5*w_k*vel_k*sin(yaw_k+slip_k)*dt*dt + * y_{k+1} = y_k + vel_k*sin(yaw_k+slip_k)*dt + 0.5*w_k*vel_k*cos(yaw_k+slip_k)*dt*dt + * yaw_{k+1} = yaw_k + w_k * dt + * vel_{k+1} = vel_k * slip_{k+1} = slip_k * */ - /* == Linearized model == + /* Jacobian Matrix + * + * A = [ 1, 0, -vel*sin(yaw+slip)*dt - 0.5*w_k*vel_k*cos(yaw+slip)*dt*dt, + * cos(yaw+slip)*dt - w*sin(yaw+slip)*dt*dt, + * -vel*sin(yaw+slip)*dt - 0.5*vel*vel/l_r*(cos(slip)sin(yaw+slip)+sin(slip)cos(yaw+slip))*dt*dt ] + * [ 0, 1, vel*cos(yaw+slip)*dt - 0.5*w_k*vel_k*sin(yaw+slip)*dt*dt, + * sin(yaw+slip)*dt + w*cos(yaw+slip)*dt*dt, + * vel*cos(yaw+slip)*dt + 0.5*vel*vel/l_r*(cos(slip)cos(yaw+slip)-sin(slip)sin(yaw+slip))*dt*dt ] + * [ 0, 0, 1, 1/l_r*sin(slip)*dt, vel/l_r*cos(slip)*dt] + * [ 0, 0, 0, 1, 0] + * [ 0, 0, 0, 0, 1] * - * A = [ 1, 0, -vx*sin(yaw+slip)*dt, cos(yaw+slip)*dt, -vx*sin(yaw+slip)*dt] - * [ 0, 1, vx*cos(yaw+slip)*dt, sin(yaw+slip)*dt, vx*cos(yaw+slip)*dt] - * [ 0, 0, 1, 1/l_r*sin(slip)*dt, vx/l_r*cos(slip)*dt] - * [ 0, 0, 0, 1, 0] - * [ 0, 0, 0, 0, 1] */ - // X t + // Current state vector X t Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); // predicted state ekf.getX(X_t); const double cos_yaw = std::cos(X_t(IDX::YAW) + X_t(IDX::SLIP)); const double sin_yaw = std::sin(X_t(IDX::YAW) + X_t(IDX::SLIP)); + const double vel = X_t(IDX::VEL); const double cos_slip = std::cos(X_t(IDX::SLIP)); const double sin_slip = std::sin(X_t(IDX::SLIP)); - const double vx = X_t(IDX::VX); - const double sin_2yaw = std::sin(2.0f * X_t(IDX::YAW)); - - // X t+1 - Eigen::MatrixXd X_next_t(ekf_params_.dim_x, 1); // predicted state - X_next_t(IDX::X) = X_t(IDX::X) + vx * cos_yaw * dt; // dx = v * cos(yaw) - X_next_t(IDX::Y) = X_t(IDX::Y) + vx * sin_yaw * dt; // dy = v * sin(yaw) - X_next_t(IDX::YAW) = X_t(IDX::YAW) + vx / lr_ * sin_slip * dt; // dyaw = omega - X_next_t(IDX::VX) = X_t(IDX::VX); - X_next_t(IDX::SLIP) = X_t(IDX::SLIP); - // A + double w = vel * sin_slip / lr_; + const double sin_2yaw = std::sin(2.0f * X_t(IDX::YAW)); + const double w_dtdt = w * dt * dt; + const double vv_dtdt__lr = vel * vel * dt * dt / lr_; + + // Predict state vector X t+1 + Eigen::MatrixXd X_next_t(ekf_params_.dim_x, 1); // predicted state + X_next_t(IDX::X) = + X_t(IDX::X) + vel * cos_yaw * dt - 0.5 * vel * sin_slip * w_dtdt; // dx = v * cos(yaw) * dt + X_next_t(IDX::Y) = + X_t(IDX::Y) + vel * sin_yaw * dt + 0.5 * vel * cos_slip * w_dtdt; // dy = v * sin(yaw) * dt + X_next_t(IDX::YAW) = X_t(IDX::YAW) + w * dt; // d(yaw) = w * dt + X_next_t(IDX::VEL) = X_t(IDX::VEL); + X_next_t(IDX::SLIP) = X_t(IDX::SLIP); // slip_angle = asin(lr * w / v) + + // State transition matrix A Eigen::MatrixXd A = Eigen::MatrixXd::Identity(ekf_params_.dim_x, ekf_params_.dim_x); - A(IDX::X, IDX::YAW) = -vx * sin_yaw * dt; - A(IDX::X, IDX::VX) = cos_yaw * dt; - A(IDX::X, IDX::SLIP) = -vx * sin_yaw * dt; - A(IDX::Y, IDX::YAW) = vx * cos_yaw * dt; - A(IDX::Y, IDX::VX) = sin_yaw * dt; - A(IDX::Y, IDX::SLIP) = vx * cos_yaw * dt; - A(IDX::YAW, IDX::VX) = 1.0 / lr_ * sin_slip * dt; - A(IDX::YAW, IDX::SLIP) = vx / lr_ * cos_slip * dt; - - // Q + A(IDX::X, IDX::YAW) = -vel * sin_yaw * dt - 0.5 * vel * cos_yaw * w_dtdt; + A(IDX::X, IDX::VEL) = cos_yaw * dt - sin_yaw * w_dtdt; + A(IDX::X, IDX::SLIP) = + -vel * sin_yaw * dt - 0.5 * (cos_slip * sin_yaw + sin_slip * cos_yaw) * vv_dtdt__lr; + A(IDX::Y, IDX::YAW) = vel * cos_yaw * dt - 0.5 * vel * sin_yaw * w_dtdt; + A(IDX::Y, IDX::VEL) = sin_yaw * dt + cos_yaw * w_dtdt; + A(IDX::Y, IDX::SLIP) = + vel * cos_yaw * dt + 0.5 * (cos_slip * cos_yaw - sin_slip * sin_yaw) * vv_dtdt__lr; + A(IDX::YAW, IDX::VEL) = 1.0 / lr_ * sin_slip * dt; + A(IDX::YAW, IDX::SLIP) = vel / lr_ * cos_slip * dt; + + // Process noise covariance Q + float q_stddev_yaw_rate{0.0}; + if (vel <= 0.01) { + q_stddev_yaw_rate = ekf_params_.q_stddev_yaw_rate_min; + } else { + // uncertainty of the yaw rate is limited by + // centripetal acceleration a_lat : d(yaw)/dt = w = a_lat/v + // or maximum slip angle slip_max : w = v*sin(slip_max)/l_r + q_stddev_yaw_rate = std::min( + ekf_params_.q_stddev_acc_lat / vel, + vel * std::sin(ekf_params_.q_max_slip_angle) / lr_); // [rad/s] + q_stddev_yaw_rate = std::min(q_stddev_yaw_rate, ekf_params_.q_stddev_yaw_rate_max); + q_stddev_yaw_rate = std::max(q_stddev_yaw_rate, ekf_params_.q_stddev_yaw_rate_min); + } + float q_cov_slip_rate{0.0f}; + if (vel <= 0.01) { + q_cov_slip_rate = ekf_params_.q_cov_slip_rate_min; + } else { + // The slip angle rate uncertainty is modeled as follows: + // d(slip)/dt ~ - sin(slip)/v * d(v)/dt + l_r/v * d(w)/dt + // where sin(slip) = w * l_r / v + // d(w)/dt is assumed to be proportional to w (more uncertain when slip is large) + // d(v)/dt and d(w)/t are considered to be uncorrelated + q_cov_slip_rate = + std::pow(ekf_params_.q_stddev_acc_lat * sin_slip / vel, 2) + std::pow(sin_slip * 1.5, 2); + q_cov_slip_rate = std::min(q_cov_slip_rate, ekf_params_.q_cov_slip_rate_max); + q_cov_slip_rate = std::max(q_cov_slip_rate, ekf_params_.q_cov_slip_rate_min); + } + const float q_cov_x = std::pow(0.5 * ekf_params_.q_stddev_acc_long * dt * dt, 2); + const float q_cov_y = std::pow(0.5 * ekf_params_.q_stddev_acc_lat * dt * dt, 2); + const float q_cov_yaw = std::pow(q_stddev_yaw_rate * dt, 2); + const float q_cov_vel = std::pow(ekf_params_.q_stddev_acc_long * dt, 2); + const float q_cov_slip = q_cov_slip_rate * dt * dt; + Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); // Rotate the covariance matrix according to the vehicle yaw // because q_cov_x and y are in the vehicle coordinate system. - Q(IDX::X, IDX::X) = - (ekf_params_.q_cov_x * cos_yaw * cos_yaw + ekf_params_.q_cov_y * sin_yaw * sin_yaw) * dt * dt; - Q(IDX::X, IDX::Y) = (0.5f * (ekf_params_.q_cov_x - ekf_params_.q_cov_y) * sin_2yaw) * dt * dt; - Q(IDX::Y, IDX::Y) = - (ekf_params_.q_cov_x * sin_yaw * sin_yaw + ekf_params_.q_cov_y * cos_yaw * cos_yaw) * dt * dt; + Q(IDX::X, IDX::X) = (q_cov_x * cos_yaw * cos_yaw + q_cov_y * sin_yaw * sin_yaw); + Q(IDX::X, IDX::Y) = (0.5f * (q_cov_x - q_cov_y) * sin_2yaw); + Q(IDX::Y, IDX::Y) = (q_cov_x * sin_yaw * sin_yaw + q_cov_y * cos_yaw * cos_yaw); Q(IDX::Y, IDX::X) = Q(IDX::X, IDX::Y); - Q(IDX::YAW, IDX::YAW) = ekf_params_.q_cov_yaw * dt * dt; - Q(IDX::VX, IDX::VX) = ekf_params_.q_cov_vx * dt * dt; - Q(IDX::SLIP, IDX::SLIP) = ekf_params_.q_cov_slip * dt * dt; - Eigen::MatrixXd B = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); - Eigen::MatrixXd u = Eigen::MatrixXd::Zero(ekf_params_.dim_x, 1); + Q(IDX::YAW, IDX::YAW) = q_cov_yaw; + Q(IDX::VEL, IDX::VEL) = q_cov_vel; + Q(IDX::SLIP, IDX::SLIP) = q_cov_slip; + + // control-input model B and control-input u are not used + // Eigen::MatrixXd B = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); + // Eigen::MatrixXd u = Eigen::MatrixXd::Zero(ekf_params_.dim_x, 1); if (!ekf.predict(X_next_t, A, Q)) { RCLCPP_WARN(logger_, "Cannot predict"); @@ -310,20 +376,20 @@ bool BicycleTracker::measureWithPose( } } - // update with ekf + // ekf update if (!ekf_.update(Y, C, R)) { RCLCPP_WARN(logger_, "Cannot update"); } - // normalize yaw and limit vx, wz + // normalize yaw and limit vel, slip { Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); Eigen::MatrixXd P_t(ekf_params_.dim_x, ekf_params_.dim_x); ekf_.getX(X_t); ekf_.getP(P_t); X_t(IDX::YAW) = tier4_autoware_utils::normalizeRadian(X_t(IDX::YAW)); - if (!(-max_vx_ <= X_t(IDX::VX) && X_t(IDX::VX) <= max_vx_)) { - X_t(IDX::VX) = X_t(IDX::VX) < 0 ? -max_vx_ : max_vx_; + if (!(-max_vel_ <= X_t(IDX::VEL) && X_t(IDX::VEL) <= max_vel_)) { + X_t(IDX::VEL) = X_t(IDX::VEL) < 0 ? -max_vel_ : max_vel_; } if (!(-max_slip_ <= X_t(IDX::SLIP) && X_t(IDX::SLIP) <= max_slip_)) { X_t(IDX::SLIP) = X_t(IDX::SLIP) < 0 ? -max_slip_ : max_slip_; @@ -341,14 +407,26 @@ bool BicycleTracker::measureWithPose( bool BicycleTracker::measureWithShape( const autoware_auto_perception_msgs::msg::DetectedObject & object) { + // if the input shape is convex type, convert it to bbox type + autoware_auto_perception_msgs::msg::DetectedObject bbox_object; if (object.shape.type != autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { - return false; + utils::convertConvexHullToBoundingBox(object, bbox_object); + } else { + bbox_object = object; } - constexpr float gain = 0.9; - bounding_box_.length = gain * bounding_box_.length + (1.0 - gain) * object.shape.dimensions.x; - bounding_box_.width = gain * bounding_box_.width + (1.0 - gain) * object.shape.dimensions.y; - bounding_box_.height = gain * bounding_box_.height + (1.0 - gain) * object.shape.dimensions.z; + constexpr float gain = 0.9; + bounding_box_.length = + gain * bounding_box_.length + (1.0 - gain) * bbox_object.shape.dimensions.x; + bounding_box_.width = gain * bounding_box_.width + (1.0 - gain) * bbox_object.shape.dimensions.y; + bounding_box_.height = + gain * bounding_box_.height + (1.0 - gain) * bbox_object.shape.dimensions.z; + last_input_bounding_box_ = { + bbox_object.shape.dimensions.x, bbox_object.shape.dimensions.y, bbox_object.shape.dimensions.z}; + + // update lf, lr + lf_ = bounding_box_.length * 0.3; // 30% front from the center + lr_ = bounding_box_.length * 0.3; // 30% rear from the center return true; } @@ -382,7 +460,7 @@ bool BicycleTracker::getTrackedObject( object.object_id = getUUID(); object.classification = getClassification(); - // predict kinematics + // predict state KalmanFilter tmp_ekf_for_no_update = ekf_; const double dt = (time - last_update_time_).seconds(); if (0.001 /*1msec*/ < dt) { @@ -429,23 +507,44 @@ bool BicycleTracker::getTrackedObject( pose_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = P(IDX::YAW, IDX::YAW); // twist - twist_with_cov.twist.linear.x = X_t(IDX::VX) * std::cos(X_t(IDX::SLIP)); - twist_with_cov.twist.linear.y = X_t(IDX::VX) * std::sin(X_t(IDX::SLIP)); + twist_with_cov.twist.linear.x = X_t(IDX::VEL) * std::cos(X_t(IDX::SLIP)); + twist_with_cov.twist.linear.y = X_t(IDX::VEL) * std::sin(X_t(IDX::SLIP)); twist_with_cov.twist.angular.z = - X_t(IDX::VX) / lr_ * std::sin(X_t(IDX::SLIP)); // yaw_rate = vx_k / l_r * sin(slip_k) - // twist covariance - constexpr double vy_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative + X_t(IDX::VEL) * std::sin(X_t(IDX::SLIP)) / lr_; // yaw_rate = vel_k * sin(slip_k) / l_r + /* twist covariance + * convert covariance from velocity, slip angle to vx, vy, and yaw angle + * + * vx = vel * cos(slip) + * vy = vel * sin(slip) + * wz = vel * sin(slip) / l_r = vy / l_r + * + */ + constexpr double vz_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative constexpr double wx_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative constexpr double wy_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - twist_with_cov.covariance[utils::MSG_COV_IDX::X_X] = P(IDX::VX, IDX::VX); - twist_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = vy_cov; + + Eigen::MatrixXd cov_jacob(3, 2); + cov_jacob << std::cos(X_t(IDX::SLIP)), -X_t(IDX::VEL) * std::sin(X_t(IDX::SLIP)), + std::sin(X_t(IDX::SLIP)), X_t(IDX::VEL) * std::cos(X_t(IDX::SLIP)), + std::sin(X_t(IDX::SLIP)) / lr_, X_t(IDX::VEL) * std::cos(X_t(IDX::SLIP)) / lr_; + Eigen::MatrixXd cov_twist(2, 2); + cov_twist << P(IDX::VEL, IDX::VEL), P(IDX::VEL, IDX::SLIP), P(IDX::SLIP, IDX::VEL), + P(IDX::SLIP, IDX::SLIP); + Eigen::MatrixXd twist_cov_mat = cov_jacob * cov_twist * cov_jacob.transpose(); + + twist_with_cov.covariance[utils::MSG_COV_IDX::X_X] = twist_cov_mat(0, 0); + twist_with_cov.covariance[utils::MSG_COV_IDX::X_Y] = twist_cov_mat(0, 1); + twist_with_cov.covariance[utils::MSG_COV_IDX::X_YAW] = twist_cov_mat(0, 2); + twist_with_cov.covariance[utils::MSG_COV_IDX::Y_X] = twist_cov_mat(1, 0); + twist_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = twist_cov_mat(1, 1); + twist_with_cov.covariance[utils::MSG_COV_IDX::Y_YAW] = twist_cov_mat(1, 2); + twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_X] = twist_cov_mat(2, 0); + twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_Y] = twist_cov_mat(2, 1); + twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = twist_cov_mat(2, 2); twist_with_cov.covariance[utils::MSG_COV_IDX::Z_Z] = vz_cov; - twist_with_cov.covariance[utils::MSG_COV_IDX::X_YAW] = P(IDX::VX, IDX::SLIP); - twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_X] = P(IDX::SLIP, IDX::VX); twist_with_cov.covariance[utils::MSG_COV_IDX::ROLL_ROLL] = wx_cov; twist_with_cov.covariance[utils::MSG_COV_IDX::PITCH_PITCH] = wy_cov; - twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = P(IDX::SLIP, IDX::SLIP); // set shape object.shape.dimensions.x = bounding_box_.length; diff --git a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp index 97d6d48c35d1b..34a72437d7721 100644 --- a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp @@ -16,6 +16,14 @@ // Author: v1.0 Yukihiro Saito // +#include "multi_object_tracker/tracker/model/big_vehicle_tracker.hpp" + +#include "multi_object_tracker/utils/utils.hpp" + +#include +#include +#include + #include #include #include @@ -26,17 +34,11 @@ #else #include #endif - -#define EIGEN_MPL2_ONLY -#include "multi_object_tracker/tracker/model/big_vehicle_tracker.hpp" -#include "multi_object_tracker/utils/utils.hpp" #include "object_recognition_utils/object_recognition_utils.hpp" +#define EIGEN_MPL2_ONLY #include #include -#include -#include -#include using Label = autoware_auto_perception_msgs::msg::ObjectClassification; @@ -51,53 +53,59 @@ BigVehicleTracker::BigVehicleTracker( { object_ = object; - // initialize params - float q_stddev_x = 1.5; // [m/s] - float q_stddev_y = 1.5; // [m/s] - float q_stddev_yaw = tier4_autoware_utils::deg2rad(20); // [rad/s] - float q_stddev_vx = tier4_autoware_utils::kmph2mps(10); // [m/(s*s)] - float q_stddev_slip = tier4_autoware_utils::deg2rad(5); // [rad/(s*s)] - float r_stddev_x = 1.5; // [m] - float r_stddev_y = 0.5; // [m] - float r_stddev_yaw = tier4_autoware_utils::deg2rad(30); // [rad] - float r_stddev_vx = 1.0; // [m/s] - float p0_stddev_x = 1.5; // [m] - float p0_stddev_y = 0.5; // [m] - float p0_stddev_yaw = tier4_autoware_utils::deg2rad(30); // [rad] - float p0_stddev_vx = tier4_autoware_utils::kmph2mps(1000); // [m/s] - float p0_stddev_slip = tier4_autoware_utils::deg2rad(10); // [rad/s] - ekf_params_.q_cov_x = std::pow(q_stddev_x, 2.0); - ekf_params_.q_cov_y = std::pow(q_stddev_y, 2.0); - ekf_params_.q_cov_yaw = std::pow(q_stddev_yaw, 2.0); - ekf_params_.q_cov_vx = std::pow(q_stddev_vx, 2.0); - ekf_params_.q_cov_slip = std::pow(q_stddev_slip, 2.0); + // Initialize parameters + // measurement noise covariance: detector uncertainty + ego vehicle motion uncertainty + float r_stddev_x = 0.5; // in vehicle coordinate [m] + float r_stddev_y = 0.4; // in vehicle coordinate [m] + float r_stddev_yaw = tier4_autoware_utils::deg2rad(20); // [rad] + float r_stddev_vel = 1.0; // [m/s] ekf_params_.r_cov_x = std::pow(r_stddev_x, 2.0); ekf_params_.r_cov_y = std::pow(r_stddev_y, 2.0); ekf_params_.r_cov_yaw = std::pow(r_stddev_yaw, 2.0); - ekf_params_.r_cov_vx = std::pow(r_stddev_vx, 2.0); + ekf_params_.r_cov_vel = std::pow(r_stddev_vel, 2.0); + // initial state covariance + float p0_stddev_x = 1.5; // [m] + float p0_stddev_y = 0.5; // [m] + float p0_stddev_yaw = tier4_autoware_utils::deg2rad(25); // in map coordinate [rad] + float p0_stddev_vel = tier4_autoware_utils::kmph2mps(1000); // in object coordinate [m/s] + float p0_stddev_slip = tier4_autoware_utils::deg2rad(5); // in object coordinate [rad/s] ekf_params_.p0_cov_x = std::pow(p0_stddev_x, 2.0); ekf_params_.p0_cov_y = std::pow(p0_stddev_y, 2.0); ekf_params_.p0_cov_yaw = std::pow(p0_stddev_yaw, 2.0); - ekf_params_.p0_cov_vx = std::pow(p0_stddev_vx, 2.0); + ekf_params_.p0_cov_vel = std::pow(p0_stddev_vel, 2.0); ekf_params_.p0_cov_slip = std::pow(p0_stddev_slip, 2.0); - max_vx_ = tier4_autoware_utils::kmph2mps(100); // [m/s] + // process noise covariance + ekf_params_.q_stddev_acc_long = 9.8 * 0.35; // [m/(s*s)] uncertain longitudinal acceleration + ekf_params_.q_stddev_acc_lat = 9.8 * 0.15; // [m/(s*s)] uncertain lateral acceleration + ekf_params_.q_stddev_yaw_rate_min = + tier4_autoware_utils::deg2rad(1.5); // [rad/s] uncertain yaw change rate + ekf_params_.q_stddev_yaw_rate_max = + tier4_autoware_utils::deg2rad(15.0); // [rad/s] uncertain yaw change rate + float q_stddev_slip_rate_min = + tier4_autoware_utils::deg2rad(0.3); // [rad/s] uncertain slip angle change rate + float q_stddev_slip_rate_max = + tier4_autoware_utils::deg2rad(10.0); // [rad/s] uncertain slip angle change rate + ekf_params_.q_cov_slip_rate_min = std::pow(q_stddev_slip_rate_min, 2.0); + ekf_params_.q_cov_slip_rate_max = std::pow(q_stddev_slip_rate_max, 2.0); + ekf_params_.q_max_slip_angle = tier4_autoware_utils::deg2rad(30); // [rad] max slip angle + // limitations + max_vel_ = tier4_autoware_utils::kmph2mps(100); // [m/s] max_slip_ = tier4_autoware_utils::deg2rad(30); // [rad] velocity_deviation_threshold_ = tier4_autoware_utils::kmph2mps(10); // [m/s] - // initialize X matrix + // initialize state vector X Eigen::MatrixXd X(ekf_params_.dim_x, 1); X(IDX::X) = object.kinematics.pose_with_covariance.pose.position.x; X(IDX::Y) = object.kinematics.pose_with_covariance.pose.position.y; X(IDX::YAW) = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); X(IDX::SLIP) = 0.0; if (object.kinematics.has_twist) { - X(IDX::VX) = object.kinematics.twist_with_covariance.twist.linear.x; - // X(IDX::YAW) = object.kinematics.twist_with_covariance.twist.angular.z; + X(IDX::VEL) = object.kinematics.twist_with_covariance.twist.linear.x; } else { - X(IDX::VX) = 0.0; + X(IDX::VEL) = 0.0; } - // initialize P matrix + // initialize state covariance matrix P Eigen::MatrixXd P = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); if (!object.kinematics.has_position_covariance) { const double cos_yaw = std::cos(X(IDX::YAW)); @@ -112,7 +120,7 @@ BigVehicleTracker::BigVehicleTracker( ekf_params_.p0_cov_x * sin_yaw * sin_yaw + ekf_params_.p0_cov_y * cos_yaw * cos_yaw; P(IDX::Y, IDX::X) = P(IDX::X, IDX::Y); P(IDX::YAW, IDX::YAW) = ekf_params_.p0_cov_yaw; - P(IDX::VX, IDX::VX) = ekf_params_.p0_cov_vx; + P(IDX::VEL, IDX::VEL) = ekf_params_.p0_cov_vel; P(IDX::SLIP, IDX::SLIP) = ekf_params_.p0_cov_slip; } else { P(IDX::X, IDX::X) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; @@ -122,10 +130,10 @@ BigVehicleTracker::BigVehicleTracker( P(IDX::YAW, IDX::YAW) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_YAW]; if (object.kinematics.has_twist_covariance) { - P(IDX::VX, IDX::VX) = + P(IDX::VEL, IDX::VEL) = object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; } else { - P(IDX::VX, IDX::VX) = ekf_params_.p0_cov_vx; + P(IDX::VEL, IDX::VEL) = ekf_params_.p0_cov_vel; } P(IDX::SLIP, IDX::SLIP) = ekf_params_.p0_cov_slip; } @@ -142,9 +150,7 @@ BigVehicleTracker::BigVehicleTracker( bounding_box_ = { bbox_object.shape.dimensions.x, bbox_object.shape.dimensions.y, bbox_object.shape.dimensions.z}; - last_input_bounding_box_ = { - bbox_object.shape.dimensions.x, bbox_object.shape.dimensions.y, - bbox_object.shape.dimensions.z}; + last_input_bounding_box_ = bounding_box_; } ekf_.init(X, P); @@ -152,15 +158,28 @@ BigVehicleTracker::BigVehicleTracker( setNearestCornerOrSurfaceIndex(self_transform); // this index is used in next measure step // Set lf, lr - double point_ratio = 0.2; // under steered if smaller than 0.5 - lf_ = bounding_box_.length * point_ratio; - lr_ = bounding_box_.length * (1.0 - point_ratio); + lf_ = bounding_box_.length * 0.3; // 30% front from the center + lr_ = bounding_box_.length * 0.25; // 25% rear from the center } bool BigVehicleTracker::predict(const rclcpp::Time & time) { const double dt = (time - last_update_time_).seconds(); - bool ret = predict(dt, ekf_); + if (dt < 0.0) { + RCLCPP_WARN(logger_, "dt is negative. (%f)", dt); + return false; + } + // if dt is too large, shorten dt and repeat prediction + const double dt_max = 0.11; + const uint32_t repeat = std::ceil(dt / dt_max); + const double dt_ = dt / repeat; + bool ret = false; + for (uint32_t i = 0; i < repeat; ++i) { + ret = predict(dt_, ekf_); + if (!ret) { + return false; + } + } if (ret) { last_update_time_ = time; } @@ -169,69 +188,116 @@ bool BigVehicleTracker::predict(const rclcpp::Time & time) bool BigVehicleTracker::predict(const double dt, KalmanFilter & ekf) const { - /* == Nonlinear model == static bicycle model + /* static bicycle model (constant slip angle, constant velocity) * - * x_{k+1} = x_k + vx_k * cos(yaw_k + slip_k) * dt - * y_{k+1} = y_k + vx_k * sin(yaw_k + slip_k) * dt - * yaw_{k+1} = yaw_k + vx_k / l_r * sin(slip_k) * dt - * vx_{k+1} = vx_k + * w_k = vel_k * sin(slip_k) / l_r + * x_{k+1} = x_k + vel_k*cos(yaw_k+slip_k)*dt - 0.5*w_k*vel_k*sin(yaw_k+slip_k)*dt*dt + * y_{k+1} = y_k + vel_k*sin(yaw_k+slip_k)*dt + 0.5*w_k*vel_k*cos(yaw_k+slip_k)*dt*dt + * yaw_{k+1} = yaw_k + w_k * dt + * vel_{k+1} = vel_k * slip_{k+1} = slip_k * */ - /* == Linearized model == + /* Jacobian Matrix + * + * A = [ 1, 0, -vel*sin(yaw+slip)*dt - 0.5*w_k*vel_k*cos(yaw+slip)*dt*dt, + * cos(yaw+slip)*dt - w*sin(yaw+slip)*dt*dt, + * -vel*sin(yaw+slip)*dt - 0.5*vel*vel/l_r*(cos(slip)sin(yaw+slip)+sin(slip)cos(yaw+slip))*dt*dt ] + * [ 0, 1, vel*cos(yaw+slip)*dt - 0.5*w_k*vel_k*sin(yaw+slip)*dt*dt, + * sin(yaw+slip)*dt + w*cos(yaw+slip)*dt*dt, + * vel*cos(yaw+slip)*dt + 0.5*vel*vel/l_r*(cos(slip)cos(yaw+slip)-sin(slip)sin(yaw+slip))*dt*dt ] + * [ 0, 0, 1, 1/l_r*sin(slip)*dt, vel/l_r*cos(slip)*dt] + * [ 0, 0, 0, 1, 0] + * [ 0, 0, 0, 0, 1] * - * A = [ 1, 0, -vx*sin(yaw+slip)*dt, cos(yaw+slip)*dt, -vx*sin(yaw+slip)*dt] - * [ 0, 1, vx*cos(yaw+slip)*dt, sin(yaw+slip)*dt, vx*cos(yaw+slip)*dt] - * [ 0, 0, 1, 1/l_r*sin(slip)*dt, vx/l_r*cos(slip)*dt] - * [ 0, 0, 0, 1, 0] - * [ 0, 0, 0, 0, 1] */ - // X t + // Current state vector X t Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); // predicted state ekf.getX(X_t); const double cos_yaw = std::cos(X_t(IDX::YAW) + X_t(IDX::SLIP)); const double sin_yaw = std::sin(X_t(IDX::YAW) + X_t(IDX::SLIP)); + const double vel = X_t(IDX::VEL); const double cos_slip = std::cos(X_t(IDX::SLIP)); const double sin_slip = std::sin(X_t(IDX::SLIP)); - const double vx = X_t(IDX::VX); - const double sin_2yaw = std::sin(2.0f * X_t(IDX::YAW)); - - // X t+1 - Eigen::MatrixXd X_next_t(ekf_params_.dim_x, 1); // predicted state - X_next_t(IDX::X) = X_t(IDX::X) + vx * cos_yaw * dt; // dx = v * cos(yaw) - X_next_t(IDX::Y) = X_t(IDX::Y) + vx * sin_yaw * dt; // dy = v * sin(yaw) - X_next_t(IDX::YAW) = X_t(IDX::YAW) + vx / lr_ * sin_slip * dt; // dyaw = omega - X_next_t(IDX::VX) = X_t(IDX::VX); - X_next_t(IDX::SLIP) = X_t(IDX::SLIP); - // A + double w = vel * sin_slip / lr_; + const double sin_2yaw = std::sin(2.0f * X_t(IDX::YAW)); + const double w_dtdt = w * dt * dt; + const double vv_dtdt__lr = vel * vel * dt * dt / lr_; + + // Predict state vector X t+1 + Eigen::MatrixXd X_next_t(ekf_params_.dim_x, 1); // predicted state + X_next_t(IDX::X) = + X_t(IDX::X) + vel * cos_yaw * dt - 0.5 * vel * sin_slip * w_dtdt; // dx = v * cos(yaw) * dt + X_next_t(IDX::Y) = + X_t(IDX::Y) + vel * sin_yaw * dt + 0.5 * vel * cos_slip * w_dtdt; // dy = v * sin(yaw) * dt + X_next_t(IDX::YAW) = X_t(IDX::YAW) + w * dt; // d(yaw) = w * dt + X_next_t(IDX::VEL) = X_t(IDX::VEL); + X_next_t(IDX::SLIP) = X_t(IDX::SLIP); // slip_angle = asin(lr * w / v) + + // State transition matrix A Eigen::MatrixXd A = Eigen::MatrixXd::Identity(ekf_params_.dim_x, ekf_params_.dim_x); - A(IDX::X, IDX::YAW) = -vx * sin_yaw * dt; - A(IDX::X, IDX::VX) = cos_yaw * dt; - A(IDX::X, IDX::SLIP) = -vx * sin_yaw * dt; - A(IDX::Y, IDX::YAW) = vx * cos_yaw * dt; - A(IDX::Y, IDX::VX) = sin_yaw * dt; - A(IDX::Y, IDX::SLIP) = vx * cos_yaw * dt; - A(IDX::YAW, IDX::VX) = 1.0 / lr_ * sin_slip * dt; - A(IDX::YAW, IDX::SLIP) = vx / lr_ * cos_slip * dt; - - // Q + A(IDX::X, IDX::YAW) = -vel * sin_yaw * dt - 0.5 * vel * cos_yaw * w_dtdt; + A(IDX::X, IDX::VEL) = cos_yaw * dt - sin_yaw * w_dtdt; + A(IDX::X, IDX::SLIP) = + -vel * sin_yaw * dt - 0.5 * (cos_slip * sin_yaw + sin_slip * cos_yaw) * vv_dtdt__lr; + A(IDX::Y, IDX::YAW) = vel * cos_yaw * dt - 0.5 * vel * sin_yaw * w_dtdt; + A(IDX::Y, IDX::VEL) = sin_yaw * dt + cos_yaw * w_dtdt; + A(IDX::Y, IDX::SLIP) = + vel * cos_yaw * dt + 0.5 * (cos_slip * cos_yaw - sin_slip * sin_yaw) * vv_dtdt__lr; + A(IDX::YAW, IDX::VEL) = 1.0 / lr_ * sin_slip * dt; + A(IDX::YAW, IDX::SLIP) = vel / lr_ * cos_slip * dt; + + // Process noise covariance Q + float q_stddev_yaw_rate{0.0}; + if (vel <= 0.01) { + q_stddev_yaw_rate = ekf_params_.q_stddev_yaw_rate_min; + } else { + // uncertainty of the yaw rate is limited by + // centripetal acceleration a_lat : d(yaw)/dt = w = a_lat/v + // or maximum slip angle slip_max : w = v*sin(slip_max)/l_r + q_stddev_yaw_rate = std::min( + ekf_params_.q_stddev_acc_lat / vel, + vel * std::sin(ekf_params_.q_max_slip_angle) / lr_); // [rad/s] + q_stddev_yaw_rate = std::min(q_stddev_yaw_rate, ekf_params_.q_stddev_yaw_rate_max); + q_stddev_yaw_rate = std::max(q_stddev_yaw_rate, ekf_params_.q_stddev_yaw_rate_min); + } + float q_cov_slip_rate{0.0f}; + if (vel <= 0.01) { + q_cov_slip_rate = ekf_params_.q_cov_slip_rate_min; + } else { + // The slip angle rate uncertainty is modeled as follows: + // d(slip)/dt ~ - sin(slip)/v * d(v)/dt + l_r/v * d(w)/dt + // where sin(slip) = w * l_r / v + // d(w)/dt is assumed to be proportional to w (more uncertain when slip is large) + // d(v)/dt and d(w)/t are considered to be uncorrelated + q_cov_slip_rate = + std::pow(ekf_params_.q_stddev_acc_lat * sin_slip / vel, 2) + std::pow(sin_slip * 1.5, 2); + q_cov_slip_rate = std::min(q_cov_slip_rate, ekf_params_.q_cov_slip_rate_max); + q_cov_slip_rate = std::max(q_cov_slip_rate, ekf_params_.q_cov_slip_rate_min); + } + const float q_cov_x = std::pow(0.5 * ekf_params_.q_stddev_acc_long * dt * dt, 2); + const float q_cov_y = std::pow(0.5 * ekf_params_.q_stddev_acc_lat * dt * dt, 2); + const float q_cov_yaw = std::pow(q_stddev_yaw_rate * dt, 2); + const float q_cov_vel = std::pow(ekf_params_.q_stddev_acc_long * dt, 2); + const float q_cov_slip = q_cov_slip_rate * dt * dt; + Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); // Rotate the covariance matrix according to the vehicle yaw // because q_cov_x and y are in the vehicle coordinate system. - Q(IDX::X, IDX::X) = - (ekf_params_.q_cov_x * cos_yaw * cos_yaw + ekf_params_.q_cov_y * sin_yaw * sin_yaw) * dt * dt; - Q(IDX::X, IDX::Y) = (0.5f * (ekf_params_.q_cov_x - ekf_params_.q_cov_y) * sin_2yaw) * dt * dt; - Q(IDX::Y, IDX::Y) = - (ekf_params_.q_cov_x * sin_yaw * sin_yaw + ekf_params_.q_cov_y * cos_yaw * cos_yaw) * dt * dt; + Q(IDX::X, IDX::X) = (q_cov_x * cos_yaw * cos_yaw + q_cov_y * sin_yaw * sin_yaw); + Q(IDX::X, IDX::Y) = (0.5f * (q_cov_x - q_cov_y) * sin_2yaw); + Q(IDX::Y, IDX::Y) = (q_cov_x * sin_yaw * sin_yaw + q_cov_y * cos_yaw * cos_yaw); Q(IDX::Y, IDX::X) = Q(IDX::X, IDX::Y); - Q(IDX::YAW, IDX::YAW) = ekf_params_.q_cov_yaw * dt * dt; - Q(IDX::VX, IDX::VX) = ekf_params_.q_cov_vx * dt * dt; - Q(IDX::SLIP, IDX::SLIP) = ekf_params_.q_cov_slip * dt * dt; - Eigen::MatrixXd B = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); - Eigen::MatrixXd u = Eigen::MatrixXd::Zero(ekf_params_.dim_x, 1); + Q(IDX::YAW, IDX::YAW) = q_cov_yaw; + Q(IDX::VEL, IDX::VEL) = q_cov_vel; + Q(IDX::SLIP, IDX::SLIP) = q_cov_slip; + + // control-input model B and control-input u are not used + // Eigen::MatrixXd B = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); + // Eigen::MatrixXd u = Eigen::MatrixXd::Zero(ekf_params_.dim_x, 1); if (!ekf.predict(X_next_t, A, Q)) { RCLCPP_WARN(logger_, "Cannot predict"); @@ -249,14 +315,14 @@ bool BigVehicleTracker::measureWithPose( float r_cov_y; const uint8_t label = object_recognition_utils::getHighestProbLabel(object.classification); - if (label == Label::CAR) { - constexpr float r_stddev_x = 8.0; // [m] - constexpr float r_stddev_y = 0.8; // [m] - r_cov_x = std::pow(r_stddev_x, 2.0); - r_cov_y = std::pow(r_stddev_y, 2.0); - } else if (utils::isLargeVehicleLabel(label)) { + if (utils::isLargeVehicleLabel(label)) { r_cov_x = ekf_params_.r_cov_x; r_cov_y = ekf_params_.r_cov_y; + } else if (label == Label::CAR) { + constexpr float r_stddev_x = 2.0; // [m] + constexpr float r_stddev_y = 2.0; // [m] + r_cov_x = std::pow(r_stddev_x, 2.0); + r_cov_y = std::pow(r_stddev_y, 2.0); } else { r_cov_x = ekf_params_.r_cov_x; r_cov_y = ekf_params_.r_cov_y; @@ -267,16 +333,16 @@ bool BigVehicleTracker::measureWithPose( if (object.kinematics.has_twist) { Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); // predicted state ekf_.getX(X_t); - const double predicted_vx = X_t(IDX::VX); - const double observed_vx = object.kinematics.twist_with_covariance.twist.linear.x; + const double predicted_vel = X_t(IDX::VEL); + const double observed_vel = object.kinematics.twist_with_covariance.twist.linear.x; - if (std::fabs(predicted_vx - observed_vx) < velocity_deviation_threshold_) { + if (std::fabs(predicted_vel - observed_vel) < velocity_deviation_threshold_) { // Velocity deviation is small enable_velocity_measurement = true; } } - // pos x, pos y, yaw, vx depending on pose measurement + // pos x, pos y, yaw, vel depending on pose measurement const int dim_y = enable_velocity_measurement ? 4 : 3; double measurement_yaw = getMeasurementYaw(object); // get sign-solved yaw angle Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); // predicted state @@ -296,7 +362,7 @@ bool BigVehicleTracker::measureWithPose( last_input_bounding_box_.width, last_input_bounding_box_.length, last_nearest_corner_index_, bbox_object, X_t(IDX::YAW), offset_object, tracking_offset_); - /* Set measurement matrix */ + /* Set measurement matrix and noise covariance*/ Eigen::MatrixXd Y(dim_y, 1); Eigen::MatrixXd C = Eigen::MatrixXd::Zero(dim_y, ekf_params_.dim_x); Eigen::MatrixXd R = Eigen::MatrixXd::Zero(dim_y, dim_y); @@ -330,31 +396,32 @@ bool BigVehicleTracker::measureWithPose( R(2, 2) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_YAW]; } + // Update the velocity when necessary if (dim_y == 4) { - Y(IDX::VX, 0) = object.kinematics.twist_with_covariance.twist.linear.x; - C(3, IDX::VX) = 1.0; // for vx + Y(IDX::VEL, 0) = object.kinematics.twist_with_covariance.twist.linear.x; + C(3, IDX::VEL) = 1.0; // for vel if (!object.kinematics.has_twist_covariance) { - R(3, 3) = ekf_params_.r_cov_vx; // vx -vx + R(3, 3) = ekf_params_.r_cov_vel; // vel -vel } else { R(3, 3) = object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; } } - /* ekf tracks constant tracking point */ + // ekf update if (!ekf_.update(Y, C, R)) { RCLCPP_WARN(logger_, "Cannot update"); } - // normalize yaw and limit vx, slip + // normalize yaw and limit vel, slip { Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); Eigen::MatrixXd P_t(ekf_params_.dim_x, ekf_params_.dim_x); ekf_.getX(X_t); ekf_.getP(P_t); X_t(IDX::YAW) = tier4_autoware_utils::normalizeRadian(X_t(IDX::YAW)); - if (!(-max_vx_ <= X_t(IDX::VX) && X_t(IDX::VX) <= max_vx_)) { - X_t(IDX::VX) = X_t(IDX::VX) < 0 ? -max_vx_ : max_vx_; + if (!(-max_vel_ <= X_t(IDX::VEL) && X_t(IDX::VEL) <= max_vel_)) { + X_t(IDX::VEL) = X_t(IDX::VEL) < 0 ? -max_vel_ : max_vel_; } if (!(-max_slip_ <= X_t(IDX::SLIP) && X_t(IDX::SLIP) <= max_slip_)) { X_t(IDX::SLIP) = X_t(IDX::SLIP) < 0 ? -max_slip_ : max_slip_; @@ -372,9 +439,8 @@ bool BigVehicleTracker::measureWithPose( bool BigVehicleTracker::measureWithShape( const autoware_auto_perception_msgs::msg::DetectedObject & object) { + // if the input shape is convex type, convert it to bbox type autoware_auto_perception_msgs::msg::DetectedObject bbox_object; - - // if input is convex shape convert it to bbox shape if (object.shape.type != autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { utils::convertConvexHullToBoundingBox(object, bbox_object); } else { @@ -389,6 +455,11 @@ bool BigVehicleTracker::measureWithShape( gain * bounding_box_.height + (1.0 - gain) * bbox_object.shape.dimensions.z; last_input_bounding_box_ = { bbox_object.shape.dimensions.x, bbox_object.shape.dimensions.y, bbox_object.shape.dimensions.z}; + + // update lf, lr + lf_ = bounding_box_.length * 0.3; // 30% front from the center + lr_ = bounding_box_.length * 0.25; // 25% rear from the center + return true; } @@ -480,23 +551,44 @@ bool BigVehicleTracker::getTrackedObject( pose_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = P(IDX::YAW, IDX::YAW); // twist - twist_with_cov.twist.linear.x = X_t(IDX::VX) * std::cos(X_t(IDX::SLIP)); - twist_with_cov.twist.linear.y = X_t(IDX::VX) * std::sin(X_t(IDX::SLIP)); + twist_with_cov.twist.linear.x = X_t(IDX::VEL) * std::cos(X_t(IDX::SLIP)); + twist_with_cov.twist.linear.y = X_t(IDX::VEL) * std::sin(X_t(IDX::SLIP)); twist_with_cov.twist.angular.z = - X_t(IDX::VX) / lr_ * std::sin(X_t(IDX::SLIP)); // yaw_rate = vx_k / l_r * sin(slip_k) - // twist covariance - constexpr double vy_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative + X_t(IDX::VEL) * std::sin(X_t(IDX::SLIP)) / lr_; // yaw_rate = vel_k * sin(slip_k) / l_r + /* twist covariance + * convert covariance from velocity, slip angle to vx, vy, and yaw angle + * + * vx = vel * cos(slip) + * vy = vel * sin(slip) + * wz = vel * sin(slip) / l_r = vy / l_r + * + */ + constexpr double vz_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative constexpr double wx_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative constexpr double wy_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - twist_with_cov.covariance[utils::MSG_COV_IDX::X_X] = P(IDX::VX, IDX::VX); - twist_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = vy_cov; + + Eigen::MatrixXd cov_jacob(3, 2); + cov_jacob << std::cos(X_t(IDX::SLIP)), -X_t(IDX::VEL) * std::sin(X_t(IDX::SLIP)), + std::sin(X_t(IDX::SLIP)), X_t(IDX::VEL) * std::cos(X_t(IDX::SLIP)), + std::sin(X_t(IDX::SLIP)) / lr_, X_t(IDX::VEL) * std::cos(X_t(IDX::SLIP)) / lr_; + Eigen::MatrixXd cov_twist(2, 2); + cov_twist << P(IDX::VEL, IDX::VEL), P(IDX::VEL, IDX::SLIP), P(IDX::SLIP, IDX::VEL), + P(IDX::SLIP, IDX::SLIP); + Eigen::MatrixXd twist_cov_mat = cov_jacob * cov_twist * cov_jacob.transpose(); + + twist_with_cov.covariance[utils::MSG_COV_IDX::X_X] = twist_cov_mat(0, 0); + twist_with_cov.covariance[utils::MSG_COV_IDX::X_Y] = twist_cov_mat(0, 1); + twist_with_cov.covariance[utils::MSG_COV_IDX::X_YAW] = twist_cov_mat(0, 2); + twist_with_cov.covariance[utils::MSG_COV_IDX::Y_X] = twist_cov_mat(1, 0); + twist_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = twist_cov_mat(1, 1); + twist_with_cov.covariance[utils::MSG_COV_IDX::Y_YAW] = twist_cov_mat(1, 2); + twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_X] = twist_cov_mat(2, 0); + twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_Y] = twist_cov_mat(2, 1); + twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = twist_cov_mat(2, 2); twist_with_cov.covariance[utils::MSG_COV_IDX::Z_Z] = vz_cov; - twist_with_cov.covariance[utils::MSG_COV_IDX::X_YAW] = P(IDX::VX, IDX::SLIP); - twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_X] = P(IDX::SLIP, IDX::VX); twist_with_cov.covariance[utils::MSG_COV_IDX::ROLL_ROLL] = wx_cov; twist_with_cov.covariance[utils::MSG_COV_IDX::PITCH_PITCH] = wy_cov; - twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = P(IDX::SLIP, IDX::SLIP); // set shape object.shape.dimensions.x = bounding_box_.length; @@ -506,7 +598,6 @@ bool BigVehicleTracker::getTrackedObject( const auto ekf_pose_yaw = tf2::getYaw(pose_with_cov.pose.orientation); object.shape.footprint = tier4_autoware_utils::rotatePolygon(object.shape.footprint, origin_yaw - ekf_pose_yaw); - return true; } diff --git a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp index aa3f7b1c30d01..fcf72360b84c2 100644 --- a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp @@ -16,6 +16,14 @@ // Author: v1.0 Yukihiro Saito // +#include "multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp" + +#include "multi_object_tracker/utils/utils.hpp" + +#include +#include +#include + #include #include #include @@ -26,17 +34,11 @@ #else #include #endif - -#define EIGEN_MPL2_ONLY -#include "multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp" -#include "multi_object_tracker/utils/utils.hpp" #include "object_recognition_utils/object_recognition_utils.hpp" +#define EIGEN_MPL2_ONLY #include #include -#include -#include -#include using Label = autoware_auto_perception_msgs::msg::ObjectClassification; @@ -51,52 +53,59 @@ NormalVehicleTracker::NormalVehicleTracker( { object_ = object; - // initialize params - float q_stddev_x = 1.0; // object coordinate [m/s] - float q_stddev_y = 1.0; // object coordinate [m/s] - float q_stddev_yaw = tier4_autoware_utils::deg2rad(20); // map coordinate[rad/s] - float q_stddev_vx = tier4_autoware_utils::kmph2mps(10); // object coordinate [m/(s*s)] - float q_stddev_slip = tier4_autoware_utils::deg2rad(5); // object coordinate [rad/(s*s)] - float r_stddev_x = 1.0; // object coordinate [m] - float r_stddev_y = 0.3; // object coordinate [m] - float r_stddev_yaw = tier4_autoware_utils::deg2rad(30); // map coordinate [rad] - float r_stddev_vx = 1.0; // object coordinate [m/s] - float p0_stddev_x = 1.0; // object coordinate [m/s] - float p0_stddev_y = 0.3; // object coordinate [m/s] - float p0_stddev_yaw = tier4_autoware_utils::deg2rad(30); // map coordinate [rad] - float p0_stddev_vx = tier4_autoware_utils::kmph2mps(1000); // object coordinate [m/s] - float p0_stddev_slip = tier4_autoware_utils::deg2rad(10); // object coordinate [rad/s] - ekf_params_.q_cov_x = std::pow(q_stddev_x, 2.0); - ekf_params_.q_cov_y = std::pow(q_stddev_y, 2.0); - ekf_params_.q_cov_yaw = std::pow(q_stddev_yaw, 2.0); - ekf_params_.q_cov_vx = std::pow(q_stddev_vx, 2.0); - ekf_params_.q_cov_slip = std::pow(q_stddev_slip, 2.0); + // Initialize parameters + // measurement noise covariance: detector uncertainty + ego vehicle motion uncertainty + float r_stddev_x = 0.5; // in vehicle coordinate [m] + float r_stddev_y = 0.4; // in vehicle coordinate [m] + float r_stddev_yaw = tier4_autoware_utils::deg2rad(20); // in map coordinate [rad] + float r_stddev_vel = 1.0; // in object coordinate [m/s] ekf_params_.r_cov_x = std::pow(r_stddev_x, 2.0); ekf_params_.r_cov_y = std::pow(r_stddev_y, 2.0); ekf_params_.r_cov_yaw = std::pow(r_stddev_yaw, 2.0); - ekf_params_.r_cov_vx = std::pow(r_stddev_vx, 2.0); + ekf_params_.r_cov_vel = std::pow(r_stddev_vel, 2.0); + // initial state covariance + float p0_stddev_x = 1.0; // in object coordinate [m] + float p0_stddev_y = 0.3; // in object coordinate [m] + float p0_stddev_yaw = tier4_autoware_utils::deg2rad(25); // in map coordinate [rad] + float p0_stddev_vel = tier4_autoware_utils::kmph2mps(1000); // in object coordinate [m/s] + float p0_stddev_slip = tier4_autoware_utils::deg2rad(5); // in object coordinate [rad/s] ekf_params_.p0_cov_x = std::pow(p0_stddev_x, 2.0); ekf_params_.p0_cov_y = std::pow(p0_stddev_y, 2.0); ekf_params_.p0_cov_yaw = std::pow(p0_stddev_yaw, 2.0); - ekf_params_.p0_cov_vx = std::pow(p0_stddev_vx, 2.0); + ekf_params_.p0_cov_vel = std::pow(p0_stddev_vel, 2.0); ekf_params_.p0_cov_slip = std::pow(p0_stddev_slip, 2.0); - max_vx_ = tier4_autoware_utils::kmph2mps(100); // [m/s] - max_slip_ = tier4_autoware_utils::deg2rad(30); // [rad/s] + // process noise covariance + ekf_params_.q_stddev_acc_long = 9.8 * 0.35; // [m/(s*s)] uncertain longitudinal acceleration + ekf_params_.q_stddev_acc_lat = 9.8 * 0.15; // [m/(s*s)] uncertain lateral acceleration + ekf_params_.q_stddev_yaw_rate_min = + tier4_autoware_utils::deg2rad(1.5); // [rad/s] uncertain yaw change rate + ekf_params_.q_stddev_yaw_rate_max = + tier4_autoware_utils::deg2rad(15.0); // [rad/s] uncertain yaw change rate + float q_stddev_slip_rate_min = + tier4_autoware_utils::deg2rad(0.3); // [rad/s] uncertain slip angle change rate + float q_stddev_slip_rate_max = + tier4_autoware_utils::deg2rad(10.0); // [rad/s] uncertain slip angle change rate + ekf_params_.q_cov_slip_rate_min = std::pow(q_stddev_slip_rate_min, 2.0); + ekf_params_.q_cov_slip_rate_max = std::pow(q_stddev_slip_rate_max, 2.0); + ekf_params_.q_max_slip_angle = tier4_autoware_utils::deg2rad(30); // [rad] max slip angle + // limitations + max_vel_ = tier4_autoware_utils::kmph2mps(100); // [m/s] + max_slip_ = tier4_autoware_utils::deg2rad(30); // [rad] velocity_deviation_threshold_ = tier4_autoware_utils::kmph2mps(10); // [m/s] - // initialize X matrix + // initialize state vector X Eigen::MatrixXd X(ekf_params_.dim_x, 1); X(IDX::X) = object.kinematics.pose_with_covariance.pose.position.x; X(IDX::Y) = object.kinematics.pose_with_covariance.pose.position.y; X(IDX::YAW) = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); + X(IDX::SLIP) = 0.0; if (object.kinematics.has_twist) { - X(IDX::VX) = object.kinematics.twist_with_covariance.twist.linear.x; + X(IDX::VEL) = object.kinematics.twist_with_covariance.twist.linear.x; } else { - X(IDX::VX) = 0.0; + X(IDX::VEL) = 0.0; } - X(IDX::SLIP) = 0.0; - // initialize P matrix + // initialize state covariance matrix P Eigen::MatrixXd P = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); if (!object.kinematics.has_position_covariance) { const double cos_yaw = std::cos(X(IDX::YAW)); @@ -111,7 +120,7 @@ NormalVehicleTracker::NormalVehicleTracker( ekf_params_.p0_cov_x * sin_yaw * sin_yaw + ekf_params_.p0_cov_y * cos_yaw * cos_yaw; P(IDX::Y, IDX::X) = P(IDX::X, IDX::Y); P(IDX::YAW, IDX::YAW) = ekf_params_.p0_cov_yaw; - P(IDX::VX, IDX::VX) = ekf_params_.p0_cov_vx; + P(IDX::VEL, IDX::VEL) = ekf_params_.p0_cov_vel; P(IDX::SLIP, IDX::SLIP) = ekf_params_.p0_cov_slip; } else { P(IDX::X, IDX::X) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; @@ -121,10 +130,10 @@ NormalVehicleTracker::NormalVehicleTracker( P(IDX::YAW, IDX::YAW) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_YAW]; if (object.kinematics.has_twist_covariance) { - P(IDX::VX, IDX::VX) = + P(IDX::VEL, IDX::VEL) = object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; } else { - P(IDX::VX, IDX::VX) = ekf_params_.p0_cov_vx; + P(IDX::VEL, IDX::VEL) = ekf_params_.p0_cov_vel; } P(IDX::SLIP, IDX::SLIP) = ekf_params_.p0_cov_slip; } @@ -132,8 +141,7 @@ NormalVehicleTracker::NormalVehicleTracker( if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { bounding_box_ = { object.shape.dimensions.x, object.shape.dimensions.y, object.shape.dimensions.z}; - last_input_bounding_box_ = { - object.shape.dimensions.x, object.shape.dimensions.y, object.shape.dimensions.z}; + last_input_bounding_box_ = bounding_box_; } else { // past default value // bounding_box_ = {4.0, 1.7, 2.0}; @@ -142,9 +150,7 @@ NormalVehicleTracker::NormalVehicleTracker( bounding_box_ = { bbox_object.shape.dimensions.x, bbox_object.shape.dimensions.y, bbox_object.shape.dimensions.z}; - last_input_bounding_box_ = { - bbox_object.shape.dimensions.x, bbox_object.shape.dimensions.y, - bbox_object.shape.dimensions.z}; + last_input_bounding_box_ = bounding_box_; } ekf_.init(X, P); @@ -152,15 +158,28 @@ NormalVehicleTracker::NormalVehicleTracker( setNearestCornerOrSurfaceIndex(self_transform); // this index is used in next measure step // Set lf, lr - double point_ratio = 0.2; // under steered if smaller than 0.5 - lf_ = bounding_box_.length * point_ratio; - lr_ = bounding_box_.length * (1.0 - point_ratio); + lf_ = bounding_box_.length * 0.3; // 30% front from the center + lr_ = bounding_box_.length * 0.25; // 25% rear from the center } bool NormalVehicleTracker::predict(const rclcpp::Time & time) { const double dt = (time - last_update_time_).seconds(); - bool ret = predict(dt, ekf_); + if (dt < 0.0) { + RCLCPP_WARN(logger_, "dt is negative. (%f)", dt); + return false; + } + // if dt is too large, shorten dt and repeat prediction + const double dt_max = 0.11; + const uint32_t repeat = std::ceil(dt / dt_max); + const double dt_ = dt / repeat; + bool ret = false; + for (uint32_t i = 0; i < repeat; ++i) { + ret = predict(dt_, ekf_); + if (!ret) { + return false; + } + } if (ret) { last_update_time_ = time; } @@ -169,69 +188,116 @@ bool NormalVehicleTracker::predict(const rclcpp::Time & time) bool NormalVehicleTracker::predict(const double dt, KalmanFilter & ekf) const { - /* == Nonlinear model == static bicycle model + /* static bicycle model (constant slip angle, constant velocity) * - * x_{k+1} = x_k + vx_k * cos(yaw_k + slip_k) * dt - * y_{k+1} = y_k + vx_k * sin(yaw_k + slip_k) * dt - * yaw_{k+1} = yaw_k + vx_k / l_r * sin(slip_k) * dt - * vx_{k+1} = vx_k + * w_k = vel_k * sin(slip_k) / l_r + * x_{k+1} = x_k + vel_k*cos(yaw_k+slip_k)*dt - 0.5*w_k*vel_k*sin(yaw_k+slip_k)*dt*dt + * y_{k+1} = y_k + vel_k*sin(yaw_k+slip_k)*dt + 0.5*w_k*vel_k*cos(yaw_k+slip_k)*dt*dt + * yaw_{k+1} = yaw_k + w_k * dt + * vel_{k+1} = vel_k * slip_{k+1} = slip_k * */ - /* == Linearized model == + /* Jacobian Matrix + * + * A = [ 1, 0, -vel*sin(yaw+slip)*dt - 0.5*w_k*vel_k*cos(yaw+slip)*dt*dt, + * cos(yaw+slip)*dt - w*sin(yaw+slip)*dt*dt, + * -vel*sin(yaw+slip)*dt - 0.5*vel*vel/l_r*(cos(slip)sin(yaw+slip)+sin(slip)cos(yaw+slip))*dt*dt ] + * [ 0, 1, vel*cos(yaw+slip)*dt - 0.5*w_k*vel_k*sin(yaw+slip)*dt*dt, + * sin(yaw+slip)*dt + w*cos(yaw+slip)*dt*dt, + * vel*cos(yaw+slip)*dt + 0.5*vel*vel/l_r*(cos(slip)cos(yaw+slip)-sin(slip)sin(yaw+slip))*dt*dt ] + * [ 0, 0, 1, 1/l_r*sin(slip)*dt, vel/l_r*cos(slip)*dt] + * [ 0, 0, 0, 1, 0] + * [ 0, 0, 0, 0, 1] * - * A = [ 1, 0, -vx*sin(yaw+slip)*dt, cos(yaw+slip)*dt, -vx*sin(yaw+slip)*dt] - * [ 0, 1, vx*cos(yaw+slip)*dt, sin(yaw+slip)*dt, vx*cos(yaw+slip)*dt] - * [ 0, 0, 1, 1/l_r*sin(slip)*dt, vx/l_r*cos(slip)*dt] - * [ 0, 0, 0, 1, 0] - * [ 0, 0, 0, 0, 1] */ - // X t + // Current state vector X t Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); // predicted state ekf.getX(X_t); const double cos_yaw = std::cos(X_t(IDX::YAW) + X_t(IDX::SLIP)); const double sin_yaw = std::sin(X_t(IDX::YAW) + X_t(IDX::SLIP)); + const double vel = X_t(IDX::VEL); const double cos_slip = std::cos(X_t(IDX::SLIP)); const double sin_slip = std::sin(X_t(IDX::SLIP)); - const double vx = X_t(IDX::VX); - const double sin_2yaw = std::sin(2.0f * X_t(IDX::YAW)); - - // X t+1 - Eigen::MatrixXd X_next_t(ekf_params_.dim_x, 1); // predicted state - X_next_t(IDX::X) = X_t(IDX::X) + vx * cos_yaw * dt; // dx = v * cos(yaw) - X_next_t(IDX::Y) = X_t(IDX::Y) + vx * sin_yaw * dt; // dy = v * sin(yaw) - X_next_t(IDX::YAW) = X_t(IDX::YAW) + vx / lr_ * sin_slip * dt; // dyaw = omega - X_next_t(IDX::VX) = X_t(IDX::VX); - X_next_t(IDX::SLIP) = X_t(IDX::SLIP); - // A + double w = vel * sin_slip / lr_; + const double sin_2yaw = std::sin(2.0f * X_t(IDX::YAW)); + const double w_dtdt = w * dt * dt; + const double vv_dtdt__lr = vel * vel * dt * dt / lr_; + + // Predict state vector X t+1 + Eigen::MatrixXd X_next_t(ekf_params_.dim_x, 1); // predicted state + X_next_t(IDX::X) = + X_t(IDX::X) + vel * cos_yaw * dt - 0.5 * vel * sin_slip * w_dtdt; // dx = v * cos(yaw) * dt + X_next_t(IDX::Y) = + X_t(IDX::Y) + vel * sin_yaw * dt + 0.5 * vel * cos_slip * w_dtdt; // dy = v * sin(yaw) * dt + X_next_t(IDX::YAW) = X_t(IDX::YAW) + w * dt; // d(yaw) = w * dt + X_next_t(IDX::VEL) = X_t(IDX::VEL); + X_next_t(IDX::SLIP) = X_t(IDX::SLIP); // slip_angle = asin(lr * w / v) + + // State transition matrix A Eigen::MatrixXd A = Eigen::MatrixXd::Identity(ekf_params_.dim_x, ekf_params_.dim_x); - A(IDX::X, IDX::YAW) = -vx * sin_yaw * dt; - A(IDX::X, IDX::VX) = cos_yaw * dt; - A(IDX::X, IDX::SLIP) = -vx * sin_yaw * dt; - A(IDX::Y, IDX::YAW) = vx * cos_yaw * dt; - A(IDX::Y, IDX::VX) = sin_yaw * dt; - A(IDX::Y, IDX::SLIP) = vx * cos_yaw * dt; - A(IDX::YAW, IDX::VX) = 1.0 / lr_ * sin_slip * dt; - A(IDX::YAW, IDX::SLIP) = vx / lr_ * cos_slip * dt; - - // Q + A(IDX::X, IDX::YAW) = -vel * sin_yaw * dt - 0.5 * vel * cos_yaw * w_dtdt; + A(IDX::X, IDX::VEL) = cos_yaw * dt - sin_yaw * w_dtdt; + A(IDX::X, IDX::SLIP) = + -vel * sin_yaw * dt - 0.5 * (cos_slip * sin_yaw + sin_slip * cos_yaw) * vv_dtdt__lr; + A(IDX::Y, IDX::YAW) = vel * cos_yaw * dt - 0.5 * vel * sin_yaw * w_dtdt; + A(IDX::Y, IDX::VEL) = sin_yaw * dt + cos_yaw * w_dtdt; + A(IDX::Y, IDX::SLIP) = + vel * cos_yaw * dt + 0.5 * (cos_slip * cos_yaw - sin_slip * sin_yaw) * vv_dtdt__lr; + A(IDX::YAW, IDX::VEL) = 1.0 / lr_ * sin_slip * dt; + A(IDX::YAW, IDX::SLIP) = vel / lr_ * cos_slip * dt; + + // Process noise covariance Q + float q_stddev_yaw_rate{0.0}; + if (vel <= 0.01) { + q_stddev_yaw_rate = ekf_params_.q_stddev_yaw_rate_min; + } else { + // uncertainty of the yaw rate is limited by + // centripetal acceleration a_lat : d(yaw)/dt = w = a_lat/v + // or maximum slip angle slip_max : w = v*sin(slip_max)/l_r + q_stddev_yaw_rate = std::min( + ekf_params_.q_stddev_acc_lat / vel, + vel * std::sin(ekf_params_.q_max_slip_angle) / lr_); // [rad/s] + q_stddev_yaw_rate = std::min(q_stddev_yaw_rate, ekf_params_.q_stddev_yaw_rate_max); + q_stddev_yaw_rate = std::max(q_stddev_yaw_rate, ekf_params_.q_stddev_yaw_rate_min); + } + float q_cov_slip_rate{0.0f}; + if (vel <= 0.01) { + q_cov_slip_rate = ekf_params_.q_cov_slip_rate_min; + } else { + // The slip angle rate uncertainty is modeled as follows: + // d(slip)/dt ~ - sin(slip)/v * d(v)/dt + l_r/v * d(w)/dt + // where sin(slip) = w * l_r / v + // d(w)/dt is assumed to be proportional to w (more uncertain when slip is large) + // d(v)/dt and d(w)/t are considered to be uncorrelated + q_cov_slip_rate = + std::pow(ekf_params_.q_stddev_acc_lat * sin_slip / vel, 2) + std::pow(sin_slip * 1.5, 2); + q_cov_slip_rate = std::min(q_cov_slip_rate, ekf_params_.q_cov_slip_rate_max); + q_cov_slip_rate = std::max(q_cov_slip_rate, ekf_params_.q_cov_slip_rate_min); + } + const float q_cov_x = std::pow(0.5 * ekf_params_.q_stddev_acc_long * dt * dt, 2); + const float q_cov_y = std::pow(0.5 * ekf_params_.q_stddev_acc_lat * dt * dt, 2); + const float q_cov_yaw = std::pow(q_stddev_yaw_rate * dt, 2); + const float q_cov_vel = std::pow(ekf_params_.q_stddev_acc_long * dt, 2); + const float q_cov_slip = q_cov_slip_rate * dt * dt; + Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); // Rotate the covariance matrix according to the vehicle yaw // because q_cov_x and y are in the vehicle coordinate system. - Q(IDX::X, IDX::X) = - (ekf_params_.q_cov_x * cos_yaw * cos_yaw + ekf_params_.q_cov_y * sin_yaw * sin_yaw) * dt * dt; - Q(IDX::X, IDX::Y) = (0.5f * (ekf_params_.q_cov_x - ekf_params_.q_cov_y) * sin_2yaw) * dt * dt; - Q(IDX::Y, IDX::Y) = - (ekf_params_.q_cov_x * sin_yaw * sin_yaw + ekf_params_.q_cov_y * cos_yaw * cos_yaw) * dt * dt; + Q(IDX::X, IDX::X) = (q_cov_x * cos_yaw * cos_yaw + q_cov_y * sin_yaw * sin_yaw); + Q(IDX::X, IDX::Y) = (0.5f * (q_cov_x - q_cov_y) * sin_2yaw); + Q(IDX::Y, IDX::Y) = (q_cov_x * sin_yaw * sin_yaw + q_cov_y * cos_yaw * cos_yaw); Q(IDX::Y, IDX::X) = Q(IDX::X, IDX::Y); - Q(IDX::YAW, IDX::YAW) = ekf_params_.q_cov_yaw * dt * dt; - Q(IDX::VX, IDX::VX) = ekf_params_.q_cov_vx * dt * dt; - Q(IDX::SLIP, IDX::SLIP) = ekf_params_.q_cov_slip * dt * dt; - Eigen::MatrixXd B = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); - Eigen::MatrixXd u = Eigen::MatrixXd::Zero(ekf_params_.dim_x, 1); + Q(IDX::YAW, IDX::YAW) = q_cov_yaw; + Q(IDX::VEL, IDX::VEL) = q_cov_vel; + Q(IDX::SLIP, IDX::SLIP) = q_cov_slip; + + // control-input model B and control-input u are not used + // Eigen::MatrixXd B = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); + // Eigen::MatrixXd u = Eigen::MatrixXd::Zero(ekf_params_.dim_x, 1); if (!ekf.predict(X_next_t, A, Q)) { RCLCPP_WARN(logger_, "Cannot predict"); @@ -253,8 +319,8 @@ bool NormalVehicleTracker::measureWithPose( r_cov_x = ekf_params_.r_cov_x; r_cov_y = ekf_params_.r_cov_y; } else if (utils::isLargeVehicleLabel(label)) { - constexpr float r_stddev_x = 8.0; // [m] - constexpr float r_stddev_y = 0.8; // [m] + constexpr float r_stddev_x = 2.0; // [m] + constexpr float r_stddev_y = 2.0; // [m] r_cov_x = std::pow(r_stddev_x, 2.0); r_cov_y = std::pow(r_stddev_y, 2.0); } else { @@ -262,35 +328,25 @@ bool NormalVehicleTracker::measureWithPose( r_cov_y = ekf_params_.r_cov_y; } - // extract current state - Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); // predicted state - ekf_.getX(X_t); - // Decide dimension of measurement vector bool enable_velocity_measurement = false; if (object.kinematics.has_twist) { - const double predicted_vx = X_t(IDX::VX); - const double observed_vx = object.kinematics.twist_with_covariance.twist.linear.x; + Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); // predicted state + ekf_.getX(X_t); + const double predicted_vel = X_t(IDX::VEL); + const double observed_vel = object.kinematics.twist_with_covariance.twist.linear.x; - if (std::fabs(predicted_vx - observed_vx) < velocity_deviation_threshold_) { + if (std::fabs(predicted_vel - observed_vel) < velocity_deviation_threshold_) { // Velocity deviation is small enable_velocity_measurement = true; } } - // pos x, pos y, yaw, vx depending on pose output + // pos x, pos y, yaw, vel depending on pose measurement const int dim_y = enable_velocity_measurement ? 4 : 3; - double measurement_yaw = tier4_autoware_utils::normalizeRadian( - tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation)); - { - // Fixed measurement_yaw to be in the range of +-90 degrees of X_t(IDX::YAW) - while (M_PI_2 <= X_t(IDX::YAW) - measurement_yaw) { - measurement_yaw = measurement_yaw + M_PI; - } - while (M_PI_2 <= measurement_yaw - X_t(IDX::YAW)) { - measurement_yaw = measurement_yaw - M_PI; - } - } + double measurement_yaw = getMeasurementYaw(object); // get sign-solved yaw angle + Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); // predicted state + ekf_.getX(X_t); // convert to boundingbox if input is convex shape autoware_auto_perception_msgs::msg::DetectedObject bbox_object; @@ -342,30 +398,30 @@ bool NormalVehicleTracker::measureWithPose( // Update the velocity when necessary if (dim_y == 4) { - Y(IDX::VX, 0) = object.kinematics.twist_with_covariance.twist.linear.x; - C(3, IDX::VX) = 1.0; // for vx + Y(IDX::VEL, 0) = object.kinematics.twist_with_covariance.twist.linear.x; + C(3, IDX::VEL) = 1.0; // for vel if (!object.kinematics.has_twist_covariance) { - R(3, 3) = ekf_params_.r_cov_vx; // vx -vx + R(3, 3) = ekf_params_.r_cov_vel; // vel -vel } else { R(3, 3) = object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; } } - // ekf update: this tracks tracking point + // ekf update if (!ekf_.update(Y, C, R)) { RCLCPP_WARN(logger_, "Cannot update"); } - // normalize yaw and limit vx, wz + // normalize yaw and limit vel, slip { Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); Eigen::MatrixXd P_t(ekf_params_.dim_x, ekf_params_.dim_x); ekf_.getX(X_t); ekf_.getP(P_t); X_t(IDX::YAW) = tier4_autoware_utils::normalizeRadian(X_t(IDX::YAW)); - if (!(-max_vx_ <= X_t(IDX::VX) && X_t(IDX::VX) <= max_vx_)) { - X_t(IDX::VX) = X_t(IDX::VX) < 0 ? -max_vx_ : max_vx_; + if (!(-max_vel_ <= X_t(IDX::VEL) && X_t(IDX::VEL) <= max_vel_)) { + X_t(IDX::VEL) = X_t(IDX::VEL) < 0 ? -max_vel_ : max_vel_; } if (!(-max_slip_ <= X_t(IDX::SLIP) && X_t(IDX::SLIP) <= max_slip_)) { X_t(IDX::SLIP) = X_t(IDX::SLIP) < 0 ? -max_slip_ : max_slip_; @@ -383,9 +439,8 @@ bool NormalVehicleTracker::measureWithPose( bool NormalVehicleTracker::measureWithShape( const autoware_auto_perception_msgs::msg::DetectedObject & object) { + // if the input shape is convex type, convert it to bbox type autoware_auto_perception_msgs::msg::DetectedObject bbox_object; - - // if input is convex shape convert it to bbox shape if (object.shape.type != autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { utils::convertConvexHullToBoundingBox(object, bbox_object); } else { @@ -400,6 +455,11 @@ bool NormalVehicleTracker::measureWithShape( gain * bounding_box_.height + (1.0 - gain) * bbox_object.shape.dimensions.z; last_input_bounding_box_ = { bbox_object.shape.dimensions.x, bbox_object.shape.dimensions.y, bbox_object.shape.dimensions.z}; + + // update lf, lr + lf_ = bounding_box_.length * 0.3; // 30% front from the center + lr_ = bounding_box_.length * 0.25; // 25% rear from the center + return true; } @@ -422,12 +482,6 @@ bool NormalVehicleTracker::measure( measureWithPose(object); measureWithShape(object); - // refinement - Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); - Eigen::MatrixXd P_t(ekf_params_.dim_x, ekf_params_.dim_x); - ekf_.getX(X_t); - ekf_.getP(P_t); - /* calc nearest corner index*/ setNearestCornerOrSurfaceIndex(self_transform); // this index is used in next measure step @@ -441,7 +495,7 @@ bool NormalVehicleTracker::getTrackedObject( object.object_id = getUUID(); object.classification = getClassification(); - // predict kinematics + // predict state KalmanFilter tmp_ekf_for_no_update = ekf_; const double dt = (time - last_update_time_).seconds(); if (0.001 /*1msec*/ < dt) { @@ -452,6 +506,7 @@ bool NormalVehicleTracker::getTrackedObject( tmp_ekf_for_no_update.getX(X_t); tmp_ekf_for_no_update.getP(P); + /* put predicted pose and twist to output object */ auto & pose_with_cov = object.kinematics.pose_with_covariance; auto & twist_with_cov = object.kinematics.twist_with_covariance; @@ -496,23 +551,44 @@ bool NormalVehicleTracker::getTrackedObject( pose_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = P(IDX::YAW, IDX::YAW); // twist - twist_with_cov.twist.linear.x = X_t(IDX::VX) * std::cos(X_t(IDX::SLIP)); - twist_with_cov.twist.linear.y = X_t(IDX::VX) * std::sin(X_t(IDX::SLIP)); + twist_with_cov.twist.linear.x = X_t(IDX::VEL) * std::cos(X_t(IDX::SLIP)); + twist_with_cov.twist.linear.y = X_t(IDX::VEL) * std::sin(X_t(IDX::SLIP)); twist_with_cov.twist.angular.z = - X_t(IDX::VX) / lr_ * std::sin(X_t(IDX::SLIP)); // yaw_rate = vx_k / l_r * sin(slip_k) - // twist covariance - constexpr double vy_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative + X_t(IDX::VEL) * std::sin(X_t(IDX::SLIP)) / lr_; // yaw_rate = vel_k * sin(slip_k) / l_r + /* twist covariance + * convert covariance from velocity, slip angle to vx, vy, and yaw angle + * + * vx = vel * cos(slip) + * vy = vel * sin(slip) + * wz = vel * sin(slip) / l_r = vy / l_r + * + */ + constexpr double vz_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative constexpr double wx_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative constexpr double wy_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - twist_with_cov.covariance[utils::MSG_COV_IDX::X_X] = P(IDX::VX, IDX::VX); - twist_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = vy_cov; + + Eigen::MatrixXd cov_jacob(3, 2); + cov_jacob << std::cos(X_t(IDX::SLIP)), -X_t(IDX::VEL) * std::sin(X_t(IDX::SLIP)), + std::sin(X_t(IDX::SLIP)), X_t(IDX::VEL) * std::cos(X_t(IDX::SLIP)), + std::sin(X_t(IDX::SLIP)) / lr_, X_t(IDX::VEL) * std::cos(X_t(IDX::SLIP)) / lr_; + Eigen::MatrixXd cov_twist(2, 2); + cov_twist << P(IDX::VEL, IDX::VEL), P(IDX::VEL, IDX::SLIP), P(IDX::SLIP, IDX::VEL), + P(IDX::SLIP, IDX::SLIP); + Eigen::MatrixXd twist_cov_mat = cov_jacob * cov_twist * cov_jacob.transpose(); + + twist_with_cov.covariance[utils::MSG_COV_IDX::X_X] = twist_cov_mat(0, 0); + twist_with_cov.covariance[utils::MSG_COV_IDX::X_Y] = twist_cov_mat(0, 1); + twist_with_cov.covariance[utils::MSG_COV_IDX::X_YAW] = twist_cov_mat(0, 2); + twist_with_cov.covariance[utils::MSG_COV_IDX::Y_X] = twist_cov_mat(1, 0); + twist_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = twist_cov_mat(1, 1); + twist_with_cov.covariance[utils::MSG_COV_IDX::Y_YAW] = twist_cov_mat(1, 2); + twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_X] = twist_cov_mat(2, 0); + twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_Y] = twist_cov_mat(2, 1); + twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = twist_cov_mat(2, 2); twist_with_cov.covariance[utils::MSG_COV_IDX::Z_Z] = vz_cov; - twist_with_cov.covariance[utils::MSG_COV_IDX::X_YAW] = P(IDX::VX, IDX::SLIP); - twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_X] = P(IDX::SLIP, IDX::VX); twist_with_cov.covariance[utils::MSG_COV_IDX::ROLL_ROLL] = wx_cov; twist_with_cov.covariance[utils::MSG_COV_IDX::PITCH_PITCH] = wy_cov; - twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = P(IDX::SLIP, IDX::SLIP); // set shape object.shape.dimensions.x = bounding_box_.length; @@ -534,3 +610,22 @@ void NormalVehicleTracker::setNearestCornerOrSurfaceIndex( X_t(IDX::X), X_t(IDX::Y), X_t(IDX::YAW), bounding_box_.width, bounding_box_.length, self_transform); } + +double NormalVehicleTracker::getMeasurementYaw( + const autoware_auto_perception_msgs::msg::DetectedObject & object) +{ + double measurement_yaw = tier4_autoware_utils::normalizeRadian( + tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation)); + { + Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); + ekf_.getX(X_t); + // Fixed measurement_yaw to be in the range of +-90 degrees of X_t(IDX::YAW) + while (M_PI_2 <= X_t(IDX::YAW) - measurement_yaw) { + measurement_yaw = measurement_yaw + M_PI; + } + while (M_PI_2 <= measurement_yaw - X_t(IDX::YAW)) { + measurement_yaw = measurement_yaw - M_PI; + } + } + return measurement_yaw; +} diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp index 0bb480bfa26b1..a74c3a69d7bce 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp @@ -78,8 +78,6 @@ class AvoidanceModule : public SceneModuleInterface bool canTransitFailureState() override { return false; } - bool canTransitIdleToRunningState() override { return true; } - /** * @brief update RTC status for candidate shift line. * @param candidate path. diff --git a/planning/behavior_path_dynamic_avoidance_module/include/behavior_path_dynamic_avoidance_module/scene.hpp b/planning/behavior_path_dynamic_avoidance_module/include/behavior_path_dynamic_avoidance_module/scene.hpp index a5380b628387d..696df7f7971ea 100644 --- a/planning/behavior_path_dynamic_avoidance_module/include/behavior_path_dynamic_avoidance_module/scene.hpp +++ b/planning/behavior_path_dynamic_avoidance_module/include/behavior_path_dynamic_avoidance_module/scene.hpp @@ -345,7 +345,7 @@ class DynamicAvoidanceModule : public SceneModuleInterface bool canTransitFailureState() override { return false; } - bool canTransitIdleToRunningState() override { return false; } + ModuleStatus setInitState() const override { return ModuleStatus::IDLE; } bool isLabelTargetObstacle(const uint8_t label) const; void updateTargetObjects(); diff --git a/planning/behavior_path_goal_planner_module/README.md b/planning/behavior_path_goal_planner_module/README.md index 6bd2f8b9c79e4..30183d8aea9b8 100644 --- a/planning/behavior_path_goal_planner_module/README.md +++ b/planning/behavior_path_goal_planner_module/README.md @@ -4,7 +4,7 @@ Plan path around the goal. -- Park at the designated goal. +- Arrive at the designated goal. - Modify the goal to avoid obstacles or to pull over at the side of tha lane. ## Design @@ -29,13 +29,13 @@ package goal_planner{ class FreeSpacePullOver {} } - class GoalSeacher {} + class GoalSearcher {} struct GoalCandidates {} struct PullOverPath{} abstract class PullOverPlannerBase {} - abstract class GoalSeacherBase {} + abstract class GoalsearcherBase {} } @@ -62,7 +62,7 @@ package freespace_planning_algorithms ShiftPullOver --|> PullOverPlannerBase GeometricPullOver --|> PullOverPlannerBase FreeSpacePullOver --|> PullOverPlannerBase -GoalSeacher --|> GoalSeacherBase +GoalSearcher --|> GoalSearcherBase DefaultFixedPlanner --|> FixedGoalPlannerBase PathShifter --o ShiftPullOver @@ -71,23 +71,27 @@ AstarSearch --o FreeSpacePullOver RRTStar --o FreeSpacePullOver PullOverPlannerBase --o GoalPlannerModule -GoalSeacherBase --o GoalPlannerModule +GoalSearcherBase --o GoalPlannerModule FixedGoalPlannerBase --o GoalPlannerModule PullOverPath --o PullOverPlannerBase -GoalCandidates --o GoalSeacherBase +GoalCandidates --o GoalSearcherBase @enduml ``` ## start condition -Either one is activated when all conditions are met. - ### fixed_goal_planner -- Route is set with `allow_goal_modification=false` by default. -- ego-vehicle is in the same lane as the goal. +This is a very simple function that plans a smooth path to a specified goal. This function does not require approval and always runs with the other modules. +_NOTE: this planner does not perform the several features described below, such as "goal search", "collision check", "safety check", etc._ + +Executed when both conditions are met. + +- Route is set with `allow_goal_modification=false`. This is the default. +- The goal is set in the normal lane. In other words, it is NOT `road_shoulder`. +- Ego-vehicle exists in the same lane sequence as the goal. If the target path contains a goal, modify the points of the path so that the path and the goal are connected smoothly. This process will change the shape of the path by the distance of `refine_goal_search_radius_range` from the goal. Note that this logic depends on the interpolation algorithm that will be executed in a later module (at the moment it uses spline interpolation), so it needs to be updated in the future. @@ -129,61 +133,78 @@ If the target path contains a goal, modify the points of the path so that the pa | th_stopped_time | [s] | double | time threshold for arrival of path termination | 2.0 | | center_line_path_interval | [m] | double | reference center line path point interval | 1.0 | -## **collision check** +## **Goal Search** -### **occupancy grid based collision check** +To realize pull over even when an obstacle exists near the original goal, a collision free area is searched within a certain range around the original goal. The goal found will be published as `/planning/scenario_planning/modified_goal`. -Generate footprints from ego-vehicle path points and determine obstacle collision from the value of occupancy_grid of the corresponding cell. +[goal search video](https://user-images.githubusercontent.com/39142679/188359594-c6724e3e-1cb7-4051-9a18-8d2c67d4dee9.mp4) -#### Parameters for occupancy grid based collision check +1. The original goal is set, and the refined goal pose is obtained by moving in the direction normal to the lane center line and keeping `margin_from_boundary` from the edge of the lane. + ![refined_goal](./images/goal_planner-refined_goal.drawio.svg) -| Name | Unit | Type | Description | Default value | -| :---------------------------------------------- | :--- | :----- | :-------------------------------------------------------------------------------------------------------------- | :------------ | -| use_occupancy_grid_for_goal_search | [-] | bool | flag whether to use occupancy grid for goal search collision check | true | -| use_occupancy_grid_for_goal_longitudinal_margin | [-] | bool | flag whether to use occupancy grid for keeping longitudinal margin | false | -| use_occupancy_grid_for_path_collision_check | [-] | bool | flag whether to use occupancy grid for collision check | false | -| occupancy_grid_collision_check_margin | [m] | double | margin to calculate ego-vehicle cells from footprint. | 0.0 | -| theta_size | [-] | int | size of theta angle to be considered. angular resolution for collision check will be 2$\pi$ / theta_size [rad]. | 360 | -| obstacle_threshold | [-] | int | threshold of cell values to be considered as obstacles | 60 | +2. Using `refined_goal` as the base goal, search for candidate goals in the range of `-forward_goal_search_length` to `backward_goal_search_length` in the longitudinal direction and `longitudinal_margin` to `longitudinal_margin+max_lateral_offset` in th lateral direction based on refined_goal. + ![goal_candidates](./images/goal_planner-goal_candidates.drawio.svg) -### **object recognition based collision check** +3. Each candidate goal is prioritized and a path is generated for each planner for each goal. The priority of a candidate goal is determined by its distance from the base goal. The ego vehicle tries to park for the highest possible goal. The distance is determined by the selected policy. In case `minimum_longitudinal_distance`, sort with smaller longitudinal distances taking precedence over smaller lateral distances. In case `minimum_weighted_distance`, sort with the sum of weighted lateral distance and longitudinal distance. This means the distance is calculated by `longitudinal_distance + lateral_cost*lateral_distance` + ![goal_distance](./images/goal_planner-goal_distance.drawio.svg) + The following figure is an example of minimum_weighted_distance.​ The white number indicates the goal candidate priority, and the smaller the number, the higher the priority. the 0 goal indicates the base goal. + ![goal_priority_rviz_with_goal](./images/goal_priority_with_goal.png) + ![goal_priority_rviz](./images/goal_priority_rviz.png) -#### Parameters for object recognition based collision check +4. If the footprint in each goal candidate is within `object_recognition_collision_check_margin` of that of the object, it is determined to be unsafe. These goals are not selected. If `use_occupancy_grid_for_goal_search` is enabled, collision detection on the grid is also performed with `occupancy_grid_collision_check_margin`. -| Name | Unit | Type | Description | Default value | -| :----------------------------------------------------------- | :--- | :----- | :------------------------------------------------------------------------------------------------------- | :------------ | -| use_object_recognition | [-] | bool | flag whether to use object recognition for collision check | true | -| object_recognition_collision_check_margin | [m] | double | margin to calculate ego-vehicle cells from footprint. | 0.6 | -| object_recognition_collision_check_max_extra_stopping_margin | [m] | double | maximum value when adding longitudinal distance margin for collision check considering stopping distance | 1.0 | -| detection_bound_offset | [m] | double | expand pull over lane with this offset to make detection area for collision check of path generation | 15.0 | +Red goals candidates in the image indicate unsafe ones. -## **Goal Search** +![is_safe](./images/goal_planner-is_safe.drawio.svg) -If it is not possible to park safely at a given goal, `/planning/scenario_planning/modified_goal` is -searched for in certain range of the shoulder lane. +It is possible to keep `longitudinal_margin` in the longitudinal direction apart from the collision margin for obstacles from the goal candidate. This is intended to provide natural spacing for parking and efficient departure. -[goal search video](https://user-images.githubusercontent.com/39142679/188359594-c6724e3e-1cb7-4051-9a18-8d2c67d4dee9.mp4) +![longitudinal_margin](./images/goal_planner-longitudinal_margin.drawio.svg) + +Also, if `prioritize_goals_before_objects` is enabled, To arrive at each goal, the number of objects that need to be avoided in the target range is counted, and those with the lowest number are given priority. + +The images represent a count of objects to be avoided at each range, with priority given to those with the lowest number, regardless of the aforementioned distances. + +![object_to_avoid](./images/goal_planner-object_to_avoid.drawio.svg) + +The gray numbers represent objects to avoid, and you can see that the goal in front has a higher priority in this case. + +![goal_priority_object_to_avoid_rviz.png](./images/goal_priority_object_to_avoid_rviz.png) ### Parameters for goal search -| Name | Unit | Type | Description | Default value | -| :------------------------------ | :--- | :----- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------ | :-------------------------- | -| goal_priority | [-] | string | In case `minimum_weighted_distance`, sort with smaller longitudinal distances taking precedence over smaller lateral distances. In case `minimum_longitudinal_distance`, sort with weighted lateral distance against longitudinal distance. | `minimum_weighted_distance` | -| prioritize_goals_before_objects | [-] | bool | If there are objects that may need to be avoided, prioritize the goal in front of them | true | -| forward_goal_search_length | [m] | double | length of forward range to be explored from the original goal | 20.0 | -| backward_goal_search_length | [m] | double | length of backward range to be explored from the original goal | 20.0 | -| goal_search_interval | [m] | double | distance interval for goal search | 2.0 | -| longitudinal_margin | [m] | double | margin between ego-vehicle at the goal position and obstacles | 3.0 | -| max_lateral_offset | [m] | double | maximum offset of goal search in the lateral direction | 0.5 | -| lateral_offset_interval | [m] | double | distance interval of goal search in the lateral direction | 0.25 | -| ignore_distance_from_lane_start | [m] | double | distance from start of pull over lanes for ignoring goal candidates | 0.0 | -| ignore_distance_from_lane_start | [m] | double | distance from start of pull over lanes for ignoring goal candidates | 0.0 | -| margin_from_boundary | [m] | double | distance margin from edge of the shoulder lane | 0.5 | +| Name | Unit | Type | Description | Default value | +| :------------------------------ | :--- | :----- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------ | :-------------------------- | +| goal_priority | [-] | string | In case `minimum_longitudinal_distance`, sort with smaller longitudinal distances taking precedence over smaller lateral distances. In case `minimum_weighted_distance`, sort with the sum of weighted lateral distance and longitudinal distance | `minimum_weighted_distance` | +| lateral_weight | [-] | double | Weight for lateral distance used when `minimum_weighted_distance` | 40.0 | +| prioritize_goals_before_objects | [-] | bool | If there are objects that may need to be avoided, prioritize the goal in front of them | true | +| forward_goal_search_length | [m] | double | length of forward range to be explored from the original goal | 20.0 | +| backward_goal_search_length | [m] | double | length of backward range to be explored from the original goal | 20.0 | +| goal_search_interval | [m] | double | distance interval for goal search | 2.0 | +| longitudinal_margin | [m] | double | margin between ego-vehicle at the goal position and obstacles | 3.0 | +| max_lateral_offset | [m] | double | maximum offset of goal search in the lateral direction | 0.5 | +| lateral_offset_interval | [m] | double | distance interval of goal search in the lateral direction | 0.25 | +| ignore_distance_from_lane_start | [m] | double | distance from start of pull over lanes for ignoring goal candidates | 0.0 | +| ignore_distance_from_lane_start | [m] | double | distance from start of pull over lanes for ignoring goal candidates | 0.0 | +| margin_from_boundary | [m] | double | distance margin from edge of the shoulder lane | 0.5 | ## **Pull Over** There are three path generation methods. -The path is generated with a certain margin (default: `0.5 m`) from the boundary of shoulder lane. +The path is generated with a certain margin (default: `0.75 m`) from the boundary of shoulder lane. + +The process is time consuming because multiple planners are used to generate path for each candidate goal. Therefore, in this module, the path generation is performed in a thread different from the main thread. +Path generation is performed at the timing when the shape of the output path of the previous module changes. If a new module launches, it is expected to go to the previous stage before the goal planner, in which case the goal planner re-generates the path. The goal planner is expected to run at the end of multiple modules, which is achieved by `keep_last` in the planner manager. + +Threads in the goal planner are shown below. + +![threads.png](./images/goal_planner-threads.drawio.svg) + +The main thread will be the one called from the planner manager flow. + +- The goal candidate generation and path candidate generation are done in a separate thread(lane path generation thread). +- The path candidates generated there are referred to by the main thread, and the one judged to be valid for the current planner data (e.g. ego and object information) is selected from among them. valid means no sudden deceleration, no collision with obstacles, etc. The selected path will be the output of this module. +- If there is no path selected, or if the selected path is collision and ego is stuck, a separate thread(freespace path generation thread) will generate a path using freespace planning algorithm. If a valid free space path is found, it will be the output of the module. If the object moves and the pull over path generated along the lane is collision-free, the path is used as output again. See also the section on freespace parking for more information on the flow of generating freespace paths. | Name | Unit | Type | Description | Default value | | :------------------------------- | :----- | :----- | :----------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :--------------------------------------- | @@ -276,12 +297,6 @@ Simultaneous execution with `avoidance_module` in the flowchart is under develop -#### Unimplemented parts / limitations for freespace parking - -- When a short path is generated, the ego does can not drive with it. -- Complex cases take longer to generate or fail. -- The drivable area is not guaranteed to fit in the parking_lot. - #### Parameters freespace parking | Name | Unit | Type | Description | Default value | @@ -289,3 +304,125 @@ Simultaneous execution with `avoidance_module` in the flowchart is under develop | enable_freespace_parking | [-] | bool | This flag enables freespace parking, which runs when the vehicle is stuck due to e.g. obstacles in the parking area. | true | See [freespace_planner](../freespace_planner/README.md) for other parameters. + +## **collision check for path generation** + +To select a safe one from the path candidates, a collision check with obstacles is performed. + +### **occupancy grid based collision check** + +Generate footprints from ego-vehicle path points and determine obstacle collision from the value of occupancy_grid of the corresponding cell. + +#### Parameters for occupancy grid based collision check + +| Name | Unit | Type | Description | Default value | +| :---------------------------------------------- | :--- | :----- | :-------------------------------------------------------------------------------------------------------------- | :------------ | +| use_occupancy_grid_for_goal_search | [-] | bool | flag whether to use occupancy grid for goal search collision check | true | +| use_occupancy_grid_for_goal_longitudinal_margin | [-] | bool | flag whether to use occupancy grid for keeping longitudinal margin | false | +| use_occupancy_grid_for_path_collision_check | [-] | bool | flag whether to use occupancy grid for collision check | false | +| occupancy_grid_collision_check_margin | [m] | double | margin to calculate ego-vehicle cells from footprint. | 0.0 | +| theta_size | [-] | int | size of theta angle to be considered. angular resolution for collision check will be 2$\pi$ / theta_size [rad]. | 360 | +| obstacle_threshold | [-] | int | threshold of cell values to be considered as obstacles | 60 | + +### **object recognition based collision check** + +A collision decision is made for each of the path candidates, and a collision-free path is selected. +There are three main margins at this point. + +- `object_recognition_collision_check_margin` is margin in all directions of ego. +- In the forward direction, a margin is added by the braking distance calculated from the current speed and maximum deceleration. The maximum distance is The maximum value of the distance is suppressed by the `object_recognition_collision_check_max_extra_stopping_margin` +- In curves, the lateral margin is larger than in straight lines.This is because curves are more prone to control errors or to fear when close to objects (The maximum value is limited by `object_recognition_collision_check_max_extra_stopping_margin`, although it has no basis.) + +![collision_check_margin](./images/goal_planner-collision_check_margin.drawio.svg) + +Then there is the concept of soft and hard margins. Although not currently parameterized, if a collision-free path can be generated by a margin several times larger than `object_recognition_collision_check_margin`, then the priority is higher. + +#### Parameters for object recognition based collision check + +| Name | Unit | Type | Description | Default value | +| :----------------------------------------------------------- | :--- | :----- | :------------------------------------------------------------------------------------------------------- | :------------ | +| use_object_recognition | [-] | bool | flag whether to use object recognition for collision check | true | +| object_recognition_collision_check_margin | [m] | double | margin to calculate ego-vehicle cells from footprint. | 0.6 | +| object_recognition_collision_check_max_extra_stopping_margin | [m] | double | maximum value when adding longitudinal distance margin for collision check considering stopping distance | 1.0 | +| detection_bound_offset | [m] | double | expand pull over lane with this offset to make detection area for collision check of path generation | 15.0 | + +## **safety check** + +Perform safety checks on moving objects. If the object is determined to be dangerous, no path decision is made and no approval is given, + +- path decision is not made and approval is not granted. +- After approval, the ego vehicle stops under deceleration and jerk constraints. + +This module has two methods of safety check, `RSS` and `integral_predicted_polygon`. + +`RSS` method is a method commonly used by other behavior path planner modules, see [RSS based safety check utils explanation](../behavior_path_planner_common/docs/behavior_path_planner_safety_check.md). + +`integral_predicted_polygon` is a more safety-oriented method. This method is implemented because speeds during pull over are lower than during driving, and fewer objects travel along the edge of the lane. (It is sometimes too reactive and may be less available.) +This method integrates the footprints of egos and objects at a given time and checks for collisions between them. + +![safety_check](./images/goal_planner-safety_check.drawio.svg) + +In addition, the safety check has a time hysteresis, and if the path is judged "safe" for a certain period of time(`keep_unsafe_time`), it is finally treated as "safe". + +```txt + 　　 ==== is_safe + 　　 ---- current_is_safe + 　　 is_safe + 　　 ^ + 　　 | + 　　 | time + 　　 1 +--+ +---+ +---========= +--+ + 　　 | | | | | | | | + 　　 | | | | | | | | + 　　 | | | | | | | | + 　　 | | | | | | | | + 　　 0 =========================-------==========--> t +``` + +### Parameters for safety check + +| Name | Unit | Type | Description | Default value | +| :----------------------- | :--- | :----- | :------------------------------------------------------------------------------------------------------- | :--------------------------- | +| enable_safety_check | [-] | bool | flag whether to use safety check | true | +| method | [-] | string | method for safety check. `RSS` or `integral_predicted_polygon` | `integral_predicted_polygon` | +| keep_unsafe_time | [s] | double | safety check Hysteresis time. if the path is judged "safe" for the time it is finally treated as "safe". | 3.0 | +| check_all_predicted_path | - | bool | Flag to check all predicted paths | true | +| publish_debug_marker | - | bool | Flag to publish debug markers | false | + +#### Parameters for RSS safety check + +| Name | Unit | Type | Description | Default value | +| :---------------------------------- | :--- | :----- | :-------------------------------------- | :------------ | +| rear_vehicle_reaction_time | [s] | double | Reaction time for rear vehicles | 2.0 | +| rear_vehicle_safety_time_margin | [s] | double | Safety time margin for rear vehicles | 1.0 | +| lateral_distance_max_threshold | [m] | double | Maximum lateral distance threshold | 2.0 | +| longitudinal_distance_min_threshold | [m] | double | Minimum longitudinal distance threshold | 3.0 | +| longitudinal_velocity_delta_time | [s] | double | Delta time for longitudinal velocity | 0.8 | + +#### Parameters for integral_predicted_polygon safety check + +| Name | Unit | Type | Description | Default value | +| :-------------- | :--- | :----- | :------------------------------------- | :------------ | +| forward_margin | [m] | double | forward margin for ego footprint | 1.0 | +| backward_margin | [m] | double | backward margin for ego footprint | 1.0 | +| lat_margin | [m] | double | lateral margin for ego footprint | 1.0 | +| time_horizon | [s] | double | Time width to integrate each footprint | 10.0 | + +## **path deciding** + +When `decide_path_distance` closer to the start of the pull over, if it is collision-free at that time and safe for the predicted path of the objects, it transitions to DECIDING. If it is safe for a certain period of time, it moves to DECIDED. + +![path_deciding](./images/goal_planner-deciding_path.drawio.svg) + +## Unimplemented parts / limitations + +- Only shift pull over can be executed concurrently with other modules +- Parking in tight spots and securing margins are traded off. A mode is needed to reduce the margin by using a slower speed depending on the situation, but there is no mechanism for dynamic switching of speeds. +- Parking space available depends on visibility of objects, and sometimes parking decisions cannot be made properly. +- Margin to unrecognized objects(Not even unknown objects) depends on the occupancy grid. May get too close to unrecognized ground objects because the objects that are allowed to approach (e.g., grass, leaves) are indistinguishable. + +Unimplemented parts / limitations for freespace parking + +- When a short path is generated, the ego does can not drive with it. +- Complex cases take longer to generate or fail. +- The drivable area is not guaranteed to fit in the parking_lot. diff --git a/planning/behavior_path_goal_planner_module/images/goal_planner-collision_check_margin.drawio.svg b/planning/behavior_path_goal_planner_module/images/goal_planner-collision_check_margin.drawio.svg new file mode 100644 index 0000000000000..04b53eadaec04 --- /dev/null +++ b/planning/behavior_path_goal_planner_module/images/goal_planner-collision_check_margin.drawio.svg @@ -0,0 +1,122 @@ + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ collision_check_margin +
+
+
+ + + + + + + + +
+
+
+ extra_stopping_margin(v, max_decel) +
+
+
+ + + + + + + + +
+
+
+ extra_lateral_margin(v, κ) +
+
+
+ + + + + + + + diff --git a/planning/behavior_path_goal_planner_module/images/goal_planner-deciding_path.drawio.svg b/planning/behavior_path_goal_planner_module/images/goal_planner-deciding_path.drawio.svg new file mode 100644 index 0000000000000..38f3c4917c41c --- /dev/null +++ b/planning/behavior_path_goal_planner_module/images/goal_planner-deciding_path.drawio.svg @@ -0,0 +1,121 @@ + + + + + + + + + + + + + +
+
+
+ NOT DECIDED +
+
+
+ + NOT DECIDED + + + + + + + + + + +
+
+
+ DECIDING +
+
+
+ + DECIDING + + + + + + +
+
+
+ DECIDED +
+
+
+ + DECIDED + + + + + + +
+
+
not safe
+
+
+ + not safe + + + + + + +
+
+
+ safe for a certain time +
+
+
+ + safe for a certain ti... + + + + + + +
+
+
safe & close to pull over start point
+
+
+ + safe & close to pull over start poi... + + + + diff --git a/planning/behavior_path_goal_planner_module/images/goal_planner-goal_candidates.drawio.svg b/planning/behavior_path_goal_planner_module/images/goal_planner-goal_candidates.drawio.svg new file mode 100644 index 0000000000000..67b2f89a99bb0 --- /dev/null +++ b/planning/behavior_path_goal_planner_module/images/goal_planner-goal_candidates.drawio.svg @@ -0,0 +1,585 @@ + + + + + + + + + + + + + + +
+
+
+ margin_from_boundary +
+
+
+ + + + + + + + Red Car - Top View image/svg+xml Openclipart Red Car - Top View + 2010-05-19T15:02:12 + + I was thinking of Trophy ( http://trophy.sourceforge.net/index.php?body=screenshots ) when remixing this one :) + http://openclipart.org/detail/61201/red-racing-car-top-view-by-qubodup qubodup + car + clip art clipart game + game sprite + racing racing car red + red car + simple simple car sprite + transport + transportation travel video game + video game art + video game sprite + + + + +
+
+
+ + max_lateral_offset + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ forward_goal_search_length +
+
+
+ + + + + + + + + + + +
+
+
+ backward_goal_search_length +
+
+
+ + + + + + + + +
+
+
+ lateral_search_interval +
+
+
+ + + + + + + + +
+
+
+ + goal_search_interval + +
+
+
+ + + + + + + + + + + + + + +
+
+
+
refined goal
+
+
+
+ + + + + + diff --git a/planning/behavior_path_goal_planner_module/images/goal_planner-goal_distance.drawio.svg b/planning/behavior_path_goal_planner_module/images/goal_planner-goal_distance.drawio.svg new file mode 100644 index 0000000000000..8bca6f4c7e18c --- /dev/null +++ b/planning/behavior_path_goal_planner_module/images/goal_planner-goal_distance.drawio.svg @@ -0,0 +1,466 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Red Car - Top View image/svg+xml Openclipart Red Car - Top View + 2010-05-19T15:02:12 + + I was thinking of Trophy ( http://trophy.sourceforge.net/index.php?body=screenshots ) when remixing this one :) + http://openclipart.org/detail/61201/red-racing-car-top-view-by-qubodup qubodup + car + clip art clipart game + game sprite + racing racing car red + red car + simple simple car sprite + transport + transportation travel video game + video game art + video game sprite + + + + +
+
+
+
refined goal
+
+
+
+ + + + + + + + + + + + + + +
+
+
+
longidudinal distance
+
+
+
+ + + + + + + + +
+
+
+
lateral distance
+
+
+
+ + + + + + diff --git a/planning/behavior_path_goal_planner_module/images/goal_planner-is_safe.drawio.svg b/planning/behavior_path_goal_planner_module/images/goal_planner-is_safe.drawio.svg new file mode 100644 index 0000000000000..d7dab102a7890 --- /dev/null +++ b/planning/behavior_path_goal_planner_module/images/goal_planner-is_safe.drawio.svg @@ -0,0 +1,400 @@ + + + + + + + + + + + + Red Car - Top View image/svg+xml Openclipart Red Car - Top View + 2010-05-19T15:02:12 + + I was thinking of Trophy ( http://trophy.sourceforge.net/index.php?body=screenshots ) when remixing this one :) + http://openclipart.org/detail/61201/red-racing-car-top-view-by-qubodup qubodup + car + clip art clipart game + game sprite + racing racing car red + red car + simple simple car sprite + transport + transportation travel video game + video game art + video game sprite + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/planning/behavior_path_goal_planner_module/images/goal_planner-longitudinal_margin.drawio.svg b/planning/behavior_path_goal_planner_module/images/goal_planner-longitudinal_margin.drawio.svg new file mode 100644 index 0000000000000..66f021bdf887d --- /dev/null +++ b/planning/behavior_path_goal_planner_module/images/goal_planner-longitudinal_margin.drawio.svg @@ -0,0 +1,65 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ longitudinal_margin +
+
+
+ + + + + + diff --git a/planning/behavior_path_goal_planner_module/images/goal_planner-object_to_avoid.drawio.svg b/planning/behavior_path_goal_planner_module/images/goal_planner-object_to_avoid.drawio.svg new file mode 100644 index 0000000000000..b990f3c75dcdf --- /dev/null +++ b/planning/behavior_path_goal_planner_module/images/goal_planner-object_to_avoid.drawio.svg @@ -0,0 +1,472 @@ + + + + + + + + + + + + Red Car - Top View image/svg+xml Openclipart Red Car - Top View + 2010-05-19T15:02:12 + + I was thinking of Trophy ( http://trophy.sourceforge.net/index.php?body=screenshots ) when remixing this one :) + http://openclipart.org/detail/61201/red-racing-car-top-view-by-qubodup qubodup + car + clip art clipart game + game sprite + racing racing car red + red car + simple simple car sprite + transport + transportation travel video game + video game art + video game sprite + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ 0 +
+
+
+ + + + + + + + +
+
+
+ 1 +
+
+
+ + + + + + + + +
+
+
+ 2 +
+
+
+ + + + + + diff --git a/planning/behavior_path_goal_planner_module/images/goal_planner-refined_goal.drawio.svg b/planning/behavior_path_goal_planner_module/images/goal_planner-refined_goal.drawio.svg new file mode 100644 index 0000000000000..5c3f7ddcb2a03 --- /dev/null +++ b/planning/behavior_path_goal_planner_module/images/goal_planner-refined_goal.drawio.svg @@ -0,0 +1,427 @@ + + + + + + + + + + + + + + +
+
+
+ margin_from_boundary +
+
+
+ + + + + + + + + + + + + + + + +
+
+
+
original goal
+
+
+
+ + + + + + + + +
+
+
+
refined goal
+
+
+
+ + + + + Red Car - Top View image/svg+xml Openclipart Red Car - Top View + 2010-05-19T15:02:12 + + I was thinking of Trophy ( http://trophy.sourceforge.net/index.php?body=screenshots ) when remixing this one :) + http://openclipart.org/detail/61201/red-racing-car-top-view-by-qubodup qubodup + car + clip art clipart game + game sprite + racing racing car red + red car + simple simple car sprite + transport + transportation travel video game + video game art + video game sprite + + diff --git a/planning/behavior_path_goal_planner_module/images/goal_planner-safety_check.drawio.svg b/planning/behavior_path_goal_planner_module/images/goal_planner-safety_check.drawio.svg new file mode 100644 index 0000000000000..4fe6313a8da45 --- /dev/null +++ b/planning/behavior_path_goal_planner_module/images/goal_planner-safety_check.drawio.svg @@ -0,0 +1,47 @@ + + + + + + + + + + + + + + + + diff --git a/planning/behavior_path_goal_planner_module/images/goal_planner-threads.drawio.svg b/planning/behavior_path_goal_planner_module/images/goal_planner-threads.drawio.svg new file mode 100644 index 0000000000000..aff3293e5a279 --- /dev/null +++ b/planning/behavior_path_goal_planner_module/images/goal_planner-threads.drawio.svg @@ -0,0 +1,651 @@ + + + + + + + + + + + + + + + +
+
+
+ pull over path candidates +
+
+
+ + + + + + + + + + +
+
+
+ sort paths by priority +
+
+
+ + + + + + + + + + +
+
+
+ + generate goal candidates + +
+
+
+ + + + + + + + + + + + +
+
+
+ select highest priority collision free path +
+
+
+ + + + + + + + +
+
+
run()
+
+
+ + + + + + + + + + +
+
+
+ main thread +
+
+
+ + + + + + + + +
+
+
get
+
+
+ + + + + + + + +
+
+
get
+
+
+ + + + + + + + +
+
+
+ collsion-free path +
+
+
+ + + + + + + + +
+
+
+ collision-free paths +
+
+
+ + + + + + + + +
+
+
+ + collision-free +
+ path is found + +
+
+
+ + + + + + + + +
+
+
+ get +
+
+
+ + + + + + + + + + + + +
+
+
Yes
+
+
+ + + + + + + + + + +
+
+
+ lane path generation thread +
+
+
+ + + + + + + + +
+
+
+ + generate path candidates + +
+
+
+ + + + + + + + + + +
+
+
+ onTimer() +
+
+
+ + + + + + + + +
+
+
+ Trigger: previous module output path  is changed. +
+
+
+ + + + + + + + +
+
+
goal candidates
+
+
+ + + + + + + + + + +
+
+
previous module path
+
+
+ + + + + + + + + + +
+
+
+ freespace path generation thead +
+
+
+ + + + + + + + +
+
+
+ Trigger: ego vehicle is stuck +
+
+
+ + + + + + + + +
+
+
+ + generate freespace paths + +
+
+
+ + + + + + + + + + +
+
+
+ onTimer() +
+
+
+ + + + + + + + +
+
+
freespace path
+
+
+ + + + + + + + + + +
+
+
+ if collision-fee path is found... +
+
+
+ + + + + + + + + + + + +
+
+
No
+
+
+ + + + + + + + diff --git a/planning/behavior_path_goal_planner_module/images/goal_priority_object_to_avoid_rviz.png b/planning/behavior_path_goal_planner_module/images/goal_priority_object_to_avoid_rviz.png new file mode 100644 index 0000000000000..5067e0efc90a8 Binary files /dev/null and b/planning/behavior_path_goal_planner_module/images/goal_priority_object_to_avoid_rviz.png differ diff --git a/planning/behavior_path_goal_planner_module/images/goal_priority_rviz.png b/planning/behavior_path_goal_planner_module/images/goal_priority_rviz.png new file mode 100644 index 0000000000000..dbcd3e1055497 Binary files /dev/null and b/planning/behavior_path_goal_planner_module/images/goal_priority_rviz.png differ diff --git a/planning/behavior_path_goal_planner_module/images/goal_priority_with_goal.png b/planning/behavior_path_goal_planner_module/images/goal_priority_with_goal.png new file mode 100644 index 0000000000000..4094734cb8996 Binary files /dev/null and b/planning/behavior_path_goal_planner_module/images/goal_priority_with_goal.png differ diff --git a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp index 71c309581f251..7350a7767fe01 100644 --- a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp @@ -356,7 +356,6 @@ class GoalPlannerModule : public SceneModuleInterface // If terminating it, it may switch to lane following and could generate an inappropriate path. bool canTransitSuccessState() override { return false; } bool canTransitFailureState() override { return false; } - bool canTransitIdleToRunningState() override { return true; } mutable StartGoalPlannerData goal_planner_data_; diff --git a/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp b/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp index 3eb973dc9841e..c3404bf838a2c 100644 --- a/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp +++ b/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp @@ -1863,6 +1863,9 @@ void GoalPlannerModule::updateSafetyCheckTargetObjectsData( std::pair GoalPlannerModule::isSafePath() const { + if (!thread_safe_data_.get_pull_over_path()) { + return {false, false}; + } const auto pull_over_path = thread_safe_data_.get_pull_over_path()->getCurrentPath(); const auto & current_pose = planner_data_->self_odometry->pose.pose; const double current_velocity = std::hypot( diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/interface.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/interface.hpp index 6a9a8c40c01d1..757ccd3a3116d 100644 --- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/interface.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/interface.hpp @@ -126,7 +126,7 @@ class LaneChangeInterface : public SceneModuleInterface bool canTransitFailureState() override; - bool canTransitIdleToRunningState() override; + ModuleStatus setInitState() const override { return ModuleStatus::WAITING_APPROVAL; }; void updateDebugMarker() const; diff --git a/planning/behavior_path_lane_change_module/src/interface.cpp b/planning/behavior_path_lane_change_module/src/interface.cpp index c8d53d8756cf3..e3763720c2d8a 100644 --- a/planning/behavior_path_lane_change_module/src/interface.cpp +++ b/planning/behavior_path_lane_change_module/src/interface.cpp @@ -297,29 +297,6 @@ bool LaneChangeInterface::canTransitFailureState() return false; } -bool LaneChangeInterface::canTransitIdleToRunningState() -{ - updateDebugMarker(); - - auto log_debug_throttled = [&](std::string_view message) -> void { - RCLCPP_DEBUG(getLogger(), "%s", message.data()); - }; - - log_debug_throttled(__func__); - - if (!isActivated() || isWaitingApproval()) { - if (module_type_->specialRequiredCheck()) { - return true; - } - log_debug_throttled("Module is idling."); - return false; - } - - log_debug_throttled("Can lane change safely. Executing lane change."); - module_type_->toNormalState(); - return true; -} - void LaneChangeInterface::updateDebugMarker() const { if (!parameters_->publish_debug_marker) { diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_interface.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_interface.hpp index 1a8c8241abe1a..73f7448133ee7 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_interface.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_interface.hpp @@ -373,13 +373,10 @@ class SceneModuleInterface RCLCPP_DEBUG(getLogger(), "%s", message.data()); }; if (current_state_ == ModuleStatus::IDLE) { - if (canTransitIdleToRunningState()) { - log_debug_throttled("transiting from IDLE to RUNNING"); - return ModuleStatus::RUNNING; - } - - log_debug_throttled("transiting from IDLE to IDLE"); - return ModuleStatus::IDLE; + auto init_state = setInitState(); + RCLCPP_DEBUG( + getLogger(), "transiting from IDLE to %s", magic_enum::enum_name(init_state).data()); + return init_state; } if (current_state_ == ModuleStatus::RUNNING) { @@ -460,9 +457,9 @@ class SceneModuleInterface virtual bool canTransitFailureState() = 0; /** - * @brief State transition condition IDLE -> RUNNING + * @brief Explicitly set the initial state */ - virtual bool canTransitIdleToRunningState() = 0; + virtual ModuleStatus setInitState() const { return ModuleStatus::RUNNING; } /** * @brief Get candidate path. This information is used for external judgement. diff --git a/planning/behavior_path_side_shift_module/include/behavior_path_side_shift_module/scene.hpp b/planning/behavior_path_side_shift_module/include/behavior_path_side_shift_module/scene.hpp index 7d72afebfb359..1748f57a61bec 100644 --- a/planning/behavior_path_side_shift_module/include/behavior_path_side_shift_module/scene.hpp +++ b/planning/behavior_path_side_shift_module/include/behavior_path_side_shift_module/scene.hpp @@ -75,8 +75,6 @@ class SideShiftModule : public SceneModuleInterface bool canTransitFailureState() override { return false; } - bool canTransitIdleToRunningState() override { return true; } - void initVariables(); // non-const methods diff --git a/planning/behavior_path_start_planner_module/README.md b/planning/behavior_path_start_planner_module/README.md index b26864eb450d5..5280f9c2ad020 100644 --- a/planning/behavior_path_start_planner_module/README.md +++ b/planning/behavior_path_start_planner_module/README.md @@ -2,24 +2,77 @@ ## Purpose / Role -The Start Planner module is designed to generate a path from the current ego position to the driving lane, avoiding static obstacles and stopping in response to dynamic obstacles when a collision is detected. +This module generates and plans a path for safely merging from the shoulder lane or side of road lane into the center of the road lane. -Use cases include: +Specifically, it includes the following features: -- pull out from the side of the road lane to centerline. +- Plan the path to automatically start from the shoulder lane or side of road lane to center of road lane. +- When parked vehicles are present on the shoulder lane, the module generates a path that allows for starting with a gap of a specified margin. +- If a collision with other traffic participants is detected while traveling on the generated path, it will stop as much as possible.
- ![case1](images/start_from_road_side.drawio.svg){width=1000} -
pull out from side of the road lane
+ ![start_planner_module](images/start_planner_example.png){width=1100}
-- pull out from the shoulder lane to the road lane centerline. +## Use Cases + +Give an typical example of how path generation is executed. Showing example of path generation starts from shoulder lane, but also from side of road lane can be generated. + +
+ ![shift_pull_out_from_road_lane](images/shift_pull_out_path_from_road_lane.drawio.svg){width=1100} +
+ +### **Use Case 1: Shift pull out** + +In the shoulder lane, when there are no parked vehicles ahead and the shoulder lane is sufficiently long, a forward shift pull out path (a clothoid curve with consistent jerk) is generated. + +
+ ![shift_pull_out](images/shift_pull_out_path.drawio.svg){width=1100} +
+ +### **Use Case 2: Geometric pull out** + +In the shoulder lane, when there are objects in front and the lane is not sufficiently long behind, a geometric pull out path is generated. + +
+ ![geometric_pull_out](images/geometric_pull_out_path.drawio.svg){width=1100} +
+ +### **Use Case 3: Backward and shift pull out** + +In the shoulder lane, when there are parked vehicles ahead and the lane is sufficiently long behind, a path that involves reversing before generating a forward shift pull out path is created. + +
+ ![shift_pull_out_with_back](images/shift_pull_out_path_with_back.drawio.svg){width=1100} +
+ +### **Use Case 4: Backward and geometric pull out** + +In the shoulder lane, when there is an object ahead and not enough space to reverse sufficiently, a path that involves reversing before making an geometric pull out is generated.
- ![case2](images/start_from_road_shoulder.drawio.svg){width=1000} -
pull out from the shoulder lane
+ ![geometric_pull_out_with_back](images/geometric_pull_out_path_with_back.drawio.svg){width=1100}
+### **Use Case 5: Freespace pull out** + +If the map is annotated with the information that a free space path can be generated in situations where both shift and geometric pull out paths are impossible to create, a path based on the free space algorithm will be generated. + +
+ ![freespace_path](images/freespace_pull_out.png){width=1100} +
+ +**As a note, the patterns for generating these paths are based on default parameters, but as will be explained in the following sections, it is possible to control aspects such as making paths that involve reversing more likely to be generated, or making geometric paths more likely to be generated, by changing the path generation policy or adjusting the margin around static objects.** + +## Limitations + +Here are some notable limitations: + +- If parked vehicles exist in front of, behind, or on both sides of the shoulder, and it's not possible to maintain a specified distance from them, a stopping path will be generated, leading to a potential deadlock. +- In the default settings of the behavior_path_planner, an avoidance module operates in a later stage and attempts to avoid objects. However, it is not guaranteed that the start_planner module will output a path that can successfully navigate around obstacles. This means that if an unavoidable path is generated, it could result in a deadlock. +- The performance of safety check relies on the accuracy of the predicted paths generated by the map_based_prediction node. It's important to note that, currently, predicted paths that consider acceleration are not generated, and paths for low-speed objects may not be accurately produced, which requires caution. +- Given the current specifications of the rule-based algorithms, there is a trade-off between the safety of a path and its naturalness to humans. While it's possible to adjust parameters to manage this trade-off, improvements are necessary to better reconcile these aspects. + ## Design ```plantuml diff --git a/planning/behavior_path_start_planner_module/config/start_planner.param.yaml b/planning/behavior_path_start_planner_module/config/start_planner.param.yaml index 9e5a05a2d60cd..df89427e90fa6 100644 --- a/planning/behavior_path_start_planner_module/config/start_planner.param.yaml +++ b/planning/behavior_path_start_planner_module/config/start_planner.param.yaml @@ -12,6 +12,7 @@ center_line_path_interval: 1.0 # shift pull out enable_shift_pull_out: true + check_shift_path_lane_departure: true shift_collision_check_distance_from_end: -10.0 minimum_shift_pull_out_distance: 0.0 deceleration_interval: 15.0 diff --git a/planning/behavior_path_start_planner_module/images/freespace_pull_out.png b/planning/behavior_path_start_planner_module/images/freespace_pull_out.png new file mode 100644 index 0000000000000..86b95379f9bc1 Binary files /dev/null and b/planning/behavior_path_start_planner_module/images/freespace_pull_out.png differ diff --git a/planning/behavior_path_start_planner_module/images/geometric_pull_out_path.drawio.svg b/planning/behavior_path_start_planner_module/images/geometric_pull_out_path.drawio.svg new file mode 100644 index 0000000000000..295b5f97c1cf0 --- /dev/null +++ b/planning/behavior_path_start_planner_module/images/geometric_pull_out_path.drawio.svg @@ -0,0 +1,48 @@ + + + + + + + + + + + + + + + + + + + diff --git a/planning/behavior_path_start_planner_module/images/geometric_pull_out_path_with_back.drawio.svg b/planning/behavior_path_start_planner_module/images/geometric_pull_out_path_with_back.drawio.svg new file mode 100644 index 0000000000000..37465f8b60cc7 --- /dev/null +++ b/planning/behavior_path_start_planner_module/images/geometric_pull_out_path_with_back.drawio.svg @@ -0,0 +1,57 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/planning/behavior_path_start_planner_module/images/shift_pull_out_path.drawio.svg b/planning/behavior_path_start_planner_module/images/shift_pull_out_path.drawio.svg new file mode 100644 index 0000000000000..a8f16716e6b07 --- /dev/null +++ b/planning/behavior_path_start_planner_module/images/shift_pull_out_path.drawio.svg @@ -0,0 +1,41 @@ + + + + + + + + + + + + + + + + + + + diff --git a/planning/behavior_path_start_planner_module/images/shift_pull_out_path_from_road_lane.drawio.svg b/planning/behavior_path_start_planner_module/images/shift_pull_out_path_from_road_lane.drawio.svg new file mode 100644 index 0000000000000..24fe0ccc5f672 --- /dev/null +++ b/planning/behavior_path_start_planner_module/images/shift_pull_out_path_from_road_lane.drawio.svg @@ -0,0 +1,31 @@ + + + + + + + + + + + + + + + + diff --git a/planning/behavior_path_start_planner_module/images/shift_pull_out_path_with_back.drawio.svg b/planning/behavior_path_start_planner_module/images/shift_pull_out_path_with_back.drawio.svg new file mode 100644 index 0000000000000..cc7223786833a --- /dev/null +++ b/planning/behavior_path_start_planner_module/images/shift_pull_out_path_with_back.drawio.svg @@ -0,0 +1,47 @@ + + + + + + + + + + + + + + + + + + diff --git a/planning/behavior_path_start_planner_module/images/start_from_road_shoulder.drawio.svg b/planning/behavior_path_start_planner_module/images/start_from_road_shoulder.drawio.svg deleted file mode 100644 index 4a70534d42d2e..0000000000000 --- a/planning/behavior_path_start_planner_module/images/start_from_road_shoulder.drawio.svg +++ /dev/null @@ -1,35 +0,0 @@ - - - - - - - - - - - diff --git a/planning/behavior_path_start_planner_module/images/start_from_road_side.drawio.svg b/planning/behavior_path_start_planner_module/images/start_from_road_side.drawio.svg deleted file mode 100644 index 46cdb07150c3a..0000000000000 --- a/planning/behavior_path_start_planner_module/images/start_from_road_side.drawio.svg +++ /dev/null @@ -1,34 +0,0 @@ - - - - - - - - - - diff --git a/planning/behavior_path_start_planner_module/images/start_planner_example.png b/planning/behavior_path_start_planner_module/images/start_planner_example.png new file mode 100644 index 0000000000000..598367c1a6e84 Binary files /dev/null and b/planning/behavior_path_start_planner_module/images/start_planner_example.png differ diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/data_structs.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/data_structs.hpp index f42aef4075777..ecd99393eae2b 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/data_structs.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/data_structs.hpp @@ -46,6 +46,7 @@ struct StartPlannerDebugData std::vector ego_predicted_path; // collision check debug map CollisionCheckDebugMap collision_check; + lanelet::ConstLanelets departure_check_lanes; Pose refined_start_pose; std::vector start_pose_candidates; diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/shift_pull_out.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/shift_pull_out.hpp index fef9a4aa8ebfc..6bf85a4679dfd 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/shift_pull_out.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/shift_pull_out.hpp @@ -50,9 +50,9 @@ class ShiftPullOut : public PullOutPlannerBase ShiftedPath & shifted_path, const Pose & start_pose, const Pose & end_pose, const double longitudinal_acc, const double lateral_acc); - void setDrivableLanes(const lanelet::ConstLanelets & drivable_lanes) + void setDepartureCheckLanes(const lanelet::ConstLanelets & departure_check_lanes) { - drivable_lanes_ = drivable_lanes; + departure_check_lanes_ = departure_check_lanes; } std::shared_ptr lane_departure_checker_; @@ -64,7 +64,7 @@ class ShiftPullOut : public PullOutPlannerBase const double lon_acc, const double shift_time, const double shift_length, const double max_curvature, const double min_distance) const; - lanelet::ConstLanelets drivable_lanes_; + lanelet::ConstLanelets departure_check_lanes_; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp index 90ce99692e57a..e6f5d78a6eb71 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp @@ -139,8 +139,6 @@ class StartPlannerModule : public SceneModuleInterface bool canTransitFailureState() override { return false; } - bool canTransitIdleToRunningState() override { return true; } - /** * @brief init member variables. */ @@ -244,8 +242,8 @@ class StartPlannerModule : public SceneModuleInterface const std::vector & ego_predicted_path) const; bool isSafePath() const; void setDrivableAreaInfo(BehaviorModuleOutput & output) const; - void updateDrivableLanes(); - lanelet::ConstLanelets createDrivableLanes() const; + void updateDepartureCheckLanes(); + lanelet::ConstLanelets createDepartureCheckLanes() const; // check if the goal is located behind the ego in the same route segment. bool isGoalBehindOfEgoInSameRouteSegment() const; diff --git a/planning/behavior_path_start_planner_module/src/shift_pull_out.cpp b/planning/behavior_path_start_planner_module/src/shift_pull_out.cpp index 3b4d08b65923c..8eca8479fbd44 100644 --- a/planning/behavior_path_start_planner_module/src/shift_pull_out.cpp +++ b/planning/behavior_path_start_planner_module/src/shift_pull_out.cpp @@ -88,7 +88,7 @@ std::optional ShiftPullOut::plan(const Pose & start_pose, const Pos const auto transformed_vehicle_footprint = transformVector(vehicle_footprint_, tier4_autoware_utils::pose2transform(pose)); const bool is_out_of_lane = - LaneDepartureChecker::isOutOfLane(drivable_lanes_, transformed_vehicle_footprint); + LaneDepartureChecker::isOutOfLane(departure_check_lanes_, transformed_vehicle_footprint); if (i <= start_segment_idx) { if (!is_out_of_lane) { cropped_path.points.push_back(shift_path.points.at(i)); @@ -100,9 +100,16 @@ std::optional ShiftPullOut::plan(const Pose & start_pose, const Pos shift_path.points = cropped_path.points; // check lane departure + // The method for lane departure checking verifies if the footprint of each point on the path is + // contained within a lanelet using `boost::geometry::within`, which incurs a high computational + // cost. + // TODO(someone): improve the method for detecting lane departures without using + // lanelet::ConstLanelets, making it unnecessary to retain departure_check_lanes_ as a member + // variable. if ( parameters_.check_shift_path_lane_departure && - lane_departure_checker_->checkPathWillLeaveLane(drivable_lanes_, path_shift_start_to_end)) { + lane_departure_checker_->checkPathWillLeaveLane( + departure_check_lanes_, path_shift_start_to_end)) { continue; } diff --git a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp index f5f0e514ce8bc..9e42a02b95219 100644 --- a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp +++ b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp @@ -24,6 +24,7 @@ #include #include #include +#include #include #include @@ -137,7 +138,7 @@ void StartPlannerModule::initVariables() debug_marker_.markers.clear(); initializeSafetyCheckParameters(); initializeCollisionCheckDebugMap(debug_data_.collision_check); - updateDrivableLanes(); + updateDepartureCheckLanes(); } void StartPlannerModule::updateEgoPredictedPathParams( @@ -597,7 +598,7 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval() void StartPlannerModule::resetStatus() { status_ = PullOutStatus{}; - updateDrivableLanes(); + updateDepartureCheckLanes(); } void StartPlannerModule::incrementPathIndex() @@ -1244,8 +1245,12 @@ bool StartPlannerModule::isSafePath() const const double hysteresis_factor = status_.is_safe_dynamic_objects ? 1.0 : safety_check_params_->hysteresis_factor_expand_rate; - behavior_path_planner::updateSafetyCheckDebugData( - debug_data_, filtered_objects, target_objects_on_lane, ego_predicted_path); + // debug + { + debug_data_.filtered_objects = filtered_objects; + debug_data_.target_objects_on_lane = target_objects_on_lane; + debug_data_.ego_predicted_path = ego_predicted_path; + } return behavior_path_planner::utils::path_safety_checker::checkSafetyWithRSS( pull_out_path, ego_predicted_path, target_objects_on_lane.on_current_lane, @@ -1373,19 +1378,23 @@ void StartPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) cons } } -void StartPlannerModule::updateDrivableLanes() +void StartPlannerModule::updateDepartureCheckLanes() { - const auto drivable_lanes = createDrivableLanes(); + const auto departure_check_lanes = createDepartureCheckLanes(); for (auto & planner : start_planners_) { auto shift_pull_out = std::dynamic_pointer_cast(planner); if (shift_pull_out) { - shift_pull_out->setDrivableLanes(drivable_lanes); + shift_pull_out->setDepartureCheckLanes(departure_check_lanes); } } + // debug + { + debug_data_.departure_check_lanes = departure_check_lanes; + } } -lanelet::ConstLanelets StartPlannerModule::createDrivableLanes() const +lanelet::ConstLanelets StartPlannerModule::createDepartureCheckLanes() const { const double backward_path_length = planner_data_->parameters.backward_path_length + parameters_->max_back_distance; @@ -1404,13 +1413,14 @@ lanelet::ConstLanelets StartPlannerModule::createDrivableLanes() const [this](const auto & pull_out_lane) { return planner_data_->route_handler->isShoulderLanelet(pull_out_lane); }); - const auto drivable_lanes = utils::transformToLanelets( + const auto departure_check_lanes = utils::transformToLanelets( utils::generateDrivableLanesWithShoulderLanes(road_lanes, shoulder_lanes)); - return drivable_lanes; + return departure_check_lanes; } void StartPlannerModule::setDebugData() { + using lanelet::visualization::laneletsAsTriangleMarkerArray; using marker_utils::addFootprintMarker; using marker_utils::createFootprintMarkerArray; using marker_utils::createObjectsMarkerArray; @@ -1425,6 +1435,12 @@ void StartPlannerModule::setDebugData() using tier4_autoware_utils::createMarkerScale; using visualization_msgs::msg::Marker; + const auto red_color = createMarkerColor(1.0, 0.0, 0.0, 0.999); + const auto cyan_color = createMarkerColor(0.0, 1.0, 1.0, 0.2); + const auto pink_color = createMarkerColor(1.0, 0.5, 0.5, 0.35); + const auto purple_color = createMarkerColor(1.0, 0.0, 1.0, 0.99); + const auto white_color = createMarkerColor(1.0, 1.0, 1.0, 0.99); + const auto life_time = rclcpp::Duration::from_seconds(1.5); auto add = [&](MarkerArray added) { for (auto & marker : added.markers) { @@ -1456,10 +1472,9 @@ void StartPlannerModule::setDebugData() if (collision_check_end_pose) { add(createPoseMarkerArray( *collision_check_end_pose, "static_collision_check_end_pose", 0, 1.0, 0.0, 0.0)); - auto marker = tier4_autoware_utils::createDefaultMarker( + auto marker = createDefaultMarker( "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "static_collision_check_end_polygon", 0, - Marker::LINE_LIST, tier4_autoware_utils::createMarkerScale(0.1, 0.1, 0.1), - tier4_autoware_utils::createMarkerColor(1.0, 0.0, 0.0, 0.999)); + Marker::LINE_LIST, createMarkerScale(0.1, 0.1, 0.1), red_color); const auto footprint = transformVector( local_footprint, tier4_autoware_utils::pose2transform(*collision_check_end_pose)); const double ego_z = planner_data_->self_odometry->pose.pose.position.z; @@ -1479,13 +1494,13 @@ void StartPlannerModule::setDebugData() { MarkerArray start_pose_footprint_marker_array{}; MarkerArray start_pose_text_marker_array{}; - const auto purple = createMarkerColor(1.0, 0.0, 1.0, 0.99); Marker footprint_marker = createDefaultMarker( "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "start_pose_candidates", 0, Marker::LINE_STRIP, - createMarkerScale(0.2, 0.2, 0.2), purple); + createMarkerScale(0.2, 0.2, 0.2), purple_color); Marker text_marker = createDefaultMarker( "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "start_pose_candidates_idx", 0, - visualization_msgs::msg::Marker::TEXT_VIEW_FACING, createMarkerScale(0.3, 0.3, 0.3), purple); + visualization_msgs::msg::Marker::TEXT_VIEW_FACING, createMarkerScale(0.3, 0.3, 0.3), + purple_color); footprint_marker.lifetime = rclcpp::Duration::from_seconds(1.5); text_marker.lifetime = rclcpp::Duration::from_seconds(1.5); for (size_t i = 0; i < debug_data_.start_pose_candidates.size(); ++i) { @@ -1506,10 +1521,9 @@ void StartPlannerModule::setDebugData() // visualize the footprint from pull_out_start pose to pull_out_end pose along the path { MarkerArray pull_out_path_footprint_marker_array{}; - const auto pink = createMarkerColor(1.0, 0.0, 1.0, 0.99); Marker pull_out_path_footprint_marker = createDefaultMarker( "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "shift_path_footprint", 0, Marker::LINE_STRIP, - createMarkerScale(0.2, 0.2, 0.2), pink); + createMarkerScale(0.2, 0.2, 0.2), pink_color); pull_out_path_footprint_marker.lifetime = rclcpp::Duration::from_seconds(1.5); PathWithLaneId path_shift_start_to_end{}; const auto shift_path = status_.pull_out_path.partial_paths.front(); @@ -1567,8 +1581,7 @@ void StartPlannerModule::setDebugData() const auto header = planner_data_->route_handler->getRouteHeader(); { visualization_msgs::msg::MarkerArray planner_type_marker_array{}; - const auto color = status_.found_pull_out_path ? createMarkerColor(1.0, 1.0, 1.0, 0.99) - : createMarkerColor(1.0, 0.0, 0.0, 0.99); + const auto color = status_.found_pull_out_path ? white_color : red_color; auto marker = createDefaultMarker( header.frame_id, header.stamp, "planner_type", 0, visualization_msgs::msg::Marker::TEXT_VIEW_FACING, createMarkerScale(0.0, 0.0, 1.0), color); @@ -1583,6 +1596,15 @@ void StartPlannerModule::setDebugData() planner_type_marker_array.markers.push_back(marker); add(planner_type_marker_array); } + + add(laneletsAsTriangleMarkerArray( + "departure_check_lanes_for_shift_pull_out_path", debug_data_.departure_check_lanes, + cyan_color)); + + const auto pull_out_lanes = start_planner_utils::getPullOutLanes( + planner_data_, planner_data_->parameters.backward_path_length + parameters_->max_back_distance); + add(laneletsAsTriangleMarkerArray( + "pull_out_lanes_for_static_objects_collision_check", pull_out_lanes, pink_color)); } void StartPlannerModule::logPullOutStatus(rclcpp::Logger::Level log_level) const diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index 3f6136673a44a..3cf243b7893fc 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -735,9 +735,7 @@ void reactRTCApprovalByDecisionResult( path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); } } - if ( - !rtc_occlusion_approved && decision_result.occlusion_stopline_idx && - planner_param.occlusion.enable) { + if (!rtc_occlusion_approved && decision_result.occlusion_stopline_idx) { const auto occlusion_stopline_idx = decision_result.occlusion_stopline_idx.value(); planning_utils::setVelocityFromIndex(occlusion_stopline_idx, 0.0, path); debug_data->occlusion_stop_wall_pose = @@ -814,7 +812,7 @@ void reactRTCApprovalByDecisionResult( path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); } } - if (!rtc_occlusion_approved && planner_param.occlusion.enable) { + if (!rtc_occlusion_approved) { const auto stopline_idx = decision_result.occlusion_stopline_idx; planning_utils::setVelocityFromIndex(stopline_idx, 0.0, path); debug_data->occlusion_stop_wall_pose = @@ -857,7 +855,7 @@ void reactRTCApprovalByDecisionResult( path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); } } - if (!rtc_occlusion_approved && planner_param.occlusion.enable) { + if (!rtc_occlusion_approved) { if (planner_param.occlusion.creep_during_peeking.enable) { const size_t occlusion_peeking_stopline = decision_result.occlusion_stopline_idx; const size_t closest_idx = decision_result.closest_idx; @@ -895,7 +893,7 @@ void reactRTCApprovalByDecisionResult( "PeekingTowardOcclusion, approval = (default: %d, occlusion: %d)", rtc_default_approved, rtc_occlusion_approved); // NOTE: creep_velocity should be inserted first at closest_idx if !rtc_default_approved - if (!rtc_occlusion_approved && planner_param.occlusion.enable) { + if (!rtc_occlusion_approved) { const size_t occlusion_peeking_stopline = decision_result.temporal_stop_before_attention_required ? decision_result.first_attention_stopline_idx @@ -965,7 +963,7 @@ void reactRTCApprovalByDecisionResult( path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); } } - if (!rtc_occlusion_approved && planner_param.occlusion.enable) { + if (!rtc_occlusion_approved) { const auto stopline_idx = decision_result.temporal_stop_before_attention_required ? decision_result.first_attention_stopline_idx : decision_result.occlusion_stopline_idx; @@ -1066,7 +1064,7 @@ void reactRTCApprovalByDecisionResult( path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); } } - if (!rtc_occlusion_approved && planner_param.occlusion.enable) { + if (!rtc_occlusion_approved) { const auto stopline_idx = decision_result.occlusion_stopline_idx; planning_utils::setVelocityFromIndex(stopline_idx, 0.0, path); debug_data->occlusion_stop_wall_pose = @@ -1110,7 +1108,7 @@ void reactRTCApprovalByDecisionResult( path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); } } - if (!rtc_occlusion_approved && planner_param.occlusion.enable) { + if (!rtc_occlusion_approved) { const auto stopline_idx = decision_result.occlusion_stopline_idx; planning_utils::setVelocityFromIndex(stopline_idx, 0.0, path); debug_data->occlusion_stop_wall_pose = diff --git a/sensing/pointcloud_preprocessor/README.md b/sensing/pointcloud_preprocessor/README.md index 91ef34f4cae72..5c6402efdf23d 100644 --- a/sensing/pointcloud_preprocessor/README.md +++ b/sensing/pointcloud_preprocessor/README.md @@ -56,7 +56,45 @@ Detail description of each filter's algorithm is in the following links. ## Assumptions / Known limits -`pointcloud_preprocessor::Filter` is implemented based on pcl_perception [1] because of [this issue](https://github.com/ros-perception/perception_pcl/issues/9). +`pointcloud_preprocessor::Filter` is implemented based on pcl_perception [1] because +of [this issue](https://github.com/ros-perception/perception_pcl/issues/9). + +## Measuring the performance + +In Autoware, point cloud data from each LiDAR sensor undergoes preprocessing in the sensing pipeline before being input +into the perception pipeline. The preprocessing stages are illustrated in the diagram below: + +![pointcloud_preprocess_pipeline.drawio.png](docs%2Fimage%2Fpointcloud_preprocess_pipeline.drawio.png) + +Each stage in the pipeline incurs a processing delay. Mostly, we've used `ros2 topic delay /topic_name` to measure +the time between the message header and the current time. This approach works well for small-sized messages. However, +when dealing with large point cloud messages, this method introduces an additional delay. This is primarily because +accessing these large point cloud messages externally impacts the pipeline's performance. + +Our sensing/perception nodes are designed to run within composable node containers, leveraging intra-process +communication. External subscriptions to these messages (like using ros2 topic delay or rviz2) impose extra delays and +can even slow down the pipeline by subscribing externally. Therefore, these measurements will not be accurate. + +To mitigate this issue, we've adopted a method where each node in the pipeline reports its pipeline latency time. +This approach ensures the integrity of intra-process communication and provides a more accurate measure of delays in the +pipeline. + +### Benchmarking The Pipeline + +The nodes within the pipeline report the pipeline latency time, indicating the duration from the sensor driver's pointcloud +output to the node's output. This data is crucial for assessing the pipeline's health and efficiency. + +When running Autoware, you can monitor the pipeline latency times for each node in the pipeline by subscribing to the +following ROS 2 topics: + +- `/sensing/lidar/LidarX/crop_box_filter_self/debug/pipeline_latency_ms` +- `/sensing/lidar/LidarX/crop_box_filter_mirror/debug/pipeline_latency_ms` +- `/sensing/lidar/LidarX/distortion_corrector/debug/pipeline_latency_ms` +- `/sensing/lidar/LidarX/ring_outlier_filter/debug/pipeline_latency_ms` +- `/sensing/lidar/concatenate_data_synchronizer/debug/sensing/lidar/LidarX/pointcloud/pipeline_latency_ms` + +These topics provide the pipeline latency times, giving insights into the delays at various stages of the pipeline +from the sensor output of LidarX to each subsequent node. ## (Optional) Error detection and handling diff --git a/sensing/pointcloud_preprocessor/docs/image/pointcloud_preprocess_pipeline.drawio.png b/sensing/pointcloud_preprocessor/docs/image/pointcloud_preprocess_pipeline.drawio.png new file mode 100644 index 0000000000000..e690d0179afa6 Binary files /dev/null and b/sensing/pointcloud_preprocessor/docs/image/pointcloud_preprocess_pipeline.drawio.png differ diff --git a/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp b/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp index ff72e433e9602..635e0d1103f89 100644 --- a/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp @@ -380,6 +380,20 @@ void PointCloudConcatenateDataSynchronizerComponent::publish() const auto & transformed_raw_points = PointCloudConcatenateDataSynchronizerComponent::combineClouds(concat_cloud_ptr); + for (const auto & e : cloud_stdmap_) { + if (e.second != nullptr) { + if (debug_publisher_) { + const auto pipeline_latency_ms = + std::chrono::duration( + std::chrono::nanoseconds( + (this->get_clock()->now() - e.second->header.stamp).nanoseconds())) + .count(); + debug_publisher_->publish( + "debug" + e.first + "/pipeline_latency_ms", pipeline_latency_ms); + } + } + } + // publish concatenated pointcloud if (concat_cloud_ptr) { auto output = std::make_unique(*concat_cloud_ptr); diff --git a/sensing/pointcloud_preprocessor/src/crop_box_filter/crop_box_filter_nodelet.cpp b/sensing/pointcloud_preprocessor/src/crop_box_filter/crop_box_filter_nodelet.cpp index 20d1f5c8b3d6d..cfbeffee9982c 100644 --- a/sensing/pointcloud_preprocessor/src/crop_box_filter/crop_box_filter_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/crop_box_filter/crop_box_filter_nodelet.cpp @@ -65,7 +65,7 @@ CropBoxFilterComponent::CropBoxFilterComponent(const rclcpp::NodeOptions & optio using tier4_autoware_utils::DebugPublisher; using tier4_autoware_utils::StopWatch; stop_watch_ptr_ = std::make_unique>(); - debug_publisher_ = std::make_unique(this, "crop_box_filter"); + debug_publisher_ = std::make_unique(this, this->get_name()); stop_watch_ptr_->tic("cyclic_time"); stop_watch_ptr_->tic("processing_time"); } @@ -195,6 +195,14 @@ void CropBoxFilterComponent::faster_filter( "debug/cyclic_time_ms", cyclic_time_ms); debug_publisher_->publish( "debug/processing_time_ms", processing_time_ms); + + auto pipeline_latency_ms = + std::chrono::duration( + std::chrono::nanoseconds((this->get_clock()->now() - input->header.stamp).nanoseconds())) + .count(); + + debug_publisher_->publish( + "debug/pipeline_latency_ms", pipeline_latency_ms); } } diff --git a/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp b/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp index dd38b85a2b56d..d1d91ed7ec439 100644 --- a/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp +++ b/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp @@ -128,6 +128,16 @@ void DistortionCorrectorComponent::onPointCloud(PointCloud2::UniquePtr points_ms undistortPointCloud(tf2_base_link_to_sensor, *points_msg); + if (debug_publisher_) { + auto pipeline_latency_ms = + std::chrono::duration( + std::chrono::nanoseconds( + (this->get_clock()->now() - points_msg->header.stamp).nanoseconds())) + .count(); + debug_publisher_->publish( + "debug/pipeline_latency_ms", pipeline_latency_ms); + } + undistorted_points_pub_->publish(std::move(points_msg)); // add processing time for debug diff --git a/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp b/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp index d2570b9c4d786..d968b06a0dc61 100644 --- a/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp @@ -204,6 +204,14 @@ void RingOutlierFilterComponent::faster_filter( "debug/cyclic_time_ms", cyclic_time_ms); debug_publisher_->publish( "debug/processing_time_ms", processing_time_ms); + + auto pipeline_latency_ms = + std::chrono::duration( + std::chrono::nanoseconds((this->get_clock()->now() - input->header.stamp).nanoseconds())) + .count(); + + debug_publisher_->publish( + "debug/pipeline_latency_ms", pipeline_latency_ms); } }