diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index 7a54fa35a31fb..90f0aa3ef40fe 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -1,160 +1,160 @@ ### Copied from .github/CODEOWNERS-manual ### ### Automatically generated from package.xml ### -control/external_cmd_selector/** kenji.miyake@tier4.jp -control/obstacle_collision_checker/** kenji.miyake@tier4.jp -control/vehicle_cmd_gate/** takamasa.horibe@tier4.jp -control/trajectory_follower/** takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp -control/trajectory_follower_nodes/** takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp -control/joy_controller/** kenji.miyake@tier4.jp -control/lane_departure_checker/** kyoichi.sugahara@tier4.jp -control/shift_decider/** takamasa.horibe@tier4.jp -control/pure_pursuit/** takamasa.horibe@tier4.jp -control/operation_mode_transition_manager/** takamasa.horibe@tier4.jp -control/control_performance_analysis/** ali.boyali@tier4.jp berkay@leodrive.ai -common/autoware_ad_api_msgs/** isamu.takagi@tier4.jp -common/motion_common/** christopherj.ho@gmail.com -common/component_interface_specs/** isamu.takagi@tier4.jp -common/motion_testing/** christopherj.ho@gmail.com +common/autoware_ad_api_specs/** isamu.takagi@tier4.jp +common/autoware_auto_common/** opensource@apex.ai +common/autoware_auto_geometry/** opensource@apex.ai common/autoware_auto_perception_rviz_plugin/** opensource@apex.ai taichi.higashide@tier4.jp common/autoware_auto_tf2/** jit.ray.c@gmail.com -common/signal_processing/** takayuki.murooka@tier4.jp -common/tier4_perception_rviz_plugin/** yukihiro.saito@tier4.jp -common/path_distance_calculator/** isamu.takagi@tier4.jp -common/kalman_filter/** yukihiro.saito@tier4.jp -common/tier4_traffic_light_rviz_plugin/** satoshi.ota@tier4.jp -common/vehicle_constants_manager/** mfc@leodrive.ai +common/autoware_point_types/** taichi.higashide@tier4.jp +common/autoware_testing/** adam.dabrowski@robotec.ai +common/bag_time_manager_rviz_plugin/** taiki.tanaka@tier4.jp +common/component_interface_specs/** isamu.takagi@tier4.jp common/component_interface_utils/** isamu.takagi@tier4.jp -common/tier4_planning_rviz_plugin/** yukihiro.saito@tier4.jp -common/perception_utils/** takayuki.murooka@tier4.jp -common/web_controller/** yukihiro.saito@tier4.jp +common/fake_test_node/** opensource@apex.ai +common/global_parameter_loader/** kenji.miyake@tier4.jp common/goal_distance_calculator/** taiki.tanaka@tier4.jp -common/motion_utils/** yutaka.shimizu@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp +common/grid_map_utils/** maxime.clement@tier4.jp +common/interpolation/** fumiya.watanabe@tier4.jp takayuki.murooka@tier4.jp +common/kalman_filter/** yukihiro.saito@tier4.jp +common/motion_common/** christopherj.ho@gmail.com +common/motion_testing/** christopherj.ho@gmail.com +common/motion_utils/** yutaka.shimizu@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp taiki.tanaka@tier4.jp +common/neural_networks_provider/** ambroise.vincent@arm.com +common/osqp_interface/** maxime.clement@tier4.jp +common/path_distance_calculator/** isamu.takagi@tier4.jp +common/perception_utils/** takayuki.murooka@tier4.jp +common/polar_grid/** yukihiro.saito@tier4.jp +common/signal_processing/** takayuki.murooka@tier4.jp +common/tier4_api_utils/** isamu.takagi@tier4.jp +common/tier4_autoware_utils/** takamasa.horibe@tier4.jp kenji.miyake@tier4.jp +common/tier4_calibration_rviz_plugin/** tomoya.kimura@tier4.jp +common/tier4_control_rviz_plugin/** taiki.tanaka@tier4.jp +common/tier4_datetime_rviz_plugin/** isamu.takagi@tier4.jp common/tier4_debug_tools/** kenji.miyake@tier4.jp common/tier4_localization_rviz_plugin/** isamu.takagi@tier4.jp -common/autoware_point_types/** taichi.higashide@tier4.jp +common/tier4_perception_rviz_plugin/** yukihiro.saito@tier4.jp +common/tier4_planning_rviz_plugin/** yukihiro.saito@tier4.jp common/tier4_screen_capture_rviz_plugin/** taiki.tanaka@tier4.jp -common/tier4_control_rviz_plugin/** taiki.tanaka@tier4.jp -common/bag_time_manager_rviz_plugin/** taiki.tanaka@tier4.jp -common/osqp_interface/** maxime.clement@tier4.jp common/tier4_simulated_clock_rviz_plugin/** maxime.clement@tier4.jp -common/neural_networks_provider/** ambroise.vincent@arm.com -common/global_parameter_loader/** kenji.miyake@tier4.jp -common/autoware_auto_common/** opensource@apex.ai -common/tvm_utility/** ambroise.vincent@arm.com -common/autoware_auto_geometry/** opensource@apex.ai -common/autoware_ad_api_specs/** isamu.takagi@tier4.jp common/tier4_state_rviz_plugin/** hiroki.ota@tier4.jp -common/tier4_datetime_rviz_plugin/** isamu.takagi@tier4.jp -common/grid_map_utils/** maxime.clement@tier4.jp -common/tier4_autoware_utils/** takamasa.horibe@tier4.jp kenji.miyake@tier4.jp -common/fake_test_node/** opensource@apex.ai -common/autoware_testing/** adam.dabrowski@robotec.ai -common/tier4_api_utils/** isamu.takagi@tier4.jp -common/tier4_calibration_rviz_plugin/** tomoya.kimura@tier4.jp -common/interpolation/** fumiya.watanabe@tier4.jp takayuki.murooka@tier4.jp -common/time_utils/** christopherj.ho@gmail.com -common/polar_grid/** yukihiro.saito@tier4.jp +common/tier4_traffic_light_rviz_plugin/** satoshi.ota@tier4.jp common/tier4_vehicle_rviz_plugin/** yukihiro.saito@tier4.jp -launch/tier4_vehicle_launch/** yukihiro.saito@tier4.jp -launch/tier4_system_launch/** kenji.miyake@tier4.jp -launch/tier4_map_launch/** ryohsuke.mitsudome@tier4.jp -launch/tier4_control_launch/** takamasa.horibe@tier4.jp -launch/tier4_sensing_launch/** yukihiro.saito@tier4.jp +common/time_utils/** christopherj.ho@gmail.com +common/trtexec_vendor/** daisuke.nishimatsu@tier4.jp +common/tvm_utility/** ambroise.vincent@arm.com +common/vehicle_constants_manager/** mfc@leodrive.ai +common/web_controller/** yukihiro.saito@tier4.jp +control/control_performance_analysis/** ali.boyali@tier4.jp berkay@leodrive.ai +control/external_cmd_selector/** kenji.miyake@tier4.jp +control/joy_controller/** kenji.miyake@tier4.jp +control/lane_departure_checker/** kyoichi.sugahara@tier4.jp +control/obstacle_collision_checker/** kenji.miyake@tier4.jp +control/operation_mode_transition_manager/** takamasa.horibe@tier4.jp +control/pure_pursuit/** takamasa.horibe@tier4.jp +control/shift_decider/** takamasa.horibe@tier4.jp +control/trajectory_follower/** takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp +control/trajectory_follower_nodes/** takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp +control/vehicle_cmd_gate/** takamasa.horibe@tier4.jp +evaluator/kinematic_evaluator/** dominik.jargot@robotec.ai +evaluator/localization_evaluator/** dominik.jargot@robotec.ai launch/tier4_autoware_api_launch/** isamu.takagi@tier4.jp ryohsuke.mitsudome@tier4.jp +launch/tier4_control_launch/** takamasa.horibe@tier4.jp +launch/tier4_localization_launch/** yamato.ando@tier4.jp +launch/tier4_map_launch/** ryohsuke.mitsudome@tier4.jp launch/tier4_perception_launch/** yukihiro.saito@tier4.jp launch/tier4_planning_launch/** takamasa.horibe@tier4.jp -launch/tier4_localization_launch/** yamato.ando@tier4.jp +launch/tier4_sensing_launch/** yukihiro.saito@tier4.jp launch/tier4_simulator_launch/** keisuke.shima@tier4.jp -evaluator/localization_evaluator/** dominik.jargot@robotec.ai -evaluator/kinematic_evaluator/** dominik.jargot@robotec.ai +launch/tier4_system_launch/** kenji.miyake@tier4.jp fumihito.ito@tier4.jp +launch/tier4_vehicle_launch/** yukihiro.saito@tier4.jp +localization/ekf_localizer/** takamasa.horibe@tier4.jp koji.minoda@tier4.jp yamato.ando@tier4.jp +localization/gyro_odometer/** yamato.ando@tier4.jp +localization/initial_pose_button_panel/** yamato.ando@tier4.jp +localization/localization_error_monitor/** yamato.ando@tier4.jp +localization/ndt/** yamato.ando@tier4.jp +localization/ndt_pcl_modified/** yamato.ando@tier4.jp +localization/ndt_scan_matcher/** yamato.ando@tier4.jp +localization/pose2twist/** yamato.ando@tier4.jp +localization/pose_initializer/** yamato.ando@tier4.jp isamu.takagi@tier4.jp +localization/stop_filter/** koji.minoda@tier4.jp yamato.ando@tier4.jp +localization/twist2accel/** koji.minoda@tier4.jp yamato.ando@tier4.jp map/map_loader/** ryohsuke.mitsudome@tier4.jp kenji.miyake@tier4.jp map/map_tf_generator/** azumi.suzuki@tier4.jp map/util/lanelet2_map_preprocessor/** ryohsuke.mitsudome@tier4.jp -simulator/simple_planning_simulator/** takamasa.horibe@tier4.jp -simulator/dummy_perception_publisher/** yukihiro.saito@tier4.jp -simulator/fault_injection/** keisuke.shima@tier4.jp -tools/simulator_test/simulator_compatibility_test/** shpark@morai.ai -sensing/livox/livox_tag_filter/** kenji.miyake@tier4.jp -sensing/pointcloud_preprocessor/** abrahammonrroy@yahoo.com yukihiro.saito@tier4.jp -sensing/probabilistic_occupancy_grid_map/** yukihiro.saito@tier4.jp -sensing/imu_corrector/** yamato.ando@tier4.jp -sensing/geo_pos_conv/** yamato.ando@tier4.jp -sensing/image_transport_decompressor/** yukihiro.saito@tier4.jp -sensing/vehicle_velocity_converter/** ryu.yamamoto@tier4.jp -sensing/image_diagnostics/** dai.nguyen@tier4.jp -sensing/tier4_pcl_extensions/** ryu.yamamoto@tier4.jp -sensing/gnss_poser/** yamato.ando@tier4.jp -planning/freespace_planner/** takamasa.horibe@tier4.jp -planning/obstacle_cruise_planner/** takayuki.murooka@tier4.jp yutaka.shimizu@tier4.jp -planning/planning_error_monitor/** yutaka.shimizu@tier4.jp -planning/surround_obstacle_checker/** satoshi.ota@tier4.jp -planning/costmap_generator/** kenji.miyake@tier4.jp -planning/behavior_path_planner/** zulfaqar.azmi@tier4.jp kosuke.takeuchi@tier4.jp kosuke.takeuchi@tier4.jp fumiya.watanabe@tier4.jp takamasa.horibe@tier4.jp yutaka.shimizu@tier4.jp takayuki.murooka@tier4.jp -planning/obstacle_stop_planner/** satoshi.ota@tier4.jp -planning/behavior_velocity_planner/** mamoru.sobue@tier4.jp satoshi.ota@tier4.jp kyoichi.sugahara@tier4.jp taiki.tanaka@tier4.jp kosuke.takeuchi@tier4.jp satoshi.ota@tier4.jp mamoru.sobue@tier4.jp yutaka.shimizu@tier4.jp kosuke.takeuchi@tier4.jp tomohito.ando@tier4.jp -planning/rtc_auto_approver/** fumiya.watanabe@tier4.jp -planning/scenario_selector/** kenji.miyake@tier4.jp -planning/planning_evaluator/** maxime.clement@tier4.jp -planning/rtc_interface/** fumiya.watanabe@tier4.jp -planning/planning_debug_tools/** takamasa.horibe@tier4.jp taiki.tanaka@tier4.jp -planning/external_velocity_limit_selector/** satoshi.ota@tier4.jp -planning/freespace_planning_algorithms/** takamasa.horibe@tier4.jp -planning/obstacle_avoidance_planner/** takayuki.murooka@tier4.jp -planning/motion_velocity_smoother/** fumiya.watanabe@tier4.jp -planning/route_handler/** fumiya.watanabe@tier4.jp -planning/mission_planner/** ryohsuke.mitsudome@tier4.jp -perception/shape_estimation/** yukihiro.saito@tier4.jp -perception/front_vehicle_velocity_estimator/** satoshi.tanaka@tier4.jp -perception/traffic_light_visualization/** yukihiro.saito@tier4.jp -perception/detection_by_tracker/** yukihiro.saito@tier4.jp -perception/object_range_splitter/** yukihiro.saito@tier4.jp -perception/heatmap_visualizer/** kotaro.uetake@tier4.jp -perception/detected_object_feature_remover/** tomoya.kimura@tier4.jp +perception/compare_map_segmentation/** abrahammonrroy@yahoo.com yukihiro.saito@tier4.jp perception/crosswalk_traffic_light_estimator/** satoshi.ota@tier4.jp +perception/detected_object_feature_remover/** tomoya.kimura@tier4.jp +perception/detected_object_validation/** yukihiro.saito@tier4.jp +perception/detection_by_tracker/** yukihiro.saito@tier4.jp +perception/elevation_map_loader/** kosuke.takeuchi@tier4.jp taichi.higashide@tier4.jp perception/euclidean_cluster/** yukihiro.saito@tier4.jp +perception/front_vehicle_velocity_estimator/** satoshi.tanaka@tier4.jp +perception/ground_segmentation/** abrahammonrroy@yahoo.com yukihiro.saito@tier4.jp +perception/heatmap_visualizer/** kotaro.uetake@tier4.jp perception/image_projection_based_fusion/** yukihiro.saito@tier4.jp yusuke.muramatsu@tier4.jp -perception/traffic_light_ssd_fine_detector/** daisuke.nishimatsu@tier4.jp +perception/lidar_apollo_instance_segmentation/** yukihiro.saito@tier4.jp perception/lidar_centerpoint/** yusuke.muramatsu@tier4.jp perception/map_based_prediction/** yutaka.shimizu@tier4.jp perception/multi_object_tracker/** yukihiro.saito@tier4.jp jilada.eccleston@tier4.jp perception/object_merger/** yukihiro.saito@tier4.jp -perception/elevation_map_loader/** kosuke.takeuchi@tier4.jp taichi.higashide@tier4.jp -perception/traffic_light_map_based_detector/** yukihiro.saito@tier4.jp -perception/tensorrt_yolo/** daisuke.nishimatsu@tier4.jp -perception/lidar_apollo_instance_segmentation/** yukihiro.saito@tier4.jp +perception/object_range_splitter/** yukihiro.saito@tier4.jp +perception/occupancy_grid_map_outlier_filter/** abrahammonrroy@yahoo.com yukihiro.saito@tier4.jp perception/radar_fusion_to_detected_object/** satoshi.tanaka@tier4.jp perception/radar_tracks_msgs_converter/** satoshi.tanaka@tier4.jp +perception/shape_estimation/** yukihiro.saito@tier4.jp +perception/tensorrt_yolo/** daisuke.nishimatsu@tier4.jp perception/traffic_light_classifier/** yukihiro.saito@tier4.jp -perception/detected_object_validation/** yukihiro.saito@tier4.jp -perception/ground_segmentation/** abrahammonrroy@yahoo.com yukihiro.saito@tier4.jp -perception/occupancy_grid_map_outlier_filter/** abrahammonrroy@yahoo.com yukihiro.saito@tier4.jp -perception/compare_map_segmentation/** abrahammonrroy@yahoo.com yukihiro.saito@tier4.jp -vehicle/steer_offset_estimator/** taiki.tanaka@tier4.jp -vehicle/raw_vehicle_cmd_converter/** takamasa.horibe@tier4.jp -vehicle/vehicle_info_util/** yamato.ando@tier4.jp -vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/** tomoya.kimura@tier4.jp -vehicle/external_cmd_converter/** takamasa.horibe@tier4.jp -localization/pose_initializer/** yamato.ando@tier4.jp -localization/ndt_pcl_modified/** yamato.ando@tier4.jp -localization/pose2twist/** yamato.ando@tier4.jp -localization/ndt_scan_matcher/** yamato.ando@tier4.jp -localization/gyro_odometer/** yamato.ando@tier4.jp -localization/localization_error_monitor/** yamato.ando@tier4.jp -localization/initial_pose_button_panel/** yamato.ando@tier4.jp -localization/twist2accel/** koji.minoda@tier4.jp yamato.ando@tier4.jp -localization/ndt/** yamato.ando@tier4.jp -localization/ekf_localizer/** takamasa.horibe@tier4.jp koji.minoda@tier4.jp yamato.ando@tier4.jp -localization/stop_filter/** koji.minoda@tier4.jp yamato.ando@tier4.jp +perception/traffic_light_map_based_detector/** yukihiro.saito@tier4.jp +perception/traffic_light_ssd_fine_detector/** daisuke.nishimatsu@tier4.jp +perception/traffic_light_visualization/** yukihiro.saito@tier4.jp +planning/behavior_path_planner/** zulfaqar.azmi@tier4.jp kosuke.takeuchi@tier4.jp kosuke.takeuchi@tier4.jp fumiya.watanabe@tier4.jp takamasa.horibe@tier4.jp yutaka.shimizu@tier4.jp takayuki.murooka@tier4.jp taiki.tanaka@tier4.jp tomoya.kimura@tier4.jp shumpei.wakabayashi@tier4.jp +planning/behavior_velocity_planner/** mamoru.sobue@tier4.jp satoshi.ota@tier4.jp kyoichi.sugahara@tier4.jp taiki.tanaka@tier4.jp kosuke.takeuchi@tier4.jp satoshi.ota@tier4.jp mamoru.sobue@tier4.jp yutaka.shimizu@tier4.jp kosuke.takeuchi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp shumpei.wakabayashi@tier4.jp +planning/costmap_generator/** kenji.miyake@tier4.jp +planning/external_velocity_limit_selector/** satoshi.ota@tier4.jp +planning/freespace_planner/** takamasa.horibe@tier4.jp +planning/freespace_planning_algorithms/** takamasa.horibe@tier4.jp +planning/mission_planner/** ryohsuke.mitsudome@tier4.jp +planning/motion_velocity_smoother/** fumiya.watanabe@tier4.jp +planning/obstacle_avoidance_planner/** takayuki.murooka@tier4.jp +planning/obstacle_cruise_planner/** takayuki.murooka@tier4.jp yutaka.shimizu@tier4.jp +planning/obstacle_stop_planner/** satoshi.ota@tier4.jp +planning/planning_debug_tools/** takamasa.horibe@tier4.jp taiki.tanaka@tier4.jp +planning/planning_error_monitor/** yutaka.shimizu@tier4.jp +planning/planning_evaluator/** maxime.clement@tier4.jp +planning/route_handler/** fumiya.watanabe@tier4.jp +planning/rtc_auto_mode_manager/** fumiya.watanabe@tier4.jp +planning/rtc_interface/** fumiya.watanabe@tier4.jp +planning/scenario_selector/** kenji.miyake@tier4.jp +planning/surround_obstacle_checker/** satoshi.ota@tier4.jp +sensing/geo_pos_conv/** yamato.ando@tier4.jp +sensing/gnss_poser/** yamato.ando@tier4.jp +sensing/image_diagnostics/** dai.nguyen@tier4.jp +sensing/image_transport_decompressor/** yukihiro.saito@tier4.jp +sensing/imu_corrector/** yamato.ando@tier4.jp +sensing/livox/livox_tag_filter/** kenji.miyake@tier4.jp +sensing/pointcloud_preprocessor/** abrahammonrroy@yahoo.com yukihiro.saito@tier4.jp +sensing/probabilistic_occupancy_grid_map/** yukihiro.saito@tier4.jp +sensing/tier4_pcl_extensions/** ryu.yamamoto@tier4.jp +sensing/vehicle_velocity_converter/** ryu.yamamoto@tier4.jp +simulator/dummy_perception_publisher/** yukihiro.saito@tier4.jp +simulator/fault_injection/** keisuke.shima@tier4.jp +simulator/simple_planning_simulator/** takamasa.horibe@tier4.jp +system/ad_service_state_monitor/** kenji.miyake@tier4.jp +system/bluetooth_monitor/** fumihito.ito@tier4.jp +system/default_ad_api/** isamu.takagi@tier4.jp system/default_ad_api_helpers/ad_api_adaptors/** isamu.takagi@tier4.jp system/default_ad_api_helpers/automatic_pose_initializer/** isamu.takagi@tier4.jp -system/velodyne_monitor/** fumihito.ito@tier4.jp +system/dummy_diag_publisher/** kenji.miyake@tier4.jp system/dummy_infrastructure/** kenji.miyake@tier4.jp -system/topic_state_monitor/** kenji.miyake@tier4.jp -system/bluetooth_monitor/** fumihito.ito@tier4.jp -system/ad_service_state_monitor/** kenji.miyake@tier4.jp -system/system_monitor/** fumihito.ito@tier4.jp system/emergency_handler/** kenji.miyake@tier4.jp -system/dummy_diag_publisher/** kenji.miyake@tier4.jp -system/system_error_monitor/** kenji.miyake@tier4.jp -system/default_ad_api/** isamu.takagi@tier4.jp +system/system_error_monitor/** kenji.miyake@tier4.jp fumihito.ito@tier4.jp +system/system_monitor/** fumihito.ito@tier4.jp +system/topic_state_monitor/** kenji.miyake@tier4.jp +system/velodyne_monitor/** fumihito.ito@tier4.jp +tools/simulator_test/simulator_compatibility_test/** shpark@morai.ai +vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/** tomoya.kimura@tier4.jp +vehicle/external_cmd_converter/** takamasa.horibe@tier4.jp +vehicle/raw_vehicle_cmd_converter/** takamasa.horibe@tier4.jp +vehicle/steer_offset_estimator/** taiki.tanaka@tier4.jp +vehicle/vehicle_info_util/** yamato.ando@tier4.jp taiki.tanaka@tier4.jp tomoya.kimura@tier4.jp shumpei.wakabayashi@tier4.jp diff --git a/.github/PULL_REQUEST_TEMPLATE/standard-change.md b/.github/PULL_REQUEST_TEMPLATE/standard-change.md index 2df18b2bd013e..cfdf7101b5afd 100644 --- a/.github/PULL_REQUEST_TEMPLATE/standard-change.md +++ b/.github/PULL_REQUEST_TEMPLATE/standard-change.md @@ -4,7 +4,7 @@ ## Related links - + ## Tests performed diff --git a/.github/workflows/build-and-test-differential.yaml b/.github/workflows/build-and-test-differential.yaml index 248dbb09b61a4..f8db906263843 100644 --- a/.github/workflows/build-and-test-differential.yaml +++ b/.github/workflows/build-and-test-differential.yaml @@ -80,7 +80,7 @@ jobs: - name: Get modified files id: get-modified-files - uses: tj-actions/changed-files@v28 + uses: tj-actions/changed-files@v29 with: files: | **/*.cpp diff --git a/build_depends.humble.repos b/build_depends.humble.repos index 7c8f1df6c523c..5e8ce5953172a 100644 --- a/build_depends.humble.repos +++ b/build_depends.humble.repos @@ -12,6 +12,10 @@ repositories: type: git url: https://github.com/autowarefoundation/autoware_msgs.git version: main + core/autoware_adapi_msgs: + type: git + url: https://github.com/autowarefoundation/autoware_adapi_msgs.git + version: main core/external/autoware_auto_msgs: type: git url: https://github.com/tier4/autoware_auto_msgs.git diff --git a/build_depends.repos b/build_depends.repos index b510096aef881..2b47af3ef8e81 100644 --- a/build_depends.repos +++ b/build_depends.repos @@ -12,6 +12,10 @@ repositories: type: git url: https://github.com/autowarefoundation/autoware_msgs.git version: main + core/autoware_adapi_msgs: + type: git + url: https://github.com/autowarefoundation/autoware_adapi_msgs.git + version: main core/external/autoware_auto_msgs: type: git url: https://github.com/tier4/autoware_auto_msgs.git diff --git a/common/autoware_ad_api_msgs/CMakeLists.txt b/common/autoware_ad_api_msgs/CMakeLists.txt deleted file mode 100644 index 2cb112502f676..0000000000000 --- a/common/autoware_ad_api_msgs/CMakeLists.txt +++ /dev/null @@ -1,26 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(autoware_ad_api_msgs) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -rosidl_generate_interfaces(${PROJECT_NAME} - common/msg/ResponseStatus.msg - interface/srv/InterfaceVersion.srv - localization/msg/LocalizationInitializationState.msg - localization/srv/InitializeLocalization.srv - routing/msg/RouteState.msg - routing/msg/Route.msg - routing/msg/RouteData.msg - routing/msg/RoutePrimitive.msg - routing/msg/RouteSegment.msg - routing/srv/ClearRoute.srv - routing/srv/SetRoute.srv - routing/srv/SetRoutePoints.srv - DEPENDENCIES - builtin_interfaces - std_msgs - geometry_msgs -) - -ament_auto_package() diff --git a/common/autoware_ad_api_msgs/README.md b/common/autoware_ad_api_msgs/README.md deleted file mode 100644 index a596e8c5ba8b4..0000000000000 --- a/common/autoware_ad_api_msgs/README.md +++ /dev/null @@ -1,39 +0,0 @@ -# autoware_ad_api_msgs - -## ResponseStatus - -This message is a response status commonly used in the service type API. Each API can define its own status codes. -The status codes are primarily used to indicate the error cause, such as invalid parameter and timeout. -If the API succeeds, set success to true, code to zero, and message to the empty string. -Alternatively, codes and messages can be used for warnings or additional information. -If the API fails, set success to false, code to the related status code, and message to the information. -The status code zero is reserved for success. The status code 50000 or over are also reserved for typical cases. - -| Name | Code | Description | -| ---------- | ----: | ------------------------------------ | -| SUCCESS | 0 | This API has completed successfully. | -| DEPRECATED | 50000 | This API is deprecated. | - -## InterfaceVersion - -Considering the product life cycle, there may be multiple vehicles using different versions of the AD API due to changes in requirements or functional improvements. For example, one vehicle uses `v1` for stability and another vehicle uses `v2` to enable more advanced functionality. - -In that situation, the AD API users such as developers of a web service have to switch the application behavior based on the version that each vehicle uses. -The version of AD API follows [Semantic Versioning][semver] in order to provide an intuitive understanding of the changes between versions. - -## Routing - -The routing service support two formats. One uses pose and the other uses map dependent data directly. -The body part of the route message is optional, since the route does not exist when it is cleared by the service. -[See also routing API][api-routing]. - -## Localization - -The initialization state does not reflect localization errors. Use diagnostics for that purpose. -[See also localization API][api-localization]. - - - -[semver]: https://semver.org/ -[api-localization]: https://autowarefoundation.github.io/autoware-documentation/main/design/autoware-interfaces/ad-api/list/api/localization/ -[api-routing]: https://autowarefoundation.github.io/autoware-documentation/main/design/autoware-interfaces/ad-api/list/api/routing/ diff --git a/common/autoware_ad_api_msgs/common/msg/ResponseStatus.msg b/common/autoware_ad_api_msgs/common/msg/ResponseStatus.msg deleted file mode 100644 index 4fe0e5d4208e3..0000000000000 --- a/common/autoware_ad_api_msgs/common/msg/ResponseStatus.msg +++ /dev/null @@ -1,10 +0,0 @@ -# constants for code -uint16 DEPRECATED = 50000 -uint16 SERVICE_UNREADY = 50001 -uint16 SERVICE_TIMEOUT = 50002 -uint16 TRANSFORM_ERROR = 50003 - -# variables -bool success -uint16 code -string message diff --git a/common/autoware_ad_api_msgs/interface/srv/InterfaceVersion.srv b/common/autoware_ad_api_msgs/interface/srv/InterfaceVersion.srv deleted file mode 100644 index 93ba2680f1859..0000000000000 --- a/common/autoware_ad_api_msgs/interface/srv/InterfaceVersion.srv +++ /dev/null @@ -1,4 +0,0 @@ ---- -uint16 major -uint16 minor -uint16 patch diff --git a/common/autoware_ad_api_msgs/localization/msg/LocalizationInitializationState.msg b/common/autoware_ad_api_msgs/localization/msg/LocalizationInitializationState.msg deleted file mode 100644 index 8a4d790dbc321..0000000000000 --- a/common/autoware_ad_api_msgs/localization/msg/LocalizationInitializationState.msg +++ /dev/null @@ -1,7 +0,0 @@ -uint16 UNKNOWN = 0 -uint16 UNINITIALIZED = 1 -uint16 INITIALIZING = 2 -uint16 INITIALIZED = 3 - -builtin_interfaces/Time stamp -uint16 state diff --git a/common/autoware_ad_api_msgs/localization/srv/InitializeLocalization.srv b/common/autoware_ad_api_msgs/localization/srv/InitializeLocalization.srv deleted file mode 100644 index d3ffaece7789a..0000000000000 --- a/common/autoware_ad_api_msgs/localization/srv/InitializeLocalization.srv +++ /dev/null @@ -1,7 +0,0 @@ -geometry_msgs/PoseWithCovarianceStamped[<=1] pose ---- -uint16 ERROR_UNSAFE = 1 -uint16 ERROR_GNSS_SUPPORT = 2 -uint16 ERROR_GNSS = 3 -uint16 ERROR_ESTIMATION = 4 -autoware_ad_api_msgs/ResponseStatus status diff --git a/common/autoware_ad_api_msgs/package.xml b/common/autoware_ad_api_msgs/package.xml deleted file mode 100644 index db10bd2cdb877..0000000000000 --- a/common/autoware_ad_api_msgs/package.xml +++ /dev/null @@ -1,30 +0,0 @@ - - - - autoware_ad_api_msgs - 0.0.0 - The autoware_ad_api_msgs package - Takagi, Isamu - Apache License 2.0 - - ament_cmake_auto - - autoware_cmake - builtin_interfaces - rosidl_default_generators - - geometry_msgs - std_msgs - - builtin_interfaces - rosidl_default_runtime - - ament_lint_auto - autoware_lint_common - - rosidl_interface_packages - - - ament_cmake - - diff --git a/common/autoware_ad_api_msgs/routing/msg/Route.msg b/common/autoware_ad_api_msgs/routing/msg/Route.msg deleted file mode 100644 index 3e6ff3a06e18b..0000000000000 --- a/common/autoware_ad_api_msgs/routing/msg/Route.msg +++ /dev/null @@ -1,2 +0,0 @@ -std_msgs/Header header -autoware_ad_api_msgs/RouteData[<=1] data diff --git a/common/autoware_ad_api_msgs/routing/msg/RouteData.msg b/common/autoware_ad_api_msgs/routing/msg/RouteData.msg deleted file mode 100644 index e5d03efb14008..0000000000000 --- a/common/autoware_ad_api_msgs/routing/msg/RouteData.msg +++ /dev/null @@ -1,3 +0,0 @@ -geometry_msgs/Pose start -geometry_msgs/Pose goal -autoware_ad_api_msgs/RouteSegment[] segments diff --git a/common/autoware_ad_api_msgs/routing/msg/RoutePrimitive.msg b/common/autoware_ad_api_msgs/routing/msg/RoutePrimitive.msg deleted file mode 100644 index f7d9312611e51..0000000000000 --- a/common/autoware_ad_api_msgs/routing/msg/RoutePrimitive.msg +++ /dev/null @@ -1,2 +0,0 @@ -int64 id -string type # The same id may be used for each type. diff --git a/common/autoware_ad_api_msgs/routing/msg/RouteSegment.msg b/common/autoware_ad_api_msgs/routing/msg/RouteSegment.msg deleted file mode 100644 index a3f3d12d3872d..0000000000000 --- a/common/autoware_ad_api_msgs/routing/msg/RouteSegment.msg +++ /dev/null @@ -1,2 +0,0 @@ -autoware_ad_api_msgs/RoutePrimitive preferred -autoware_ad_api_msgs/RoutePrimitive[] alternatives # Does not include the preferred primitive. diff --git a/common/autoware_ad_api_msgs/routing/msg/RouteState.msg b/common/autoware_ad_api_msgs/routing/msg/RouteState.msg deleted file mode 100644 index 9dfe77ddee175..0000000000000 --- a/common/autoware_ad_api_msgs/routing/msg/RouteState.msg +++ /dev/null @@ -1,8 +0,0 @@ -uint16 UNKNOWN = 0 -uint16 UNSET = 1 -uint16 SET = 2 -uint16 ARRIVED = 3 -uint16 CHANGING = 4 - -builtin_interfaces/Time stamp -uint16 state diff --git a/common/autoware_ad_api_msgs/routing/srv/ClearRoute.srv b/common/autoware_ad_api_msgs/routing/srv/ClearRoute.srv deleted file mode 100644 index 38a3199e9d091..0000000000000 --- a/common/autoware_ad_api_msgs/routing/srv/ClearRoute.srv +++ /dev/null @@ -1,2 +0,0 @@ ---- -autoware_ad_api_msgs/ResponseStatus status diff --git a/common/autoware_ad_api_msgs/routing/srv/SetRoute.srv b/common/autoware_ad_api_msgs/routing/srv/SetRoute.srv deleted file mode 100644 index 7299b12fe8141..0000000000000 --- a/common/autoware_ad_api_msgs/routing/srv/SetRoute.srv +++ /dev/null @@ -1,6 +0,0 @@ -std_msgs/Header header -geometry_msgs/Pose goal -autoware_ad_api_msgs/RouteSegment[] segments ---- -uint16 ERROR_ROUTE_EXISTS = 1 -autoware_ad_api_msgs/ResponseStatus status diff --git a/common/autoware_ad_api_msgs/routing/srv/SetRoutePoints.srv b/common/autoware_ad_api_msgs/routing/srv/SetRoutePoints.srv deleted file mode 100644 index c05a211fae4fb..0000000000000 --- a/common/autoware_ad_api_msgs/routing/srv/SetRoutePoints.srv +++ /dev/null @@ -1,8 +0,0 @@ -std_msgs/Header header -geometry_msgs/Pose goal -geometry_msgs/Pose[] waypoints ---- -uint16 ERROR_ROUTE_EXISTS = 1 -uint16 ERROR_PLANNER_UNREADY = 2 -uint16 ERROR_PLANNER_FAILED = 3 -autoware_ad_api_msgs/ResponseStatus status diff --git a/common/autoware_ad_api_specs/include/autoware_ad_api_specs/interface.hpp b/common/autoware_ad_api_specs/include/autoware_ad_api_specs/interface.hpp index a304bc32da20c..95dca9671e768 100644 --- a/common/autoware_ad_api_specs/include/autoware_ad_api_specs/interface.hpp +++ b/common/autoware_ad_api_specs/include/autoware_ad_api_specs/interface.hpp @@ -15,14 +15,14 @@ #ifndef AUTOWARE_AD_API_SPECS__INTERFACE_HPP_ #define AUTOWARE_AD_API_SPECS__INTERFACE_HPP_ -#include +#include namespace autoware_ad_api::interface { struct Version { - using Service = autoware_ad_api_msgs::srv::InterfaceVersion; + using Service = autoware_adapi_version_msgs::srv::InterfaceVersion; static constexpr char name[] = "/api/interface/version"; }; diff --git a/common/autoware_ad_api_specs/include/autoware_ad_api_specs/localization.hpp b/common/autoware_ad_api_specs/include/autoware_ad_api_specs/localization.hpp index 112a8882d2873..a2db70fb8cb7a 100644 --- a/common/autoware_ad_api_specs/include/autoware_ad_api_specs/localization.hpp +++ b/common/autoware_ad_api_specs/include/autoware_ad_api_specs/localization.hpp @@ -17,21 +17,21 @@ #include -#include -#include +#include +#include namespace autoware_ad_api::localization { struct Initialize { - using Service = autoware_ad_api_msgs::srv::InitializeLocalization; + using Service = autoware_adapi_v1_msgs::srv::InitializeLocalization; static constexpr char name[] = "/api/localization/initialize"; }; struct InitializationState { - using Message = autoware_ad_api_msgs::msg::LocalizationInitializationState; + using Message = autoware_adapi_v1_msgs::msg::LocalizationInitializationState; static constexpr char name[] = "/api/localization/initialization_state"; static constexpr size_t depth = 3; static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; diff --git a/common/autoware_ad_api_specs/include/autoware_ad_api_specs/motion.hpp b/common/autoware_ad_api_specs/include/autoware_ad_api_specs/motion.hpp new file mode 100644 index 0000000000000..dcec2a4670f04 --- /dev/null +++ b/common/autoware_ad_api_specs/include/autoware_ad_api_specs/motion.hpp @@ -0,0 +1,43 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE_AD_API_SPECS__MOTION_HPP_ +#define AUTOWARE_AD_API_SPECS__MOTION_HPP_ + +#include + +#include +#include + +namespace autoware_ad_api::motion +{ + +struct AcceptStart +{ + using Service = autoware_ad_api_msgs::srv::AcceptStart; + static constexpr char name[] = "/api/motion/accept_start"; +}; + +struct State +{ + using Message = autoware_ad_api_msgs::msg::MotionState; + static constexpr char name[] = "/api/motion/state"; + static constexpr size_t depth = 1; + static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; + static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL; +}; + +} // namespace autoware_ad_api::motion + +#endif // AUTOWARE_AD_API_SPECS__MOTION_HPP_ diff --git a/common/autoware_ad_api_specs/include/autoware_ad_api_specs/routing.hpp b/common/autoware_ad_api_specs/include/autoware_ad_api_specs/routing.hpp index 336ea670c0c8b..76fac74f667f9 100644 --- a/common/autoware_ad_api_specs/include/autoware_ad_api_specs/routing.hpp +++ b/common/autoware_ad_api_specs/include/autoware_ad_api_specs/routing.hpp @@ -17,36 +17,36 @@ #include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include namespace autoware_ad_api::routing { struct SetRoutePoints { - using Service = autoware_ad_api_msgs::srv::SetRoutePoints; + using Service = autoware_adapi_v1_msgs::srv::SetRoutePoints; static constexpr char name[] = "/api/routing/set_route_points"; }; struct SetRoute { - using Service = autoware_ad_api_msgs::srv::SetRoute; + using Service = autoware_adapi_v1_msgs::srv::SetRoute; static constexpr char name[] = "/api/routing/set_route"; }; struct ClearRoute { - using Service = autoware_ad_api_msgs::srv::ClearRoute; + using Service = autoware_adapi_v1_msgs::srv::ClearRoute; static constexpr char name[] = "/api/routing/clear_route"; }; struct RouteState { - using Message = autoware_ad_api_msgs::msg::RouteState; + using Message = autoware_adapi_v1_msgs::msg::RouteState; static constexpr char name[] = "/api/routing/state"; static constexpr size_t depth = 3; static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; @@ -55,7 +55,7 @@ struct RouteState struct Route { - using Message = autoware_ad_api_msgs::msg::Route; + using Message = autoware_adapi_v1_msgs::msg::Route; static constexpr char name[] = "/api/routing/route"; static constexpr size_t depth = 1; static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; diff --git a/common/component_interface_specs/include/component_interface_specs/control.hpp b/common/component_interface_specs/include/component_interface_specs/control.hpp new file mode 100644 index 0000000000000..ac99135eb6ef6 --- /dev/null +++ b/common/component_interface_specs/include/component_interface_specs/control.hpp @@ -0,0 +1,53 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef COMPONENT_INTERFACE_SPECS__CONTROL_HPP_ +#define COMPONENT_INTERFACE_SPECS__CONTROL_HPP_ + +#include + +#include +#include +#include + +namespace control_interface +{ + +struct SetPause +{ + using Service = tier4_control_msgs::srv::SetPause; + static constexpr char name[] = "/control/vehicle_cmd_gate/set_pause"; +}; + +struct IsPaused +{ + using Message = tier4_control_msgs::msg::IsPaused; + static constexpr char name[] = "/control/vehicle_cmd_gate/is_paused"; + static constexpr size_t depth = 1; + static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; + static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL; +}; + +struct IsStartRequested +{ + using Message = tier4_control_msgs::msg::IsStartRequested; + static constexpr char name[] = "/control/vehicle_cmd_gate/is_start_requested"; + static constexpr size_t depth = 1; + static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; + static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL; +}; + +} // namespace control_interface + +#endif // COMPONENT_INTERFACE_SPECS__CONTROL_HPP_ diff --git a/common/component_interface_specs/include/component_interface_specs/localization.hpp b/common/component_interface_specs/include/component_interface_specs/localization.hpp index 050400bc2d625..a1a39f5cd57af 100644 --- a/common/component_interface_specs/include/component_interface_specs/localization.hpp +++ b/common/component_interface_specs/include/component_interface_specs/localization.hpp @@ -17,21 +17,21 @@ #include -#include -#include +#include +#include namespace localization_interface { struct Initialize { - using Service = autoware_ad_api_msgs::srv::InitializeLocalization; + using Service = autoware_adapi_v1_msgs::srv::InitializeLocalization; static constexpr char name[] = "/localization/initialize"; }; struct InitializationState { - using Message = autoware_ad_api_msgs::msg::LocalizationInitializationState; + using Message = autoware_adapi_v1_msgs::msg::LocalizationInitializationState; static constexpr char name[] = "/localization/initialization_state"; static constexpr size_t depth = 3; static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; diff --git a/common/component_interface_specs/include/component_interface_specs/planning.hpp b/common/component_interface_specs/include/component_interface_specs/planning.hpp index 08ba456976804..641f36ffa0504 100644 --- a/common/component_interface_specs/include/component_interface_specs/planning.hpp +++ b/common/component_interface_specs/include/component_interface_specs/planning.hpp @@ -17,36 +17,36 @@ #include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include namespace planning_interface { struct SetRoutePoints { - using Service = autoware_ad_api_msgs::srv::SetRoutePoints; + using Service = autoware_adapi_v1_msgs::srv::SetRoutePoints; static constexpr char name[] = "/planning/mission_planning/set_route_points"; }; struct SetRoute { - using Service = autoware_ad_api_msgs::srv::SetRoute; + using Service = autoware_adapi_v1_msgs::srv::SetRoute; static constexpr char name[] = "/planning/mission_planning/set_route"; }; struct ClearRoute { - using Service = autoware_ad_api_msgs::srv::ClearRoute; + using Service = autoware_adapi_v1_msgs::srv::ClearRoute; static constexpr char name[] = "/planning/mission_planning/clear_route"; }; struct RouteState { - using Message = autoware_ad_api_msgs::msg::RouteState; + using Message = autoware_adapi_v1_msgs::msg::RouteState; static constexpr char name[] = "/planning/mission_planning/route_state"; static constexpr size_t depth = 3; static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; @@ -55,7 +55,7 @@ struct RouteState struct Route { - using Message = autoware_ad_api_msgs::msg::Route; + using Message = autoware_adapi_v1_msgs::msg::Route; static constexpr char name[] = "/planning/mission_planning/route"; static constexpr size_t depth = 1; static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; diff --git a/common/component_interface_utils/include/component_interface_utils/rclcpp.hpp b/common/component_interface_utils/include/component_interface_utils/rclcpp.hpp index 11f343cd2a900..283b99b6f34cc 100644 --- a/common/component_interface_utils/include/component_interface_utils/rclcpp.hpp +++ b/common/component_interface_utils/include/component_interface_utils/rclcpp.hpp @@ -32,6 +32,10 @@ class NodeAdaptor private: using CallbackGroup = rclcpp::CallbackGroup::SharedPtr; + template + using MessageCallback = + void (InstanceT::*)(const typename SharedPtrT::element_type::SpecType::Message::ConstSharedPtr); + template using ServiceCallback = void (InstanceT::*)( const typename SharedPtrT::element_type::SpecType::Service::Request::SharedPtr, @@ -92,14 +96,25 @@ class NodeAdaptor srv, [cli, timeout](auto req, auto res) { *res = *cli->call(req, timeout); }, group); } + /// Create a subscription wrapper. + template + void init_sub( + SharedPtrT & sub, InstanceT * instance, + MessageCallback && callback) const + { + using std::placeholders::_1; + init_sub(sub, std::bind(callback, instance, _1)); + } + /// Create a service wrapper for logging. template void init_srv( - SharedPtrT & srv, InstanceT * instance, ServiceCallback callback, + SharedPtrT & srv, InstanceT * instance, ServiceCallback && callback, CallbackGroup group = nullptr) const { - init_srv( - srv, [instance, callback](auto req, auto res) { (instance->*callback)(req, res); }, group); + using std::placeholders::_1; + using std::placeholders::_2; + init_srv(srv, std::bind(callback, instance, _1, _2), group); } private: diff --git a/common/component_interface_utils/include/component_interface_utils/rclcpp/exceptions.hpp b/common/component_interface_utils/include/component_interface_utils/rclcpp/exceptions.hpp index a243ba60c1954..87cb925c05ea4 100644 --- a/common/component_interface_utils/include/component_interface_utils/rclcpp/exceptions.hpp +++ b/common/component_interface_utils/include/component_interface_utils/rclcpp/exceptions.hpp @@ -15,7 +15,7 @@ #ifndef COMPONENT_INTERFACE_UTILS__RCLCPP__EXCEPTIONS_HPP_ #define COMPONENT_INTERFACE_UTILS__RCLCPP__EXCEPTIONS_HPP_ -#include +#include #include #include @@ -26,7 +26,7 @@ namespace component_interface_utils class ServiceException : public std::runtime_error { public: - using ResponseStatus = autoware_ad_api_msgs::msg::ResponseStatus; + using ResponseStatus = autoware_adapi_v1_msgs::msg::ResponseStatus; using ResponseStatusCode = ResponseStatus::_code_type; ServiceException(ResponseStatusCode code, const std::string & message) @@ -34,6 +34,15 @@ class ServiceException : public std::runtime_error { code_ = code; } + + template + void set(T & status) const + { + status.success = false; + status.code = code_; + status.message = what(); + } + ResponseStatus status() const { ResponseStatus status; diff --git a/common/component_interface_utils/include/component_interface_utils/rclcpp/service_server.hpp b/common/component_interface_utils/include/component_interface_utils/rclcpp/service_server.hpp index 30d4d2c5a14dd..f8ba8ab980fcb 100644 --- a/common/component_interface_utils/include/component_interface_utils/rclcpp/service_server.hpp +++ b/common/component_interface_utils/include/component_interface_utils/rclcpp/service_server.hpp @@ -70,7 +70,7 @@ class Service try { callback(request, response); } catch (const ServiceException & error) { - response->status = error.status(); + error.set(response->status); } } RCLCPP_INFO_STREAM(logger, "service exit: " << SpecT::name << "\n" << to_yaml(*response)); diff --git a/common/component_interface_utils/include/component_interface_utils/status.hpp b/common/component_interface_utils/include/component_interface_utils/status.hpp new file mode 100644 index 0000000000000..bf4d67ae5c833 --- /dev/null +++ b/common/component_interface_utils/include/component_interface_utils/status.hpp @@ -0,0 +1,31 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef COMPONENT_INTERFACE_UTILS__STATUS_HPP_ +#define COMPONENT_INTERFACE_UTILS__STATUS_HPP_ + +namespace component_interface_utils::status +{ + +template +void copy(const T1 & src, T2 & dst) // NOLINT(build/include_what_you_use): cpplint false positive +{ + dst->status.success = src->status.success; + dst->status.code = src->status.code; + dst->status.message = src->status.message; +} + +} // namespace component_interface_utils::status + +#endif // COMPONENT_INTERFACE_UTILS__STATUS_HPP_ diff --git a/common/component_interface_utils/package.xml b/common/component_interface_utils/package.xml index 6a67111bc2c99..94e925b7b4b7a 100644 --- a/common/component_interface_utils/package.xml +++ b/common/component_interface_utils/package.xml @@ -11,7 +11,7 @@ autoware_cmake - autoware_ad_api_msgs + autoware_adapi_v1_msgs rclcpp rclcpp_components diff --git a/common/motion_utils/package.xml b/common/motion_utils/package.xml index 8d7aa9167c2ab..d8f6340a473e4 100644 --- a/common/motion_utils/package.xml +++ b/common/motion_utils/package.xml @@ -7,6 +7,8 @@ Yutaka Shimizu Satoshi Ota Takayuki Murooka + + Taiki Tanaka Apache License 2.0 Yutaka Shimizu diff --git a/common/perception_utils/include/perception_utils/object_classification.hpp b/common/perception_utils/include/perception_utils/object_classification.hpp new file mode 100644 index 0000000000000..841466c6f8db5 --- /dev/null +++ b/common/perception_utils/include/perception_utils/object_classification.hpp @@ -0,0 +1,50 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef PERCEPTION_UTILS__OBJECT_CLASSIFICATION_HPP_ +#define PERCEPTION_UTILS__OBJECT_CLASSIFICATION_HPP_ + +#include "autoware_auto_perception_msgs/msg/object_classification.hpp" + +namespace perception_utils +{ +using autoware_auto_perception_msgs::msg::ObjectClassification; + +bool isVehicle(const uint8_t object_classification) +{ + return object_classification == ObjectClassification::BICYCLE || + object_classification == ObjectClassification::BUS || + object_classification == ObjectClassification::CAR || + object_classification == ObjectClassification::MOTORCYCLE || + object_classification == ObjectClassification::TRAILER || + object_classification == ObjectClassification::TRUCK; +} + +bool isCarLikeVehicle(const uint8_t object_classification) +{ + return object_classification == ObjectClassification::BUS || + object_classification == ObjectClassification::CAR || + object_classification == ObjectClassification::TRAILER || + object_classification == ObjectClassification::TRUCK; +} + +bool isLargeVehicle(const uint8_t object_classification) +{ + return object_classification == ObjectClassification::BUS || + object_classification == ObjectClassification::TRAILER || + object_classification == ObjectClassification::TRUCK; +} +} // namespace perception_utils + +#endif // PERCEPTION_UTILS__OBJECT_CLASSIFICATION_HPP_ diff --git a/common/perception_utils/package.xml b/common/perception_utils/package.xml index c338601d9ec46..79d08bc0ed15a 100644 --- a/common/perception_utils/package.xml +++ b/common/perception_utils/package.xml @@ -5,6 +5,7 @@ 0.1.0 The perception_utils package Takayuki Murooka + Satoshi Tanaka Apache License 2.0 ament_cmake_auto diff --git a/common/trtexec_vendor/CMakeLists.txt b/common/trtexec_vendor/CMakeLists.txt new file mode 100644 index 0000000000000..220e7e6d06f81 --- /dev/null +++ b/common/trtexec_vendor/CMakeLists.txt @@ -0,0 +1,88 @@ +cmake_minimum_required(VERSION 3.14) +project(trtexec_vendor) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD_REQUIRED ON) + set(CMAKE_CXX_EXTENSIONS OFF) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) +find_package(CUDA) +find_package(cudnn_cmake_module REQUIRED) +find_package(CUDNN) +find_package(tensorrt_cmake_module REQUIRED) +find_package(TENSORRT) + +if(NOT (${CUDA_FOUND} AND ${CUDNN_FOUND} AND ${TENSORRT_FOUND})) + message(WARNING "cuda, cudnn, tensorrt libraries are not found") + return() +endif() + +if(${TENSORRT_VERSION} VERSION_LESS 8.2.1) + message(WARNING "The tensorrt version less than 8.2.1 isn't supported.") + return() +endif() + +set(TRTEXEC_DEFAULT_BIN /usr/src/tensorrt/bin/trtexec) +if(NOT EXISTS TRTEXEC_DEFAULT_BIN) + include(FetchContent) + if(${TENSORRT_VERSION} VERSION_EQUAL 8.4.2) + set(TENSORRT_VERSION 8.4.1) + elseif(${TENSORRT_VERSION} VERSION_LESS_EQUAL 8.2.5 AND ${TENSORRT_VERSION} VERSION_GREATER 8.2.1) + set(TENSORRT_VERSION 8.2.1) + endif() + fetchcontent_declare(tensorrt + GIT_REPOSITORY https://github.com/NVIDIA/TensorRT + GIT_TAG ${TENSORRT_VERSION} + GIT_SUBMODULES "" + ) + fetchcontent_getproperties(tensorrt) + if(NOT tensorrt_POPULATED) + fetchcontent_populate(tensorrt) + endif() + set(TRTEXEC_SOURCES + ${tensorrt_SOURCE_DIR}/samples/trtexec/trtexec.cpp + ${tensorrt_SOURCE_DIR}/samples/common/sampleEngines.cpp + ${tensorrt_SOURCE_DIR}/samples/common/sampleInference.cpp + ${tensorrt_SOURCE_DIR}/samples/common/sampleOptions.cpp + ${tensorrt_SOURCE_DIR}/samples/common/sampleReporting.cpp + ${tensorrt_SOURCE_DIR}/samples/common/logger.cpp + ) + if(${TENSORRT_VERSION} VERSION_GREATER_EQUAL 8.4) + list(APPEND TRTEXEC_SOURCES + ${tensorrt_SOURCE_DIR}/samples/common/sampleUtils.cpp + ) + endif() + cuda_add_executable(${PROJECT_NAME} + ${TRTEXEC_SOURCES} + ) + target_link_libraries(${PROJECT_NAME} + ${TENSORRT_LIBRARIES} + ) + target_include_directories(${PROJECT_NAME} + PRIVATE ${tensorrt_SOURCE_DIR}/samples/common + ) + + set_target_properties(${PROJECT_NAME} + PROPERTIES OUTPUT_NAME trtexec + ) + + install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin + ) +endif() + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_environment_hooks("${CMAKE_CURRENT_SOURCE_DIR}/env-hooks/${PROJECT_NAME}.sh.in") + +ament_package() diff --git a/common/trtexec_vendor/env-hooks/trtexec_vendor.sh.in b/common/trtexec_vendor/env-hooks/trtexec_vendor.sh.in new file mode 100644 index 0000000000000..7c2ef456ecfec --- /dev/null +++ b/common/trtexec_vendor/env-hooks/trtexec_vendor.sh.in @@ -0,0 +1,5 @@ +TRTEXEC_DEFAULT_BIN_DIR=/usr/src/tensorrt/bin +TRTEXEC_DEFAULT_BIN=$TRTEXEC_DEFAULT_BIN_DIR/trtexec +if [ -f $TRTEXEC_DEFAULT_BIN ]; then + ament_prepend_unique_value PATH $TRTEXEC_DEFAULT_BIN_DIR +fi diff --git a/common/trtexec_vendor/package.xml b/common/trtexec_vendor/package.xml new file mode 100644 index 0000000000000..f197858060f3a --- /dev/null +++ b/common/trtexec_vendor/package.xml @@ -0,0 +1,20 @@ + + + + trtexec_vendor + 0.1.0 + The vendor package of trtexec + Daisuke Nishimatsu + Apache 2.0 + + ament_cmake + cudnn_cmake_module + tensorrt_cmake_module + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/control/trajectory_follower/include/trajectory_follower/input_data.hpp b/control/trajectory_follower/include/trajectory_follower/input_data.hpp index 505e5e0de2545..b356095e2f78e 100644 --- a/control/trajectory_follower/include/trajectory_follower/input_data.hpp +++ b/control/trajectory_follower/include/trajectory_follower/input_data.hpp @@ -17,6 +17,7 @@ #include "autoware_auto_planning_msgs/msg/trajectory.hpp" #include "autoware_auto_vehicle_msgs/msg/steering_report.hpp" +#include "geometry_msgs/msg/accel_with_covariance_stamped.hpp" #include "nav_msgs/msg/odometry.hpp" namespace autoware @@ -32,6 +33,7 @@ struct InputData autoware_auto_planning_msgs::msg::Trajectory::SharedPtr current_trajectory_ptr; nav_msgs::msg::Odometry::SharedPtr current_odometry_ptr; autoware_auto_vehicle_msgs::msg::SteeringReport::SharedPtr current_steering_ptr; + geometry_msgs::msg::AccelWithCovarianceStamped::SharedPtr current_accel_ptr; }; } // namespace trajectory_follower } // namespace control diff --git a/control/trajectory_follower/include/trajectory_follower/pid_longitudinal_controller.hpp b/control/trajectory_follower/include/trajectory_follower/pid_longitudinal_controller.hpp index 60c7cca13ad02..c7b5e73d5982c 100644 --- a/control/trajectory_follower/include/trajectory_follower/pid_longitudinal_controller.hpp +++ b/control/trajectory_follower/include/trajectory_follower/pid_longitudinal_controller.hpp @@ -97,7 +97,7 @@ class TRAJECTORY_FOLLOWER_PUBLIC PidLongitudinalController : public Longitudinal // pointers for ros topic nav_msgs::msg::Odometry::ConstSharedPtr m_current_kinematic_state_ptr{nullptr}; - nav_msgs::msg::Odometry::ConstSharedPtr m_prev_kienmatic_state_ptr{nullptr}; + geometry_msgs::msg::AccelWithCovarianceStamped::ConstSharedPtr m_current_accel_ptr{nullptr}; autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr m_trajectory_ptr{nullptr}; // vehicle info @@ -185,9 +185,6 @@ class TRAJECTORY_FOLLOWER_PUBLIC PidLongitudinalController : public Longitudinal double m_ego_nearest_dist_threshold; double m_ego_nearest_yaw_threshold; - // 1st order lowpass filter for acceleration - std::shared_ptr m_lpf_acc{nullptr}; - // buffer of send command std::vector m_ctrl_cmd_vec; @@ -213,6 +210,13 @@ class TRAJECTORY_FOLLOWER_PUBLIC PidLongitudinalController : public Longitudinal */ void setKinematicState(const nav_msgs::msg::Odometry::ConstSharedPtr msg); + /** + * @brief set current acceleration with received message + * @param [in] msg trajectory message + */ + void setCurrentAcceleration( + const geometry_msgs::msg::AccelWithCovarianceStamped::ConstSharedPtr msg); + /** * @brief set reference trajectory with received message * @param [in] msg trajectory message diff --git a/control/trajectory_follower/src/pid_longitudinal_controller.cpp b/control/trajectory_follower/src/pid_longitudinal_controller.cpp index f8919d06638a4..53c3f71fc11bd 100644 --- a/control/trajectory_follower/src/pid_longitudinal_controller.cpp +++ b/control/trajectory_follower/src/pid_longitudinal_controller.cpp @@ -199,26 +199,27 @@ PidLongitudinalController::PidLongitudinalController(rclcpp::Node & node) : node // set parameter callback m_set_param_res = node_->add_on_set_parameters_callback( std::bind(&PidLongitudinalController::paramCallback, this, _1)); - - // set lowpass filter for acc - m_lpf_acc = std::make_shared(0.0, 0.2); } void PidLongitudinalController::setInputData(InputData const & input_data) { setTrajectory(input_data.current_trajectory_ptr); setKinematicState(input_data.current_odometry_ptr); + setCurrentAcceleration(input_data.current_accel_ptr); } void PidLongitudinalController::setKinematicState(const nav_msgs::msg::Odometry::ConstSharedPtr msg) { if (!msg) return; - - if (m_current_kinematic_state_ptr) { - m_prev_kienmatic_state_ptr = m_current_kinematic_state_ptr; - } m_current_kinematic_state_ptr = msg; } +void PidLongitudinalController::setCurrentAcceleration( + const geometry_msgs::msg::AccelWithCovarianceStamped::ConstSharedPtr msg) +{ + if (!msg) return; + m_current_accel_ptr = msg; +} + void PidLongitudinalController::setTrajectory( const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr msg) { @@ -365,7 +366,7 @@ rcl_interfaces::msg::SetParametersResult PidLongitudinalController::paramCallbac boost::optional PidLongitudinalController::run() { // wait for initial pointers - if (!m_current_kinematic_state_ptr || !m_prev_kienmatic_state_ptr || !m_trajectory_ptr) { + if (!m_current_kinematic_state_ptr || !m_trajectory_ptr || !m_current_accel_ptr) { return boost::none; } @@ -415,7 +416,8 @@ PidLongitudinalController::ControlData PidLongitudinalController::getControlData control_data.dt = getDt(); // current velocity and acceleration - control_data.current_motion = getCurrentMotion(); + control_data.current_motion.vel = m_current_kinematic_state_ptr->twist.twist.linear.x; + control_data.current_motion.acc = m_current_accel_ptr->accel.accel.linear.x; // nearest idx const size_t nearest_idx = motion_utils::findFirstNearestIndexWithSoftConstraints( @@ -699,23 +701,6 @@ float64_t PidLongitudinalController::getDt() return std::max(std::min(dt, max_dt), min_dt); } -PidLongitudinalController::Motion PidLongitudinalController::getCurrentMotion() const -{ - const float64_t dv = m_current_kinematic_state_ptr->twist.twist.linear.x - - m_prev_kienmatic_state_ptr->twist.twist.linear.x; - const float64_t dt = std::max( - (rclcpp::Time(m_current_kinematic_state_ptr->header.stamp) - - rclcpp::Time(m_prev_kienmatic_state_ptr->header.stamp)) - .seconds(), - 1e-03); - const float64_t accel = dv / dt; - - const float64_t current_vel = m_current_kinematic_state_ptr->twist.twist.linear.x; - const float64_t current_acc = m_lpf_acc->filter(accel); - - return Motion{current_vel, current_acc}; -} - enum PidLongitudinalController::Shift PidLongitudinalController::getCurrentShift( const size_t nearest_idx) const { diff --git a/control/trajectory_follower_nodes/include/trajectory_follower_nodes/controller_node.hpp b/control/trajectory_follower_nodes/include/trajectory_follower_nodes/controller_node.hpp index 91b99fc7837c9..2c8d51b4b69e2 100644 --- a/control/trajectory_follower_nodes/include/trajectory_follower_nodes/controller_node.hpp +++ b/control/trajectory_follower_nodes/include/trajectory_follower_nodes/controller_node.hpp @@ -37,6 +37,8 @@ #include "autoware_auto_planning_msgs/msg/trajectory.hpp" #include "autoware_auto_system_msgs/msg/float32_multi_array_diagnostic.hpp" #include "autoware_auto_vehicle_msgs/msg/vehicle_odometry.hpp" +#include "geometry_msgs/msg/accel_stamped.hpp" +#include "geometry_msgs/msg/accel_with_covariance_stamped.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "nav_msgs/msg/odometry.hpp" #include "tf2_msgs/msg/tf_message.hpp" @@ -82,6 +84,7 @@ class TRAJECTORY_FOLLOWER_PUBLIC Controller : public rclcpp::Node rclcpp::Subscription::SharedPtr sub_ref_path_; rclcpp::Subscription::SharedPtr sub_odometry_; rclcpp::Subscription::SharedPtr sub_steering_; + rclcpp::Subscription::SharedPtr sub_accel_; rclcpp::Publisher::SharedPtr control_cmd_pub_; @@ -102,6 +105,7 @@ class TRAJECTORY_FOLLOWER_PUBLIC Controller : public rclcpp::Node void onTrajectory(const autoware_auto_planning_msgs::msg::Trajectory::SharedPtr); void onOdometry(const nav_msgs::msg::Odometry::SharedPtr msg); void onSteering(const autoware_auto_vehicle_msgs::msg::SteeringReport::SharedPtr msg); + void onAccel(const geometry_msgs::msg::AccelWithCovarianceStamped::SharedPtr msg); bool isTimeOut(); LateralControllerMode getLateralControllerMode(const std::string & algorithm_name) const; LongitudinalControllerMode getLongitudinalControllerMode( diff --git a/control/trajectory_follower_nodes/src/controller_node.cpp b/control/trajectory_follower_nodes/src/controller_node.cpp index 2eaf51d8a7652..f720dfc9c27fd 100644 --- a/control/trajectory_follower_nodes/src/controller_node.cpp +++ b/control/trajectory_follower_nodes/src/controller_node.cpp @@ -76,6 +76,8 @@ Controller::Controller(const rclcpp::NodeOptions & node_options) : Node("control "~/input/current_steering", rclcpp::QoS{1}, std::bind(&Controller::onSteering, this, _1)); sub_odometry_ = create_subscription( "~/input/current_odometry", rclcpp::QoS{1}, std::bind(&Controller::onOdometry, this, _1)); + sub_accel_ = create_subscription( + "~/input/current_accel", rclcpp::QoS{1}, std::bind(&Controller::onAccel, this, _1)); control_cmd_pub_ = create_publisher( "~/output/control_cmd", rclcpp::QoS{1}.transient_local()); @@ -120,6 +122,11 @@ void Controller::onSteering(const autoware_auto_vehicle_msgs::msg::SteeringRepor input_data_.current_steering_ptr = msg; } +void Controller::onAccel(const geometry_msgs::msg::AccelWithCovarianceStamped::SharedPtr msg) +{ + input_data_.current_accel_ptr = msg; +} + bool Controller::isTimeOut() { const auto now = this->now(); diff --git a/control/trajectory_follower_nodes/test/test_controller_node.cpp b/control/trajectory_follower_nodes/test/test_controller_node.cpp index d0a2b96302124..acb172f66ca28 100644 --- a/control/trajectory_follower_nodes/test/test_controller_node.cpp +++ b/control/trajectory_follower_nodes/test/test_controller_node.cpp @@ -24,6 +24,7 @@ #include "autoware_auto_planning_msgs/msg/trajectory.hpp" #include "autoware_auto_vehicle_msgs/msg/steering_report.hpp" #include "autoware_auto_vehicle_msgs/msg/vehicle_odometry.hpp" +#include "geometry_msgs/msg/accel_with_covariance_stamped.hpp" #include "geometry_msgs/msg/pose.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "geometry_msgs/msg/transform_stamped.hpp" @@ -38,6 +39,7 @@ using Trajectory = autoware_auto_planning_msgs::msg::Trajectory; using TrajectoryPoint = autoware_auto_planning_msgs::msg::TrajectoryPoint; using VehicleOdometry = nav_msgs::msg::Odometry; using SteeringReport = autoware_auto_vehicle_msgs::msg::SteeringReport; +using geometry_msgs::msg::AccelWithCovarianceStamped; using FakeNodeFixture = autoware::tools::testing::FakeTestNode; @@ -68,815 +70,415 @@ std::shared_ptr makeNode() return node; } -TEST_F(FakeNodeFixture, no_input) +class ControllerTester { - // Data to test +public: + explicit ControllerTester(FakeNodeFixture * _fnf) : fnf(_fnf) {} + + std::shared_ptr node = makeNode(); + + FakeNodeFixture * fnf; AckermannControlCommand::SharedPtr cmd_msg; bool received_control_command = false; - // Node - std::shared_ptr node = makeNode(); - // Publisher/Subscribers + + void publish_default_odom() + { + VehicleOdometry odom_msg; + odom_msg.header.stamp = node->now(); + odom_pub->publish(odom_msg); + }; + + void publish_odom_vx(const double vx) + { + VehicleOdometry odom_msg; + odom_msg.header.stamp = node->now(); + odom_msg.twist.twist.linear.x = vx; + odom_pub->publish(odom_msg); + }; + + void publish_default_steer() + { + SteeringReport steer_msg; + steer_msg.stamp = node->now(); + steer_pub->publish(steer_msg); + }; + + void publish_steer_angle(const double steer) + { + SteeringReport steer_msg; + steer_msg.stamp = node->now(); + steer_msg.steering_tire_angle = steer; + steer_pub->publish(steer_msg); + }; + + void publish_default_acc() + { + AccelWithCovarianceStamped acc_msg; + acc_msg.header.stamp = node->now(); + accel_pub->publish(acc_msg); + }; + + void publish_default_traj() + { + Trajectory traj_msg; + traj_msg.header.stamp = node->now(); + traj_msg.header.frame_id = "map"; + traj_pub->publish(traj_msg); + }; + + void send_default_transform() + { + // Dummy transform: ego is at (0.0, 0.0) in map frame + geometry_msgs::msg::TransformStamped transform = test_utils::getDummyTransform(); + transform.header.stamp = node->now(); + br->sendTransform(transform); + + // Spin for transform to be published + test_utils::spinWhile(node); + }; + rclcpp::Publisher::SharedPtr traj_pub = - this->create_publisher("controller/input/reference_trajectory"); + fnf->create_publisher("controller/input/reference_trajectory"); + rclcpp::Publisher::SharedPtr odom_pub = - this->create_publisher("controller/input/current_odometry"); + fnf->create_publisher("controller/input/current_odometry"); + rclcpp::Publisher::SharedPtr steer_pub = - this->create_publisher("controller/input/current_steering"); + fnf->create_publisher("controller/input/current_steering"); + + rclcpp::Publisher::SharedPtr accel_pub = + fnf->create_publisher("controller/input/current_accel"); + rclcpp::Subscription::SharedPtr cmd_sub = - this->create_subscription( - "controller/output/control_cmd", *this->get_fake_node(), - [&cmd_msg, &received_control_command](const AckermannControlCommand::SharedPtr msg) { + fnf->create_subscription( + "controller/output/control_cmd", *fnf->get_fake_node(), + [this](const AckermannControlCommand::SharedPtr msg) { cmd_msg = msg; received_control_command = true; }); + std::shared_ptr br = - std::make_shared(this->get_fake_node()); + std::make_shared(fnf->get_fake_node()); +}; + +TrajectoryPoint make_traj_point(const double px, const double py, const float vx) +{ + TrajectoryPoint p; + p.pose.position.x = px; + p.pose.position.y = py; + p.longitudinal_velocity_mps = vx; + return p; +} + +TEST_F(FakeNodeFixture, no_input) +{ + ControllerTester tester(this); // No published data: expect a stopped command test_utils::waitForMessage( - node, this, received_control_command, std::chrono::seconds{1LL}, false); - ASSERT_FALSE(received_control_command); + tester.node, this, tester.received_control_command, std::chrono::seconds{1LL}, false); + ASSERT_FALSE(tester.received_control_command); } TEST_F(FakeNodeFixture, empty_trajectory) { - // Data to test - AckermannControlCommand::SharedPtr cmd_msg; - bool received_control_command = false; - // Node - std::shared_ptr node = makeNode(); - // Publisher/Subscribers - rclcpp::Publisher::SharedPtr traj_pub = - this->create_publisher("controller/input/reference_trajectory"); - rclcpp::Publisher::SharedPtr odom_pub = - this->create_publisher("controller/input/current_odometry"); - rclcpp::Publisher::SharedPtr steer_pub = - this->create_publisher("controller/input/current_steering"); - rclcpp::Subscription::SharedPtr cmd_sub = - this->create_subscription( - "controller/output/control_cmd", *this->get_fake_node(), - [&cmd_msg, &received_control_command](const AckermannControlCommand::SharedPtr msg) { - cmd_msg = msg; - received_control_command = true; - }); - std::shared_ptr br = - std::make_shared(this->get_fake_node()); - - // Dummy transform: ego is at (0.0, 0.0) in map frame - geometry_msgs::msg::TransformStamped transform = test_utils::getDummyTransform(); - transform.header.stamp = node->now(); - br->sendTransform(transform); + ControllerTester tester(this); - // Spin for transform to be published - test_utils::spinWhile(node); + tester.send_default_transform(); // Empty trajectory: expect a stopped command - Trajectory traj_msg; - traj_msg.header.stamp = node->now(); - traj_msg.header.frame_id = "map"; - VehicleOdometry odom_msg; - SteeringReport steer_msg; - traj_msg.header.stamp = node->now(); - odom_msg.header.stamp = node->now(); - odom_msg.twist.twist.linear.x = 0.0; - steer_msg.stamp = node->now(); - steer_msg.steering_tire_angle = 0.0; - traj_pub->publish(traj_msg); - odom_pub->publish(odom_msg); - steer_pub->publish(steer_msg); + tester.publish_default_traj(); + tester.publish_default_odom(); + tester.publish_default_acc(); + tester.publish_default_steer(); test_utils::waitForMessage( - node, this, received_control_command, std::chrono::seconds{1LL}, false); - ASSERT_FALSE(received_control_command); + tester.node, this, tester.received_control_command, std::chrono::seconds{1LL}, false); + ASSERT_FALSE(tester.received_control_command); } // lateral TEST_F(FakeNodeFixture, straight_trajectory) { - // Data to test - AckermannControlCommand::SharedPtr cmd_msg; - bool received_control_command = false; - // Node - std::shared_ptr node = makeNode(); - // Publisher/Subscribers - rclcpp::Publisher::SharedPtr traj_pub = - this->create_publisher("controller/input/reference_trajectory"); - rclcpp::Publisher::SharedPtr odom_pub = - this->create_publisher("controller/input/current_odometry"); - rclcpp::Publisher::SharedPtr steer_pub = - this->create_publisher("controller/input/current_steering"); - rclcpp::Subscription::SharedPtr cmd_sub = - this->create_subscription( - "controller/output/control_cmd", *this->get_fake_node(), - [&cmd_msg, &received_control_command](const AckermannControlCommand::SharedPtr msg) { - cmd_msg = msg; - received_control_command = true; - }); - std::shared_ptr br = - std::make_shared(this->get_fake_node()); + ControllerTester tester(this); - // Dummy transform: ego is at (0.0, 0.0) in map frame - geometry_msgs::msg::TransformStamped transform = test_utils::getDummyTransform(); - transform.header.stamp = node->now(); - br->sendTransform(transform); + tester.send_default_transform(); + tester.publish_odom_vx(1.0); + tester.publish_default_steer(); + tester.publish_default_acc(); - // Spin for transform to be published - test_utils::spinWhile(node); - - // Straight trajectory: expect no steering - received_control_command = false; Trajectory traj_msg; - traj_msg.header.stamp = node->now(); + traj_msg.header.stamp = tester.node->now(); traj_msg.header.frame_id = "map"; - VehicleOdometry odom_msg; - SteeringReport steer_msg; - TrajectoryPoint p; - traj_msg.header.stamp = node->now(); - p.pose.position.x = -1.0; - p.pose.position.y = 0.0; - p.longitudinal_velocity_mps = 1.0f; - traj_msg.points.push_back(p); - p.pose.position.x = 0.0; - p.pose.position.y = 0.0; - p.longitudinal_velocity_mps = 1.0f; - traj_msg.points.push_back(p); - p.pose.position.x = 1.0; - p.pose.position.y = 0.0; - p.longitudinal_velocity_mps = 1.0f; - traj_msg.points.push_back(p); - p.pose.position.x = 2.0; - p.pose.position.y = 0.0; - p.longitudinal_velocity_mps = 1.0f; - traj_msg.points.push_back(p); - traj_pub->publish(traj_msg); - odom_msg.header.stamp = node->now(); - odom_msg.twist.twist.linear.x = 1.0; - steer_msg.stamp = node->now(); - steer_msg.steering_tire_angle = 0.0; - odom_pub->publish(odom_msg); - steer_pub->publish(steer_msg); - - test_utils::waitForMessage(node, this, received_control_command); - ASSERT_TRUE(received_control_command); - EXPECT_EQ(cmd_msg->lateral.steering_tire_angle, 0.0f); - EXPECT_EQ(cmd_msg->lateral.steering_tire_rotation_rate, 0.0f); - EXPECT_GT(rclcpp::Time(cmd_msg->stamp), rclcpp::Time(traj_msg.header.stamp)); + traj_msg.points.push_back(make_traj_point(-1.0, 0.0, 1.0f)); + traj_msg.points.push_back(make_traj_point(0.0, 0.0, 1.0f)); + traj_msg.points.push_back(make_traj_point(1.0, 0.0, 1.0f)); + traj_msg.points.push_back(make_traj_point(2.0, 0.0, 1.0f)); + tester.traj_pub->publish(traj_msg); + + test_utils::waitForMessage(tester.node, this, tester.received_control_command); + ASSERT_TRUE(tester.received_control_command); + EXPECT_EQ(tester.cmd_msg->lateral.steering_tire_angle, 0.0f); + EXPECT_EQ(tester.cmd_msg->lateral.steering_tire_rotation_rate, 0.0f); + EXPECT_GT(rclcpp::Time(tester.cmd_msg->stamp), rclcpp::Time(traj_msg.header.stamp)); } TEST_F(FakeNodeFixture, right_turn) { - // Data to test - AckermannControlCommand::SharedPtr cmd_msg; - bool received_control_command = false; - // Node - std::shared_ptr node = makeNode(); - // Publisher/Subscribers - rclcpp::Publisher::SharedPtr traj_pub = - this->create_publisher("controller/input/reference_trajectory"); - rclcpp::Publisher::SharedPtr odom_pub = - this->create_publisher("controller/input/current_odometry"); - rclcpp::Publisher::SharedPtr steer_pub = - this->create_publisher("controller/input/current_steering"); - rclcpp::Subscription::SharedPtr cmd_sub = - this->create_subscription( - "controller/output/control_cmd", *this->get_fake_node(), - [&cmd_msg, &received_control_command](const AckermannControlCommand::SharedPtr msg) { - cmd_msg = msg; - received_control_command = true; - }); - std::shared_ptr br = - std::make_shared(this->get_fake_node()); + ControllerTester tester(this); - // Dummy transform: ego is at (0.0, 0.0) in map frame - geometry_msgs::msg::TransformStamped transform = test_utils::getDummyTransform(); - transform.header.stamp = node->now(); - br->sendTransform(transform); - - // Spin for transform to be published - test_utils::spinWhile(node); + tester.send_default_transform(); + tester.publish_odom_vx(1.0); + tester.publish_default_steer(); + tester.publish_default_acc(); // Right turning trajectory: expect right steering - received_control_command = false; Trajectory traj_msg; - traj_msg.header.stamp = node->now(); + traj_msg.header.stamp = tester.node->now(); traj_msg.header.frame_id = "map"; - VehicleOdometry odom_msg; - SteeringReport steer_msg; - TrajectoryPoint p; - traj_msg.points.clear(); - p.pose.position.x = -1.0; - p.pose.position.y = -1.0; - p.longitudinal_velocity_mps = 1.0f; - traj_msg.points.push_back(p); - p.pose.position.x = 0.0; - p.pose.position.y = 0.0; - p.longitudinal_velocity_mps = 1.0f; - traj_msg.points.push_back(p); - p.pose.position.x = 1.0; - p.pose.position.y = -1.0; - p.longitudinal_velocity_mps = 1.0f; - traj_msg.points.push_back(p); - p.pose.position.x = 2.0; - p.pose.position.y = -2.0; - p.longitudinal_velocity_mps = 1.0f; - traj_msg.points.push_back(p); - traj_pub->publish(traj_msg); - odom_msg.header.stamp = node->now(); - odom_msg.twist.twist.linear.x = 1.0; - steer_msg.stamp = node->now(); - steer_msg.steering_tire_angle = 0.0; - odom_pub->publish(odom_msg); - steer_pub->publish(steer_msg); - - test_utils::waitForMessage(node, this, received_control_command); - ASSERT_TRUE(received_control_command); - EXPECT_LT(cmd_msg->lateral.steering_tire_angle, 0.0f); - EXPECT_LT(cmd_msg->lateral.steering_tire_rotation_rate, 0.0f); - EXPECT_GT(rclcpp::Time(cmd_msg->stamp), rclcpp::Time(traj_msg.header.stamp)); + traj_msg.points.push_back(make_traj_point(-1.0, -1.0, 1.0f)); + traj_msg.points.push_back(make_traj_point(0.0, 0.0, 1.0f)); + traj_msg.points.push_back(make_traj_point(1.0, -1.0, 1.0f)); + traj_msg.points.push_back(make_traj_point(2.0, -2.0, 1.0f)); + tester.traj_pub->publish(traj_msg); + + test_utils::waitForMessage(tester.node, this, tester.received_control_command); + ASSERT_TRUE(tester.received_control_command); + EXPECT_LT(tester.cmd_msg->lateral.steering_tire_angle, 0.0f); + EXPECT_LT(tester.cmd_msg->lateral.steering_tire_rotation_rate, 0.0f); + EXPECT_GT(rclcpp::Time(tester.cmd_msg->stamp), rclcpp::Time(traj_msg.header.stamp)); } TEST_F(FakeNodeFixture, left_turn) { - // Data to test - AckermannControlCommand::SharedPtr cmd_msg; - bool received_control_command = false; - // Node - std::shared_ptr node = makeNode(); - // Publisher/Subscribers - rclcpp::Publisher::SharedPtr traj_pub = - this->create_publisher("controller/input/reference_trajectory"); - rclcpp::Publisher::SharedPtr odom_pub = - this->create_publisher("controller/input/current_odometry"); - rclcpp::Publisher::SharedPtr steer_pub = - this->create_publisher("controller/input/current_steering"); - rclcpp::Subscription::SharedPtr cmd_sub = - this->create_subscription( - "controller/output/control_cmd", *this->get_fake_node(), - [&cmd_msg, &received_control_command](const AckermannControlCommand::SharedPtr msg) { - cmd_msg = msg; - received_control_command = true; - }); - std::shared_ptr br = - std::make_shared(this->get_fake_node()); - - // Dummy transform: ego is at (0.0, 0.0) in map frame - geometry_msgs::msg::TransformStamped transform = test_utils::getDummyTransform(); - transform.header.stamp = node->now(); - br->sendTransform(transform); + ControllerTester tester(this); - // Spin for transform to be published - test_utils::spinWhile(node); + tester.send_default_transform(); + tester.publish_odom_vx(1.0); + tester.publish_default_steer(); + tester.publish_default_acc(); // Left turning trajectory: expect left steering - received_control_command = false; Trajectory traj_msg; - traj_msg.header.stamp = node->now(); + traj_msg.header.stamp = tester.node->now(); traj_msg.header.frame_id = "map"; - VehicleOdometry odom_msg; - SteeringReport steer_msg; - TrajectoryPoint p; - traj_msg.points.clear(); - p.pose.position.x = -1.0; - p.pose.position.y = 1.0; - p.longitudinal_velocity_mps = 1.0f; - traj_msg.points.push_back(p); - p.pose.position.x = 0.0; - p.pose.position.y = 0.0; - p.longitudinal_velocity_mps = 1.0f; - traj_msg.points.push_back(p); - p.pose.position.x = 1.0; - p.pose.position.y = 1.0; - p.longitudinal_velocity_mps = 1.0f; - traj_msg.points.push_back(p); - p.pose.position.x = 2.0; - p.pose.position.y = 2.0; - p.longitudinal_velocity_mps = 1.0f; - traj_msg.points.push_back(p); - traj_pub->publish(traj_msg); - odom_msg.header.stamp = node->now(); - odom_msg.twist.twist.linear.x = 1.0; - steer_msg.stamp = node->now(); - steer_msg.steering_tire_angle = 0.0; - odom_pub->publish(odom_msg); - steer_pub->publish(steer_msg); - - test_utils::waitForMessage(node, this, received_control_command); - ASSERT_TRUE(received_control_command); - EXPECT_GT(cmd_msg->lateral.steering_tire_angle, 0.0f); - EXPECT_GT(cmd_msg->lateral.steering_tire_rotation_rate, 0.0f); - EXPECT_GT(rclcpp::Time(cmd_msg->stamp), rclcpp::Time(traj_msg.header.stamp)); + traj_msg.points.push_back(make_traj_point(-1.0, 1.0, 1.0f)); + traj_msg.points.push_back(make_traj_point(0.0, 0.0, 1.0f)); + traj_msg.points.push_back(make_traj_point(1.0, 1.0, 1.0f)); + traj_msg.points.push_back(make_traj_point(2.0, 2.0, 1.0f)); + tester.traj_pub->publish(traj_msg); + + test_utils::waitForMessage(tester.node, this, tester.received_control_command); + ASSERT_TRUE(tester.received_control_command); + EXPECT_GT(tester.cmd_msg->lateral.steering_tire_angle, 0.0f); + EXPECT_GT(tester.cmd_msg->lateral.steering_tire_rotation_rate, 0.0f); + EXPECT_GT(rclcpp::Time(tester.cmd_msg->stamp), rclcpp::Time(traj_msg.header.stamp)); } TEST_F(FakeNodeFixture, stopped) { - // Data to test - AckermannControlCommand::SharedPtr cmd_msg; - bool received_control_command = false; - // Node - std::shared_ptr node = makeNode(); - // Publisher/Subscribers - rclcpp::Publisher::SharedPtr traj_pub = - this->create_publisher("controller/input/reference_trajectory"); - rclcpp::Publisher::SharedPtr odom_pub = - this->create_publisher("controller/input/current_odometry"); - rclcpp::Publisher::SharedPtr steer_pub = - this->create_publisher("controller/input/current_steering"); - rclcpp::Subscription::SharedPtr cmd_sub = - this->create_subscription( - "controller/output/control_cmd", *this->get_fake_node(), - [&cmd_msg, &received_control_command](const AckermannControlCommand::SharedPtr msg) { - cmd_msg = msg; - received_control_command = true; - }); - std::shared_ptr br = - std::make_shared(this->get_fake_node()); + ControllerTester tester(this); - // Dummy transform: ego is at (0.0, 0.0) in map frame - geometry_msgs::msg::TransformStamped transform = test_utils::getDummyTransform(); - transform.header.stamp = node->now(); - br->sendTransform(transform); + tester.send_default_transform(); + tester.publish_default_odom(); + tester.publish_default_acc(); - // Spin for transform to be published - test_utils::spinWhile(node); + const double steering_tire_angle = -0.5; + tester.publish_steer_angle(steering_tire_angle); // Straight trajectory: expect no steering - received_control_command = false; Trajectory traj_msg; - traj_msg.header.stamp = node->now(); + traj_msg.header.stamp = tester.node->now(); traj_msg.header.frame_id = "map"; - VehicleOdometry odom_msg; - SteeringReport steer_msg; - TrajectoryPoint p; - traj_msg.header.stamp = node->now(); - p.pose.position.x = -1.0; - p.pose.position.y = 0.0; - // Set a 0 current velocity and 0 target velocity -> stopped state - p.longitudinal_velocity_mps = 0.0f; - traj_msg.points.push_back(p); - p.pose.position.x = 0.0; - p.pose.position.y = 0.0; - p.longitudinal_velocity_mps = 0.0f; - traj_msg.points.push_back(p); - p.pose.position.x = 1.0; - p.pose.position.y = 0.0; - p.longitudinal_velocity_mps = 0.0f; - traj_msg.points.push_back(p); - p.pose.position.x = 2.0; - p.pose.position.y = 0.0; - p.longitudinal_velocity_mps = 0.0f; - traj_msg.points.push_back(p); - traj_pub->publish(traj_msg); - odom_msg.header.stamp = node->now(); - odom_msg.twist.twist.linear.x = 0.0; - steer_msg.stamp = node->now(); - steer_msg.steering_tire_angle = -0.5; - odom_pub->publish(odom_msg); - steer_pub->publish(steer_msg); - - test_utils::waitForMessage(node, this, received_control_command); - ASSERT_TRUE(received_control_command); - EXPECT_EQ(cmd_msg->lateral.steering_tire_angle, steer_msg.steering_tire_angle); - EXPECT_EQ(cmd_msg->lateral.steering_tire_rotation_rate, 0.0f); - EXPECT_GT(rclcpp::Time(cmd_msg->stamp), rclcpp::Time(traj_msg.header.stamp)); + traj_msg.points.push_back(make_traj_point(-1.0, 0.0, 0.0f)); + traj_msg.points.push_back(make_traj_point(0.0, 0.0, 0.0f)); + traj_msg.points.push_back(make_traj_point(1.0, 0.0, 0.0f)); + traj_msg.points.push_back(make_traj_point(2.0, 0.0, 0.0f)); + tester.traj_pub->publish(traj_msg); + + test_utils::waitForMessage(tester.node, this, tester.received_control_command); + ASSERT_TRUE(tester.received_control_command); + EXPECT_EQ(tester.cmd_msg->lateral.steering_tire_angle, steering_tire_angle); + EXPECT_EQ(tester.cmd_msg->lateral.steering_tire_rotation_rate, 0.0f); + EXPECT_GT(rclcpp::Time(tester.cmd_msg->stamp), rclcpp::Time(traj_msg.header.stamp)); } // longitudinal TEST_F(FakeNodeFixture, longitudinal_keep_velocity) { - // Data to test - AckermannControlCommand::SharedPtr cmd_msg; - bool received_control_command = false; - // Node - std::shared_ptr node = makeNode(); - // Publisher/Subscribers - rclcpp::Publisher::SharedPtr traj_pub = - this->create_publisher("controller/input/reference_trajectory"); - rclcpp::Publisher::SharedPtr odom_pub = - this->create_publisher("controller/input/current_odometry"); - rclcpp::Publisher::SharedPtr steer_pub = - this->create_publisher("controller/input/current_steering"); - rclcpp::Subscription::SharedPtr cmd_sub = - this->create_subscription( - "controller/output/control_cmd", *this->get_fake_node(), - [&cmd_msg, &received_control_command](const AckermannControlCommand::SharedPtr msg) { - cmd_msg = msg; - received_control_command = true; - }); - std::shared_ptr br = - std::make_shared(this->get_fake_node()); - - // Dummy transform: ego is at (0.0, 0.0) in map frame - geometry_msgs::msg::TransformStamped transform = test_utils::getDummyTransform(); - transform.header.stamp = node->now(); - br->sendTransform(transform); - /// Already running at target vel + Non stopping trajectory -> no change in velocity - // Publish velocity - VehicleOdometry odom_msg; - odom_msg.header.stamp = node->now(); - odom_msg.twist.twist.linear.x = 1.0; - odom_pub->publish(odom_msg); - // the node needs to receive two velocity msg - rclcpp::spin_some(node); - rclcpp::spin_some(this->get_fake_node()); - odom_msg.header.stamp = node->now(); - odom_pub->publish(odom_msg); - // Publish steering - SteeringReport steer_msg; - steer_msg.stamp = node->now(); - steer_msg.steering_tire_angle = 0.0; - steer_pub->publish(steer_msg); + ControllerTester tester(this); + + tester.send_default_transform(); + tester.publish_odom_vx(1.0); + tester.publish_default_steer(); + tester.publish_default_acc(); + // Publish non stopping trajectory - Trajectory traj; - traj.header.stamp = node->now(); - traj.header.frame_id = "map"; - TrajectoryPoint point; - point.pose.position.x = 0.0; - point.pose.position.y = 0.0; - point.longitudinal_velocity_mps = 1.0; - traj.points.push_back(point); - point.pose.position.x = 50.0; - point.pose.position.y = 0.0; - point.longitudinal_velocity_mps = 1.0; - traj.points.push_back(point); - point.pose.position.x = 100.0; - point.pose.position.y = 0.0; - point.longitudinal_velocity_mps = 1.0; - traj.points.push_back(point); - traj_pub->publish(traj); - test_utils::waitForMessage(node, this, received_control_command); - - ASSERT_TRUE(received_control_command); - EXPECT_DOUBLE_EQ(cmd_msg->longitudinal.speed, 1.0); - EXPECT_DOUBLE_EQ(cmd_msg->longitudinal.acceleration, 0.0); + Trajectory traj_msg; + traj_msg.header.stamp = tester.node->now(); + traj_msg.header.frame_id = "map"; + traj_msg.points.push_back(make_traj_point(0.0, 0.0, 1.0f)); + traj_msg.points.push_back(make_traj_point(50.0, 0.0, 1.0f)); + traj_msg.points.push_back(make_traj_point(100.0, 0.0, 1.0f)); + tester.traj_pub->publish(traj_msg); + + test_utils::waitForMessage(tester.node, this, tester.received_control_command); + + ASSERT_TRUE(tester.received_control_command); + EXPECT_DOUBLE_EQ(tester.cmd_msg->longitudinal.speed, 1.0); + EXPECT_DOUBLE_EQ(tester.cmd_msg->longitudinal.acceleration, 0.0); // Generate another control message - received_control_command = false; - traj_pub->publish(traj); - test_utils::waitForMessage(node, this, received_control_command); - ASSERT_TRUE(received_control_command); - EXPECT_DOUBLE_EQ(cmd_msg->longitudinal.speed, 1.0); - EXPECT_DOUBLE_EQ(cmd_msg->longitudinal.acceleration, 0.0); + tester.traj_pub->publish(traj_msg); + test_utils::waitForMessage(tester.node, this, tester.received_control_command); + ASSERT_TRUE(tester.received_control_command); + EXPECT_DOUBLE_EQ(tester.cmd_msg->longitudinal.speed, 1.0); + EXPECT_DOUBLE_EQ(tester.cmd_msg->longitudinal.acceleration, 0.0); } TEST_F(FakeNodeFixture, longitudinal_slow_down) { - // Data to test - AckermannControlCommand::SharedPtr cmd_msg; - bool received_control_command = false; - // Node - std::shared_ptr node = makeNode(); - // Publisher/Subscribers - rclcpp::Publisher::SharedPtr traj_pub = - this->create_publisher("controller/input/reference_trajectory"); - rclcpp::Publisher::SharedPtr odom_pub = - this->create_publisher("controller/input/current_odometry"); - rclcpp::Publisher::SharedPtr steer_pub = - this->create_publisher("controller/input/current_steering"); - rclcpp::Subscription::SharedPtr cmd_sub = - this->create_subscription( - "controller/output/control_cmd", *this->get_fake_node(), - [&cmd_msg, &received_control_command](const AckermannControlCommand::SharedPtr msg) { - cmd_msg = msg; - received_control_command = true; - }); - std::shared_ptr br = - std::make_shared(this->get_fake_node()); - - // Dummy transform: ego is at (0.0, 0.0) in map frame - geometry_msgs::msg::TransformStamped transform = test_utils::getDummyTransform(); - transform.header.stamp = node->now(); - br->sendTransform(transform); - /// Already running at target vel + Non stopping trajectory -> no change in velocity - // Publish velocity - VehicleOdometry odom_msg; - odom_msg.header.stamp = node->now(); - odom_msg.twist.twist.linear.x = 1.0; - odom_pub->publish(odom_msg); - // the node needs to receive two velocity msg - rclcpp::spin_some(node); - rclcpp::spin_some(this->get_fake_node()); - odom_msg.header.stamp = node->now(); - odom_pub->publish(odom_msg); - // Publish steering - SteeringReport steer_msg; - steer_msg.stamp = node->now(); - steer_msg.steering_tire_angle = 0.0; - steer_pub->publish(steer_msg); + ControllerTester tester(this); + + tester.send_default_transform(); + tester.publish_default_acc(); + tester.publish_default_steer(); + + const double odom_vx = 1.0; + tester.publish_odom_vx(odom_vx); + // Publish non stopping trajectory Trajectory traj; - traj.header.stamp = node->now(); + traj.header.stamp = tester.node->now(); traj.header.frame_id = "map"; - TrajectoryPoint point; - point.pose.position.x = 0.0; - point.pose.position.y = 0.0; - point.longitudinal_velocity_mps = 0.5; - traj.points.push_back(point); - point.pose.position.x = 50.0; - point.pose.position.y = 0.0; - point.longitudinal_velocity_mps = 0.5; - traj.points.push_back(point); - point.pose.position.x = 100.0; - point.pose.position.y = 0.0; - point.longitudinal_velocity_mps = 0.5; - traj.points.push_back(point); - traj_pub->publish(traj); - test_utils::waitForMessage(node, this, received_control_command); - - ASSERT_TRUE(received_control_command); - EXPECT_LT(cmd_msg->longitudinal.speed, static_cast(odom_msg.twist.twist.linear.x)); - EXPECT_LT(cmd_msg->longitudinal.acceleration, 0.0f); + traj.points.push_back(make_traj_point(0.0, 0.0, 0.5f)); + traj.points.push_back(make_traj_point(50.0, 0.0, 0.5f)); + traj.points.push_back(make_traj_point(100.0, 0.0, 0.5f)); + tester.traj_pub->publish(traj); + + test_utils::waitForMessage(tester.node, this, tester.received_control_command); + + ASSERT_TRUE(tester.received_control_command); + EXPECT_LT(tester.cmd_msg->longitudinal.speed, static_cast(odom_vx)); + EXPECT_LT(tester.cmd_msg->longitudinal.acceleration, 0.0f); // Generate another control message - received_control_command = false; - traj_pub->publish(traj); - test_utils::waitForMessage(node, this, received_control_command); - ASSERT_TRUE(received_control_command); - EXPECT_LT(cmd_msg->longitudinal.speed, static_cast(odom_msg.twist.twist.linear.x)); - EXPECT_LT(cmd_msg->longitudinal.acceleration, 0.0f); + tester.traj_pub->publish(traj); + test_utils::waitForMessage(tester.node, this, tester.received_control_command); + ASSERT_TRUE(tester.received_control_command); + EXPECT_LT(tester.cmd_msg->longitudinal.speed, static_cast(odom_vx)); + EXPECT_LT(tester.cmd_msg->longitudinal.acceleration, 0.0f); } TEST_F(FakeNodeFixture, longitudinal_accelerate) { - // Data to test - AckermannControlCommand::SharedPtr cmd_msg; - bool received_control_command = false; - // Node - std::shared_ptr node = makeNode(); - // Publisher/Subscribers - rclcpp::Publisher::SharedPtr traj_pub = - this->create_publisher("controller/input/reference_trajectory"); - rclcpp::Publisher::SharedPtr odom_pub = - this->create_publisher("controller/input/current_odometry"); - rclcpp::Publisher::SharedPtr steer_pub = - this->create_publisher("controller/input/current_steering"); - rclcpp::Subscription::SharedPtr cmd_sub = - this->create_subscription( - "controller/output/control_cmd", *this->get_fake_node(), - [&cmd_msg, &received_control_command](const AckermannControlCommand::SharedPtr msg) { - cmd_msg = msg; - received_control_command = true; - }); - std::shared_ptr br = - std::make_shared(this->get_fake_node()); - - // Dummy transform: ego is at (0.0, 0.0) in map frame - geometry_msgs::msg::TransformStamped transform = test_utils::getDummyTransform(); - transform.header.stamp = node->now(); - br->sendTransform(transform); - /// Below target vel + Non stopping trajectory -> accelerate - // Publish velocity - VehicleOdometry odom_msg; - odom_msg.header.stamp = node->now(); - odom_msg.twist.twist.linear.x = 0.5; - odom_pub->publish(odom_msg); - // the node needs to receive two velocity msg - rclcpp::spin_some(node); - rclcpp::spin_some(this->get_fake_node()); - odom_msg.header.stamp = node->now(); - odom_pub->publish(odom_msg); - // Publish steering - SteeringReport steer_msg; - steer_msg.stamp = node->now(); - steer_msg.steering_tire_angle = 0.0; - steer_pub->publish(steer_msg); + ControllerTester tester(this); + + tester.send_default_transform(); + tester.publish_default_steer(); + tester.publish_default_acc(); + + const double odom_vx = 0.5; + tester.publish_odom_vx(odom_vx); + // Publish non stopping trajectory Trajectory traj; - traj.header.stamp = node->now(); + traj.header.stamp = tester.node->now(); traj.header.frame_id = "map"; - TrajectoryPoint point; - point.pose.position.x = 0.0; - point.pose.position.y = 0.0; - point.longitudinal_velocity_mps = 1.0; - traj.points.push_back(point); - point.pose.position.x = 50.0; - point.pose.position.y = 0.0; - point.longitudinal_velocity_mps = 1.0; - traj.points.push_back(point); - point.pose.position.x = 100.0; - point.pose.position.y = 0.0; - point.longitudinal_velocity_mps = 1.0; - traj.points.push_back(point); - traj_pub->publish(traj); - test_utils::waitForMessage(node, this, received_control_command); - - ASSERT_TRUE(received_control_command); - EXPECT_GT(cmd_msg->longitudinal.speed, static_cast(odom_msg.twist.twist.linear.x)); - EXPECT_GT(cmd_msg->longitudinal.acceleration, 0.0f); + traj.points.push_back(make_traj_point(0.0, 0.0, 1.0f)); + traj.points.push_back(make_traj_point(50.0, 0.0, 1.0f)); + traj.points.push_back(make_traj_point(100.0, 0.0, 1.0f)); + tester.traj_pub->publish(traj); + + test_utils::waitForMessage(tester.node, this, tester.received_control_command); + + ASSERT_TRUE(tester.received_control_command); + EXPECT_GT(tester.cmd_msg->longitudinal.speed, static_cast(odom_vx)); + EXPECT_GT(tester.cmd_msg->longitudinal.acceleration, 0.0f); // Generate another control message - received_control_command = false; - traj_pub->publish(traj); - test_utils::waitForMessage(node, this, received_control_command); - ASSERT_TRUE(received_control_command); - EXPECT_GT(cmd_msg->longitudinal.speed, static_cast(odom_msg.twist.twist.linear.x)); - EXPECT_GT(cmd_msg->longitudinal.acceleration, 0.0f); + tester.traj_pub->publish(traj); + test_utils::waitForMessage(tester.node, this, tester.received_control_command); + ASSERT_TRUE(tester.received_control_command); + EXPECT_GT(tester.cmd_msg->longitudinal.speed, static_cast(odom_vx)); + EXPECT_GT(tester.cmd_msg->longitudinal.acceleration, 0.0f); } TEST_F(FakeNodeFixture, longitudinal_stopped) { - // Data to test - AckermannControlCommand::SharedPtr cmd_msg; - bool received_control_command = false; - // Node - std::shared_ptr node = makeNode(); - // Publisher/Subscribers - rclcpp::Publisher::SharedPtr traj_pub = - this->create_publisher("controller/input/reference_trajectory"); - rclcpp::Publisher::SharedPtr odom_pub = - this->create_publisher("controller/input/current_odometry"); - rclcpp::Publisher::SharedPtr steer_pub = - this->create_publisher("controller/input/current_steering"); - rclcpp::Subscription::SharedPtr cmd_sub = - this->create_subscription( - "controller/output/control_cmd", *this->get_fake_node(), - [&cmd_msg, &received_control_command](const AckermannControlCommand::SharedPtr msg) { - cmd_msg = msg; - received_control_command = true; - }); - std::shared_ptr br = - std::make_shared(this->get_fake_node()); - - // Dummy transform: ego is at (0.0, 0.0) in map frame - geometry_msgs::msg::TransformStamped transform = test_utils::getDummyTransform(); - transform.header.stamp = node->now(); - br->sendTransform(transform); - /// Below target vel + Non stopping trajectory -> accelerate - // Publish velocity - VehicleOdometry odom_msg; - odom_msg.header.stamp = node->now(); - odom_msg.twist.twist.linear.x = 0.0; - odom_pub->publish(odom_msg); - // the node needs to receive two velocity msg - rclcpp::spin_some(node); - rclcpp::spin_some(this->get_fake_node()); - odom_msg.header.stamp = node->now(); - odom_pub->publish(odom_msg); - // Publish steering - SteeringReport steer_msg; - steer_msg.stamp = node->now(); - steer_msg.steering_tire_angle = 0.0; - steer_pub->publish(steer_msg); + ControllerTester tester(this); + + tester.send_default_transform(); + tester.publish_default_odom(); + tester.publish_default_steer(); + tester.publish_default_acc(); + // Publish stopping trajectory Trajectory traj; - traj.header.stamp = node->now(); + traj.header.stamp = tester.node->now(); traj.header.frame_id = "map"; - TrajectoryPoint point; - point.pose.position.x = 0.0; - point.pose.position.y = 0.0; - point.longitudinal_velocity_mps = 0.0; - traj.points.push_back(point); - point.pose.position.x = 50.0; - point.pose.position.y = 0.0; - point.longitudinal_velocity_mps = 0.0; - traj.points.push_back(point); - point.pose.position.x = 100.0; - point.pose.position.y = 0.0; - point.longitudinal_velocity_mps = 0.0; - traj.points.push_back(point); - traj_pub->publish(traj); - test_utils::waitForMessage(node, this, received_control_command); - - ASSERT_TRUE(received_control_command); - EXPECT_DOUBLE_EQ(cmd_msg->longitudinal.speed, 0.0f); + traj.points.push_back(make_traj_point(0.0, 0.0, 0.0f)); + traj.points.push_back(make_traj_point(50.0, 0.0, 0.0f)); + traj.points.push_back(make_traj_point(100.0, 0.0, 0.0f)); + tester.traj_pub->publish(traj); + + test_utils::waitForMessage(tester.node, this, tester.received_control_command); + + ASSERT_TRUE(tester.received_control_command); + EXPECT_DOUBLE_EQ(tester.cmd_msg->longitudinal.speed, 0.0f); EXPECT_LT( - cmd_msg->longitudinal.acceleration, 0.0f); // when stopped negative acceleration to brake + tester.cmd_msg->longitudinal.acceleration, + 0.0f); // when stopped negative acceleration to brake } TEST_F(FakeNodeFixture, longitudinal_reverse) { - // Data to test - AckermannControlCommand::SharedPtr cmd_msg; - bool received_control_command = false; - // Node - std::shared_ptr node = makeNode(); - // Publisher/Subscribers - rclcpp::Publisher::SharedPtr traj_pub = - this->create_publisher("controller/input/reference_trajectory"); - rclcpp::Publisher::SharedPtr odom_pub = - this->create_publisher("controller/input/current_odometry"); - rclcpp::Publisher::SharedPtr steer_pub = - this->create_publisher("controller/input/current_steering"); - rclcpp::Subscription::SharedPtr cmd_sub = - this->create_subscription( - "controller/output/control_cmd", *this->get_fake_node(), - [&cmd_msg, &received_control_command](const AckermannControlCommand::SharedPtr msg) { - cmd_msg = msg; - received_control_command = true; - }); - std::shared_ptr br = - std::make_shared(this->get_fake_node()); - - // Dummy transform: ego is at (0.0, 0.0) in map frame - geometry_msgs::msg::TransformStamped transform = test_utils::getDummyTransform(); - transform.header.stamp = node->now(); - br->sendTransform(transform); - /// Below target vel + Non stopping trajectory -> accelerate - // Publish velocity - VehicleOdometry odom_msg; - odom_msg.header.stamp = node->now(); - odom_msg.twist.twist.linear.x = 0.0; - odom_pub->publish(odom_msg); - // the node needs to receive two velocity msg - rclcpp::spin_some(node); - rclcpp::spin_some(this->get_fake_node()); - odom_msg.header.stamp = node->now(); - odom_pub->publish(odom_msg); - // Publish steering - SteeringReport steer_msg; - steer_msg.stamp = node->now(); - steer_msg.steering_tire_angle = 0.0; - steer_pub->publish(steer_msg); + ControllerTester tester(this); + + tester.send_default_transform(); + + tester.publish_default_odom(); + tester.publish_default_steer(); + tester.publish_default_acc(); + // Publish reverse Trajectory traj; - traj.header.stamp = node->now(); + traj.header.stamp = tester.node->now(); traj.header.frame_id = "map"; - TrajectoryPoint point; - point.pose.position.x = 0.0; - point.pose.position.y = 0.0; - point.longitudinal_velocity_mps = -1.0; - traj.points.push_back(point); - point.pose.position.x = 50.0; - point.pose.position.y = 0.0; - point.longitudinal_velocity_mps = -1.0; - traj.points.push_back(point); - point.pose.position.x = 100.0; - point.pose.position.y = 0.0; - point.longitudinal_velocity_mps = -1.0; - traj.points.push_back(point); - traj_pub->publish(traj); - test_utils::waitForMessage(node, this, received_control_command); - - ASSERT_TRUE(received_control_command); - EXPECT_LT(cmd_msg->longitudinal.speed, 0.0f); - EXPECT_GT(cmd_msg->longitudinal.acceleration, 0.0f); + traj.points.push_back(make_traj_point(0.0, 0.0, -1.0f)); + traj.points.push_back(make_traj_point(50.0, 0.0, -1.0f)); + traj.points.push_back(make_traj_point(100.0, 0.0, -1.0f)); + tester.traj_pub->publish(traj); + + test_utils::waitForMessage(tester.node, this, tester.received_control_command); + + ASSERT_TRUE(tester.received_control_command); + EXPECT_LT(tester.cmd_msg->longitudinal.speed, 0.0f); + EXPECT_GT(tester.cmd_msg->longitudinal.acceleration, 0.0f); } TEST_F(FakeNodeFixture, longitudinal_emergency) { - // Data to test - AckermannControlCommand::SharedPtr cmd_msg; - bool received_control_command = false; - // Node - std::shared_ptr node = makeNode(); - // Publisher/Subscribers - rclcpp::Publisher::SharedPtr traj_pub = - this->create_publisher("controller/input/reference_trajectory"); - rclcpp::Publisher::SharedPtr odom_pub = - this->create_publisher("controller/input/current_odometry"); - rclcpp::Publisher::SharedPtr steer_pub = - this->create_publisher("controller/input/current_steering"); - rclcpp::Subscription::SharedPtr cmd_sub = - this->create_subscription( - "controller/output/control_cmd", *this->get_fake_node(), - [&cmd_msg, &received_control_command](const AckermannControlCommand::SharedPtr msg) { - cmd_msg = msg; - received_control_command = true; - }); - std::shared_ptr br = - std::make_shared(this->get_fake_node()); - - // Dummy transform: ego is at (0.0, 0.0) in map frame - geometry_msgs::msg::TransformStamped transform = test_utils::getDummyTransform(); - transform.header.stamp = node->now(); - br->sendTransform(transform); - /// Below target vel + Non stopping trajectory -> accelerate - // Publish velocity - VehicleOdometry odom_msg; - odom_msg.header.stamp = node->now(); - odom_msg.twist.twist.linear.x = 0.0; - odom_pub->publish(odom_msg); - // the node needs to receive two velocity msg - rclcpp::spin_some(node); - rclcpp::spin_some(this->get_fake_node()); - odom_msg.header.stamp = node->now(); - odom_pub->publish(odom_msg); - // Publish steering - SteeringReport steer_msg; - steer_msg.stamp = node->now(); - steer_msg.steering_tire_angle = 0.0; - steer_pub->publish(steer_msg); + ControllerTester tester(this); + + tester.send_default_transform(); + tester.publish_default_odom(); + tester.publish_default_steer(); + tester.publish_default_acc(); + // Publish trajectory starting away from the current ego pose Trajectory traj; - traj.header.stamp = node->now(); + traj.header.stamp = tester.node->now(); traj.header.frame_id = "map"; - TrajectoryPoint point; - point.pose.position.x = 10.0; - point.pose.position.y = 0.0; - point.longitudinal_velocity_mps = 1.0; - traj.points.push_back(point); - point.pose.position.x = 50.0; - point.pose.position.y = 0.0; - point.longitudinal_velocity_mps = 1.0; - traj.points.push_back(point); - point.pose.position.x = 100.0; - point.pose.position.y = 0.0; - point.longitudinal_velocity_mps = 1.0; - traj.points.push_back(point); - traj_pub->publish(traj); - test_utils::waitForMessage(node, this, received_control_command); - - ASSERT_TRUE(received_control_command); + traj.points.push_back(make_traj_point(10.0, 0.0, 1.0f)); + traj.points.push_back(make_traj_point(50.0, 0.0, 1.0f)); + traj.points.push_back(make_traj_point(100.0, 0.0, 1.0f)); + tester.traj_pub->publish(traj); + + test_utils::waitForMessage(tester.node, this, tester.received_control_command); + + ASSERT_TRUE(tester.received_control_command); // Emergencies (e.g., far from trajectory) produces braking command (0 vel, negative accel) - EXPECT_DOUBLE_EQ(cmd_msg->longitudinal.speed, 0.0f); - EXPECT_LT(cmd_msg->longitudinal.acceleration, 0.0f); + EXPECT_DOUBLE_EQ(tester.cmd_msg->longitudinal.speed, 0.0f); + EXPECT_LT(tester.cmd_msg->longitudinal.acceleration, 0.0f); } diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_filter.cpp b/control/vehicle_cmd_gate/src/vehicle_cmd_filter.cpp index 41ed6c9aa62fe..2a000d9902174 100644 --- a/control/vehicle_cmd_gate/src/vehicle_cmd_filter.cpp +++ b/control/vehicle_cmd_gate/src/vehicle_cmd_filter.cpp @@ -11,7 +11,8 @@ // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. -#include "vehicle_cmd_gate/vehicle_cmd_filter.hpp" + +#include "vehicle_cmd_filter.hpp" #include #include diff --git a/control/vehicle_cmd_gate/include/vehicle_cmd_gate/vehicle_cmd_filter.hpp b/control/vehicle_cmd_gate/src/vehicle_cmd_filter.hpp similarity index 94% rename from control/vehicle_cmd_gate/include/vehicle_cmd_gate/vehicle_cmd_filter.hpp rename to control/vehicle_cmd_gate/src/vehicle_cmd_filter.hpp index 5562240a35c49..4cb6b6a1cef8f 100644 --- a/control/vehicle_cmd_gate/include/vehicle_cmd_gate/vehicle_cmd_filter.hpp +++ b/control/vehicle_cmd_gate/src/vehicle_cmd_filter.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef VEHICLE_CMD_GATE__VEHICLE_CMD_FILTER_HPP_ -#define VEHICLE_CMD_GATE__VEHICLE_CMD_FILTER_HPP_ +#ifndef VEHICLE_CMD_FILTER_HPP_ +#define VEHICLE_CMD_FILTER_HPP_ #include @@ -74,4 +74,4 @@ class VehicleCmdFilter double limitDiff(const double curr, const double prev, const double diff_lim) const; }; -#endif // VEHICLE_CMD_GATE__VEHICLE_CMD_FILTER_HPP_ +#endif // VEHICLE_CMD_FILTER_HPP_ diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp index 4c36a36d476ec..c3fe705ac51a7 100644 --- a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp +++ b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "vehicle_cmd_gate/vehicle_cmd_gate.hpp" +#include "vehicle_cmd_gate.hpp" #include #include diff --git a/control/vehicle_cmd_gate/include/vehicle_cmd_gate/vehicle_cmd_gate.hpp b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp similarity index 98% rename from control/vehicle_cmd_gate/include/vehicle_cmd_gate/vehicle_cmd_gate.hpp rename to control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp index 3af3a5e8814e0..1c3da0ea6e6d0 100644 --- a/control/vehicle_cmd_gate/include/vehicle_cmd_gate/vehicle_cmd_gate.hpp +++ b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef VEHICLE_CMD_GATE__VEHICLE_CMD_GATE_HPP_ -#define VEHICLE_CMD_GATE__VEHICLE_CMD_GATE_HPP_ +#ifndef VEHICLE_CMD_GATE_HPP_ +#define VEHICLE_CMD_GATE_HPP_ -#include "vehicle_cmd_gate/vehicle_cmd_filter.hpp" +#include "vehicle_cmd_filter.hpp" #include #include @@ -252,4 +252,4 @@ class VehicleCmdGate : public rclcpp::Node }; } // namespace vehicle_cmd_gate -#endif // VEHICLE_CMD_GATE__VEHICLE_CMD_GATE_HPP_ +#endif // VEHICLE_CMD_GATE_HPP_ diff --git a/control/vehicle_cmd_gate/test/src/test_vehicle_cmd_filter.cpp b/control/vehicle_cmd_gate/test/src/test_vehicle_cmd_filter.cpp index 382380ce82b95..47c9bde90b8b2 100644 --- a/control/vehicle_cmd_gate/test/src/test_vehicle_cmd_filter.cpp +++ b/control/vehicle_cmd_gate/test/src/test_vehicle_cmd_filter.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "vehicle_cmd_gate/vehicle_cmd_filter.hpp" +#include "../../src/vehicle_cmd_filter.hpp" #include diff --git a/launch/tier4_autoware_api_launch/launch/autoware_api.launch.xml b/launch/tier4_autoware_api_launch/launch/autoware_api.launch.xml index a98ac01548732..1feed0327ef4d 100644 --- a/launch/tier4_autoware_api_launch/launch/autoware_api.launch.xml +++ b/launch/tier4_autoware_api_launch/launch/autoware_api.launch.xml @@ -3,13 +3,15 @@ + - - + + + diff --git a/launch/tier4_autoware_api_launch/launch/include/external_api_adaptor.launch.py b/launch/tier4_autoware_api_launch/launch/include/external_api_adaptor.launch.py index e742ad59cd4e6..fc730ff19bf40 100644 --- a/launch/tier4_autoware_api_launch/launch/include/external_api_adaptor.launch.py +++ b/launch/tier4_autoware_api_launch/launch/include/external_api_adaptor.launch.py @@ -54,6 +54,7 @@ def add_launch_arg(name: str, default_value=None, description=None): _create_api_node("operator", "Operator"), _create_api_node("metadata_packages", "MetadataPackages"), _create_api_node("route", "Route"), + _create_api_node("rtc_controller", "RTCController"), _create_api_node("service", "Service"), _create_api_node("vehicle_status", "VehicleStatus"), _create_api_node("velocity", "Velocity"), diff --git a/launch/tier4_autoware_api_launch/package.xml b/launch/tier4_autoware_api_launch/package.xml index 9b7abb00a8c91..44a78ff274eca 100644 --- a/launch/tier4_autoware_api_launch/package.xml +++ b/launch/tier4_autoware_api_launch/package.xml @@ -12,9 +12,11 @@ autoware_cmake + ad_api_adaptors autoware_iv_external_api_adaptor autoware_iv_internal_api_adaptor awapi_awiv_adapter + default_ad_api path_distance_calculator topic_tools diff --git a/launch/tier4_control_launch/launch/control.launch.py b/launch/tier4_control_launch/launch/control.launch.py index 471c1ad4541c0..044c5e3a8136a 100644 --- a/launch/tier4_control_launch/launch/control.launch.py +++ b/launch/tier4_control_launch/launch/control.launch.py @@ -99,6 +99,7 @@ def launch_setup(context, *args, **kwargs): ("~/input/reference_trajectory", "/planning/scenario_planning/trajectory"), ("~/input/current_odometry", "/localization/kinematic_state"), ("~/input/current_steering", "/vehicle/status/steering_status"), + ("~/input/current_accel", "/localization/acceleration"), ("~/output/predicted_trajectory", "lateral/predicted_trajectory"), ("~/output/lateral_diagnostic", "lateral/diagnostic"), ("~/output/slope_angle", "longitudinal/slope_angle"), diff --git a/launch/tier4_localization_launch/package.xml b/launch/tier4_localization_launch/package.xml index e77c988f4ed26..13102ea5f9b70 100644 --- a/launch/tier4_localization_launch/package.xml +++ b/launch/tier4_localization_launch/package.xml @@ -12,6 +12,7 @@ autoware_cmake + automatic_pose_initializer ekf_localizer gyro_odometer ndt_scan_matcher diff --git a/launch/tier4_perception_launch/config/object_recognition/tracking/multi_object_tracker/data_association_matrix.param.yaml b/launch/tier4_perception_launch/config/object_recognition/tracking/multi_object_tracker/data_association_matrix.param.yaml index 195b558c35d07..1790315ae557d 100644 --- a/launch/tier4_perception_launch/config/object_recognition/tracking/multi_object_tracker/data_association_matrix.param.yaml +++ b/launch/tier4_perception_launch/config/object_recognition/tracking/multi_object_tracker/data_association_matrix.param.yaml @@ -26,10 +26,10 @@ # NOTE(yukke42): The size of trailer is 20 m length x 3 m width. #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN [100.00, 100.00, 100.00, 100.00, 100.00, 100.00, 100.00, 100.00, #UNKNOWN - 12.10, 12.10, 36.00, 40.00, 60.00, 10000.00, 10000.00, 10000.00, #CAR - 36.00, 12.10, 36.00, 40.00, 60.00, 10000.00, 10000.00, 10000.00, #TRUCK - 40.00, 12.10, 36.00, 40.00, 60.00, 10000.00, 10000.00, 10000.00, #BUS - 60.00, 12.10, 36.00, 40.00, 60.00, 10000.00, 10000.00, 10000.00, #TRAILER + 12.10, 12.10, 36.00, 60.00, 60.00, 10000.00, 10000.00, 10000.00, #CAR + 36.00, 12.10, 36.00, 60.00, 60.00, 10000.00, 10000.00, 10000.00, #TRUCK + 60.00, 12.10, 36.00, 60.00, 60.00, 10000.00, 10000.00, 10000.00, #BUS + 60.00, 12.10, 36.00, 60.00, 60.00, 10000.00, 10000.00, 10000.00, #TRAILER 2.50, 10000.00, 10000.00, 10000.00, 10000.00, 2.50, 2.50, 1.00, #MOTORBIKE 2.50, 10000.00, 10000.00, 10000.00, 10000.00, 2.50, 2.50, 1.00, #BICYCLE 2.00, 10000.00, 10000.00, 10000.00, 10000.00, 1.50, 1.50, 1.00] #PEDESTRIAN diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml index 775456e4721ec..9778aaf079eca 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml @@ -28,6 +28,9 @@ + + + - - - - - + + + + + + + + + + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml index 728948c10b1bd..5746033e98ae1 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml @@ -11,6 +11,9 @@ + + + @@ -68,6 +71,9 @@ + + + diff --git a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml index 0ea817ffefc81..79eb257de6889 100644 --- a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml +++ b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml @@ -14,6 +14,7 @@ detection_area_right_margin: 0.5 # [m] detection_area_left_margin: 0.5 # [m] detection_area_length: 200.0 # [m] + detection_area_angle_threshold: 0.785 # [rad] min_predicted_path_confidence: 0.05 collision_start_margin_time: 5.0 # [s] collision_end_margin_time: 2.0 # [s] diff --git a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml index 240ea8def4db8..f8f9a85deed5c 100644 --- a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml +++ b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml @@ -11,8 +11,6 @@ min_object_accel_for_rss: -1.0 # front obstacle's acceleration to calculate RSS distance [m/ss] safe_distance_margin : 6.0 # This is also used as a stop margin [m] - lpf_gain_for_accel: 0.2 # gain of low pass filter for ego acceleration [-] - nearest_dist_deviation_threshold: 3.0 # [m] for finding nearest index nearest_yaw_deviation_threshold: 1.57 # [rad] for finding nearest index min_behavior_stop_margin: 3.0 # [m] diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.py b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.py index f86b9a8858bff..99ab74f0856fe 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.py +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.py @@ -160,6 +160,7 @@ def launch_setup(context, *args, **kwargs): "/planning/scenario_planning/clear_velocity_limit", ), ("~/output/trajectory", "/planning/scenario_planning/lane_driving/trajectory"), + ("~/input/acceleration", "/localization/acceleration"), ( "~/input/pointcloud", "/perception/obstacle_segmentation/pointcloud", @@ -198,6 +199,7 @@ def launch_setup(context, *args, **kwargs): remappings=[ ("~/input/trajectory", "obstacle_avoidance_planner/trajectory"), ("~/input/odometry", "/localization/kinematic_state"), + ("~/input/acceleration", "/localization/acceleration"), ("~/input/objects", "/perception/object_recognition/objects"), ("~/output/trajectory", "/planning/scenario_planning/lane_driving/trajectory"), ("~/output/velocity_limit", "/planning/scenario_planning/max_velocity_candidates"), diff --git a/launch/tier4_planning_launch/package.xml b/launch/tier4_planning_launch/package.xml index e7b0bdbdd3bfe..f9cbe4541961e 100644 --- a/launch/tier4_planning_launch/package.xml +++ b/launch/tier4_planning_launch/package.xml @@ -11,14 +11,18 @@ autoware_cmake + behavior_path_planner behavior_velocity_planner costmap_generator + external_cmd_selector external_velocity_limit_selector freespace_planner mission_planner motion_velocity_smoother obstacle_avoidance_planner + obstacle_cruise_planner obstacle_stop_planner + planning_error_monitor scenario_selector surround_obstacle_checker diff --git a/launch/tier4_system_launch/config/system_monitor/hdd_monitor.param.yaml b/launch/tier4_system_launch/config/system_monitor/hdd_monitor.param.yaml index 77a23eb0f9aa8..04dd3afd09280 100644 --- a/launch/tier4_system_launch/config/system_monitor/hdd_monitor.param.yaml +++ b/launch/tier4_system_launch/config/system_monitor/hdd_monitor.param.yaml @@ -5,10 +5,19 @@ disks: # Until multi type lists are allowed, name N the disks as disk0...disk{N-1} disk0: name: /dev/sda3 + temp_attribute_id: 0xC2 temp_warn: 55.0 temp_error: 70.0 + power_on_hours_attribute_id: 0x09 power_on_hours_warn: 3000000 + total_data_written_attribute_id: 0xF1 total_data_written_warn: 4915200 # =150TB (1unit=32MB) total_data_written_safety_factor: 0.05 + recovered_error_attribute_id: 0xC3 + recovered_error_warn: 1 free_warn: 5120 # MB(8hour) free_error: 100 # MB(last 1 minute) + read_data_rate_warn: 360.0 # MB/s + write_data_rate_warn: 103.5 # MB/s + read_iops_warn: 63360.0 # IOPS + write_iops_warn: 24120.0 # IOPS diff --git a/launch/tier4_system_launch/config/system_monitor/net_monitor.param.yaml b/launch/tier4_system_launch/config/system_monitor/net_monitor.param.yaml index e91aa4ca3fbb3..d72b8d1334c17 100644 --- a/launch/tier4_system_launch/config/system_monitor/net_monitor.param.yaml +++ b/launch/tier4_system_launch/config/system_monitor/net_monitor.param.yaml @@ -5,3 +5,5 @@ monitor_program: "greengrass" crc_error_check_duration: 1 crc_error_count_threshold: 1 + reassembles_failed_check_duration: 1 + reassembles_failed_check_count: 1 diff --git a/launch/tier4_system_launch/package.xml b/launch/tier4_system_launch/package.xml index f5675b60cce3c..f496ac854ace3 100644 --- a/launch/tier4_system_launch/package.xml +++ b/launch/tier4_system_launch/package.xml @@ -5,6 +5,7 @@ 0.1.0 The tier4_system_launch package Kenji Miyake + Fumihito Ito Apache License 2.0 ament_cmake_auto diff --git a/localization/ekf_localizer/CMakeLists.txt b/localization/ekf_localizer/CMakeLists.txt index dea57e13b2073..f38815d303d05 100644 --- a/localization/ekf_localizer/CMakeLists.txt +++ b/localization/ekf_localizer/CMakeLists.txt @@ -15,6 +15,7 @@ ament_auto_find_build_dependencies() ament_auto_add_library(ekf_localizer_lib SHARED src/ekf_localizer.cpp + src/mahalanobis.cpp src/measurement.cpp src/state_transition.cpp ) @@ -41,7 +42,9 @@ if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) set(TEST_FILES + test/test_mahalanobis.cpp test/test_measurement.cpp + test/test_numeric.cpp test/test_state_transition.cpp) foreach(filepath ${TEST_FILES}) diff --git a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp index fbe4e3719f488..584325b67d1da 100644 --- a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp @@ -266,18 +266,6 @@ class EKFLocalizer : public rclcpp::Node */ void measurementUpdateTwist(const geometry_msgs::msg::TwistWithCovarianceStamped & twist); - /** - * @brief check whether a measurement value falls within the mahalanobis distance threshold - * @param dist_max mahalanobis distance threshold - * @param estimated current estimated state - * @param measured measured state - * @param estimated_cov current estimation covariance - * @return whether it falls within the mahalanobis distance threshold - */ - bool mahalanobisGate( - const double & dist_max, const Eigen::MatrixXd & estimated, const Eigen::MatrixXd & measured, - const Eigen::MatrixXd & estimated_cov) const; - /** * @brief get transform from frame_id */ diff --git a/localization/ekf_localizer/include/ekf_localizer/mahalanobis.hpp b/localization/ekf_localizer/include/ekf_localizer/mahalanobis.hpp new file mode 100644 index 0000000000000..ff7c9bcb29270 --- /dev/null +++ b/localization/ekf_localizer/include/ekf_localizer/mahalanobis.hpp @@ -0,0 +1,28 @@ +// Copyright 2022 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef EKF_LOCALIZER__MAHALANOBIS_HPP_ +#define EKF_LOCALIZER__MAHALANOBIS_HPP_ + +#include +#include + +double squaredMahalanobis( + const Eigen::VectorXd & x, const Eigen::VectorXd & y, const Eigen::MatrixXd & C); + +bool mahalanobisGate( + const double & dist_max, const Eigen::MatrixXd & x, const Eigen::MatrixXd & obj_x, + const Eigen::MatrixXd & cov); + +#endif // EKF_LOCALIZER__MAHALANOBIS_HPP_ diff --git a/localization/ekf_localizer/include/ekf_localizer/numeric.hpp b/localization/ekf_localizer/include/ekf_localizer/numeric.hpp new file mode 100644 index 0000000000000..0719c64e65ac1 --- /dev/null +++ b/localization/ekf_localizer/include/ekf_localizer/numeric.hpp @@ -0,0 +1,26 @@ +// Copyright 2022 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef EKF_LOCALIZER__NUMERIC_HPP_ +#define EKF_LOCALIZER__NUMERIC_HPP_ + +#include + +#include + +inline bool hasInf(const Eigen::MatrixXd & v) { return v.array().isInf().any(); } + +inline bool hasNan(const Eigen::MatrixXd & v) { return v.array().isNaN().any(); } + +#endif // EKF_LOCALIZER__NUMERIC_HPP_ diff --git a/localization/ekf_localizer/src/ekf_localizer.cpp b/localization/ekf_localizer/src/ekf_localizer.cpp index dcece543291f7..be9bba8197be2 100644 --- a/localization/ekf_localizer/src/ekf_localizer.cpp +++ b/localization/ekf_localizer/src/ekf_localizer.cpp @@ -14,8 +14,10 @@ #include "ekf_localizer/ekf_localizer.hpp" +#include "ekf_localizer/mahalanobis.hpp" #include "ekf_localizer/matrix_types.hpp" #include "ekf_localizer/measurement.hpp" +#include "ekf_localizer/numeric.hpp" #include "ekf_localizer/state_index.hpp" #include "ekf_localizer/state_transition.hpp" #include "ekf_localizer/warning.hpp" @@ -484,7 +486,7 @@ void EKFLocalizer::measurementUpdatePose(const geometry_msgs::msg::PoseWithCovar Eigen::MatrixXd y(dim_y, 1); y << pose.pose.pose.position.x, pose.pose.pose.position.y, yaw; - if (isnan(y.array()).any() || isinf(y.array()).any()) { + if (hasNan(y) || hasInf(y)) { warning_.warn( "[EKF] pose measurement matrix includes NaN of Inf. ignore update. check pose message."); return; @@ -565,7 +567,7 @@ void EKFLocalizer::measurementUpdateTwist( Eigen::MatrixXd y(dim_y, 1); y << twist.twist.twist.linear.x, twist.twist.twist.angular.z; - if (isnan(y.array()).any() || isinf(y.array()).any()) { + if (hasNan(y) || hasInf(y)) { warning_.warn( "[EKF] twist measurement matrix includes NaN of Inf. ignore update. check twist message."); return; @@ -603,24 +605,6 @@ void EKFLocalizer::measurementUpdateTwist( DEBUG_PRINT_MAT((X_result - X_curr).transpose()); } -/* - * mahalanobisGate - */ -bool EKFLocalizer::mahalanobisGate( - const double & dist_max, const Eigen::MatrixXd & x, const Eigen::MatrixXd & obj_x, - const Eigen::MatrixXd & cov) const -{ - Eigen::MatrixXd mahalanobis_squared = (x - obj_x).transpose() * cov.inverse() * (x - obj_x); - DEBUG_INFO( - get_logger(), "measurement update: mahalanobis = %f, gate limit = %f", - std::sqrt(mahalanobis_squared(0)), dist_max); - if (mahalanobis_squared(0) > dist_max * dist_max) { - return false; - } - - return true; -} - /* * publishEstimateResult */ diff --git a/localization/ekf_localizer/src/mahalanobis.cpp b/localization/ekf_localizer/src/mahalanobis.cpp new file mode 100644 index 0000000000000..0584165855bdb --- /dev/null +++ b/localization/ekf_localizer/src/mahalanobis.cpp @@ -0,0 +1,29 @@ +// Copyright 2022 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "ekf_localizer/mahalanobis.hpp" + +double squaredMahalanobis( + const Eigen::VectorXd & x, const Eigen::VectorXd & y, const Eigen::MatrixXd & C) +{ + const Eigen::VectorXd d = x - y; + return d.dot(C.inverse() * d); +} + +bool mahalanobisGate( + const double & dist_max, const Eigen::MatrixXd & x, const Eigen::MatrixXd & obj_x, + const Eigen::MatrixXd & cov) +{ + return squaredMahalanobis(x, obj_x, cov) <= dist_max * dist_max; +} diff --git a/localization/ekf_localizer/test/test_mahalanobis.cpp b/localization/ekf_localizer/test/test_mahalanobis.cpp new file mode 100644 index 0000000000000..a9f6c1a1bc38b --- /dev/null +++ b/localization/ekf_localizer/test/test_mahalanobis.cpp @@ -0,0 +1,52 @@ +// Copyright 2022 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "ekf_localizer/mahalanobis.hpp" + +#include + +constexpr double tolerance = 1e-8; + +TEST(MahalanobisGate, SquaredMahalanobis) +{ + { + Eigen::Vector2d x(0, 1); + Eigen::Vector2d y(3, 2); + Eigen::Matrix2d C; + C << 10, 0, 0, 10; + + EXPECT_NEAR(squaredMahalanobis(x, y, C), 1.0, tolerance); + } + + { + Eigen::Vector2d x(4, 1); + Eigen::Vector2d y(1, 5); + Eigen::Matrix2d C; + C << 5, 0, 0, 5; + + EXPECT_NEAR(squaredMahalanobis(x, y, C), 5.0, tolerance); + } +} + +TEST(MahalanobisGate, MahalanobisGate) +{ + Eigen::Vector2d x(0, 1); + Eigen::Vector2d y(3, 2); + Eigen::Matrix2d C; + C << 10, 0, 0, 10; + + EXPECT_FALSE(mahalanobisGate(0.99, x, y, C)); + EXPECT_FALSE(mahalanobisGate(1.00, x, y, C)); + EXPECT_TRUE(mahalanobisGate(1.01, x, y, C)); +} diff --git a/localization/ekf_localizer/test/test_numeric.cpp b/localization/ekf_localizer/test/test_numeric.cpp new file mode 100644 index 0000000000000..baf4f46db79a9 --- /dev/null +++ b/localization/ekf_localizer/test/test_numeric.cpp @@ -0,0 +1,47 @@ +// Copyright 2022 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "ekf_localizer/numeric.hpp" + +#include + +#include + +TEST(Numeric, HasNan) +{ + const Eigen::VectorXd empty(0); + const double inf = std::numeric_limits::infinity(); + const double nan = std::nan(""); + + EXPECT_FALSE(hasNan(empty)); + EXPECT_FALSE(hasNan(Eigen::Vector3d(0., 0., 1.))); + EXPECT_FALSE(hasNan(Eigen::Vector3d(1e16, 0., 1.))); + EXPECT_FALSE(hasNan(Eigen::Vector3d(0., 1., inf))); + + EXPECT_TRUE(hasNan(Eigen::Vector3d(nan, 1., 0.))); +} + +TEST(Numeric, HasInf) +{ + const Eigen::VectorXd empty(0); + const double inf = std::numeric_limits::infinity(); + const double nan = std::nan(""); + + EXPECT_FALSE(hasInf(empty)); + EXPECT_FALSE(hasInf(Eigen::Vector3d(0., 0., 1.))); + EXPECT_FALSE(hasInf(Eigen::Vector3d(1e16, 0., 1.))); + EXPECT_FALSE(hasInf(Eigen::Vector3d(nan, 1., 0.))); + + EXPECT_TRUE(hasInf(Eigen::Vector3d(0., 1., inf))); +} diff --git a/localization/pose_initializer/docs/pose_initializer.md b/localization/pose_initializer/docs/pose_initializer.md index 08d27ae83b51d..c1b2cc3d8b353 100644 --- a/localization/pose_initializer/docs/pose_initializer.md +++ b/localization/pose_initializer/docs/pose_initializer.md @@ -21,9 +21,9 @@ Finally, it publishes the initial pose to `ekf_localizer`. ### Services -| Name | Type | Description | -| -------------------------- | ------------------------------------------------- | --------------------- | -| `/localization/initialize` | autoware_ad_api_msgs::srv::InitializeLocalization | initial pose from api | +| Name | Type | Description | +| -------------------------- | --------------------------------------------------- | --------------------- | +| `/localization/initialize` | autoware_adapi_v1_msgs::srv::InitializeLocalization | initial pose from api | ### Clients @@ -40,7 +40,7 @@ Finally, it publishes the initial pose to `ekf_localizer`. ### Publications -| Name | Type | Description | -| ------------------------------------ | ---------------------------------------------------------- | --------------------------- | -| `/localization/initialization_state` | autoware_ad_api_msgs::msg::LocalizationInitializationState | pose initialization state | -| `/initialpose3d` | geometry_msgs::msg::PoseWithCovarianceStamped | calculated initial ego pose | +| Name | Type | Description | +| ------------------------------------ | ------------------------------------------------------------ | --------------------------- | +| `/localization/initialization_state` | autoware_adapi_v1_msgs::msg::LocalizationInitializationState | pose initialization state | +| `/initialpose3d` | geometry_msgs::msg::PoseWithCovarianceStamped | calculated initial ego pose | diff --git a/map/map_loader/include/map_loader/lanelet2_map_loader_node.hpp b/map/map_loader/include/map_loader/lanelet2_map_loader_node.hpp index 33f33a05e3393..72408f4af8185 100644 --- a/map/map_loader/include/map_loader/lanelet2_map_loader_node.hpp +++ b/map/map_loader/include/map_loader/lanelet2_map_loader_node.hpp @@ -22,6 +22,9 @@ #include #include +#include + +using autoware_auto_mapping_msgs::msg::HADMapBin; class Lanelet2MapLoaderNode : public rclcpp::Node { @@ -29,7 +32,12 @@ class Lanelet2MapLoaderNode : public rclcpp::Node explicit Lanelet2MapLoaderNode(const rclcpp::NodeOptions & options); private: - rclcpp::Publisher::SharedPtr pub_map_bin_; + rclcpp::Publisher::SharedPtr pub_map_bin_; + + lanelet::LaneletMapPtr load_map( + const std::string & lanelet2_filename, const std::string & lanelet2_map_projector_type); + HADMapBin create_map_bin_msg( + const lanelet::LaneletMapPtr map, const std::string & lanelet2_filename); }; #endif // MAP_LOADER__LANELET2_MAP_LOADER_NODE_HPP_ diff --git a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp index 58330e44abf5c..dc5d560b98e8e 100644 --- a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp +++ b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp @@ -51,47 +51,73 @@ Lanelet2MapLoaderNode::Lanelet2MapLoaderNode(const rclcpp::NodeOptions & options { const auto lanelet2_filename = declare_parameter("lanelet2_map_path", ""); const auto lanelet2_map_projector_type = declare_parameter("lanelet2_map_projector_type", "MGRS"); + const auto center_line_resolution = declare_parameter("center_line_resolution", 5.0); + + // load map from file + const auto map = load_map(lanelet2_filename, lanelet2_map_projector_type); + if (!map) { + return; + } + + // overwrite centerline + lanelet::utils::overwriteLaneletsCenterline(map, center_line_resolution, false); + + // create map bin msg + const auto map_bin_msg = create_map_bin_msg(map, lanelet2_filename); + + // create publisher and publish + pub_map_bin_ = + create_publisher("output/lanelet2_map", rclcpp::QoS{1}.transient_local()); + pub_map_bin_->publish(map_bin_msg); +} + +lanelet::LaneletMapPtr Lanelet2MapLoaderNode::load_map( + const std::string & lanelet2_filename, const std::string & lanelet2_map_projector_type) +{ lanelet::ErrorMessages errors{}; - lanelet::LaneletMapPtr map; if (lanelet2_map_projector_type == "MGRS") { lanelet::projection::MGRSProjector projector{}; - map = lanelet::load(lanelet2_filename, projector, &errors); + const lanelet::LaneletMapPtr map = lanelet::load(lanelet2_filename, projector, &errors); + if (errors.empty()) { + return map; + } } else if (lanelet2_map_projector_type == "UTM") { - double map_origin_lat = this->declare_parameter("latitude", 0.0); - double map_origin_lon = this->declare_parameter("longitude", 0.0); + const double map_origin_lat = declare_parameter("latitude", 0.0); + const double map_origin_lon = declare_parameter("longitude", 0.0); lanelet::GPSPoint position{map_origin_lat, map_origin_lon}; lanelet::Origin origin{position}; lanelet::projection::UtmProjector projector{origin}; - map = lanelet::load(lanelet2_filename, projector, &errors); + + const lanelet::LaneletMapPtr map = lanelet::load(lanelet2_filename, projector, &errors); + if (errors.empty()) { + return map; + } } else { - RCLCPP_ERROR(this->get_logger(), "lanelet2_map_projector_type is not supported"); + RCLCPP_ERROR(get_logger(), "lanelet2_map_projector_type is not supported"); + return nullptr; } for (const auto & error : errors) { - RCLCPP_ERROR_STREAM(this->get_logger(), error); + RCLCPP_ERROR_STREAM(get_logger(), error); } - if (!errors.empty()) { - return; - } - - const auto center_line_resolution = this->declare_parameter("center_line_resolution", 5.0); - lanelet::utils::overwriteLaneletsCenterline(map, center_line_resolution, false); + return nullptr; +} +HADMapBin Lanelet2MapLoaderNode::create_map_bin_msg( + const lanelet::LaneletMapPtr map, const std::string & lanelet2_filename) +{ std::string format_version{}, map_version{}; lanelet::io_handlers::AutowareOsmParser::parseVersions( lanelet2_filename, &format_version, &map_version); - pub_map_bin_ = this->create_publisher( - "output/lanelet2_map", rclcpp::QoS{1}.transient_local()); - - autoware_auto_mapping_msgs::msg::HADMapBin map_bin_msg; - map_bin_msg.header.stamp = this->now(); + HADMapBin map_bin_msg; + map_bin_msg.header.stamp = now(); map_bin_msg.header.frame_id = "map"; map_bin_msg.format_version = format_version; map_bin_msg.map_version = map_version; lanelet::utils::conversion::toBinMsg(map, &map_bin_msg); - pub_map_bin_->publish(map_bin_msg); + return map_bin_msg; } #include diff --git a/perception/image_projection_based_fusion/CMakeLists.txt b/perception/image_projection_based_fusion/CMakeLists.txt index ad048abd41962..c44a6fb3cc215 100644 --- a/perception/image_projection_based_fusion/CMakeLists.txt +++ b/perception/image_projection_based_fusion/CMakeLists.txt @@ -177,6 +177,11 @@ if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL) EXECUTABLE pointpainting_fusion_node ) + install( + TARGETS pointpainting_cuda_lib + DESTINATION lib + ) + else() message("Skipping build of some nodes due to missing dependencies") endif() diff --git a/perception/image_projection_based_fusion/config/pointpainting.param.yaml b/perception/image_projection_based_fusion/config/pointpainting.param.yaml index 3aeebe2db4148..0be983c455dce 100755 --- a/perception/image_projection_based_fusion/config/pointpainting.param.yaml +++ b/perception/image_projection_based_fusion/config/pointpainting.param.yaml @@ -1,7 +1,6 @@ /**: ros__parameters: class_names: ["CAR", "PEDESTRIAN", "BICYCLE"] - rename_car_to_truck_and_bus: true point_feature_size: 6 # x, y, z and car, pedestrian, bicycle max_voxel_size: 40000 point_cloud_range: [-76.8, -76.8, -3.0, 76.8, 76.8, 5.0] diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/node.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/node.hpp index 117722b72320c..53f21089b768d 100644 --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/node.hpp +++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/node.hpp @@ -21,6 +21,7 @@ #include #include #include +#include #include #include @@ -52,9 +53,10 @@ class PointpaintingFusionNode : public FusionNode class_names_; std::vector pointcloud_range; - bool rename_car_to_truck_and_bus_{false}; bool has_twist_{false}; + centerpoint::DetectionClassRemapper detection_class_remapper_; + std::unique_ptr detector_ptr_{nullptr}; bool out_of_scope(const DetectedObjects & obj); 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 3e44fddb7c5fd..d02a6484207b9 100644 --- a/perception/image_projection_based_fusion/launch/pointpainting_fusion.launch.xml +++ b/perception/image_projection_based_fusion/launch/pointpainting_fusion.launch.xml @@ -21,6 +21,7 @@ + @@ -74,6 +75,7 @@ + diff --git a/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp b/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp index 4b1a351739fc5..0b8fcfb2840ba 100644 --- a/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp +++ b/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp @@ -47,7 +47,6 @@ PointpaintingFusionNode::PointpaintingFusionNode(const rclcpp::NodeOptions & opt const std::string head_onnx_path = this->declare_parameter("head_onnx_path", ""); const std::string head_engine_path = this->declare_parameter("head_engine_path", ""); class_names_ = this->declare_parameter>("class_names"); - rename_car_to_truck_and_bus_ = this->declare_parameter("rename_car_to_truck_and_bus", false); has_twist_ = this->declare_parameter("has_twist", false); const std::size_t point_feature_size = static_cast(this->declare_parameter("point_feature_size")); @@ -59,6 +58,13 @@ PointpaintingFusionNode::PointpaintingFusionNode(const rclcpp::NodeOptions & opt static_cast(this->declare_parameter("downsample_factor")); const std::size_t encoder_in_feature_size = static_cast(this->declare_parameter("encoder_in_feature_size")); + const auto allow_remapping_by_area_matrix = + this->declare_parameter>("allow_remapping_by_area_matrix"); + const auto min_area_matrix = this->declare_parameter>("min_area_matrix"); + const auto max_area_matrix = this->declare_parameter>("max_area_matrix"); + + detection_class_remapper_.setParameters( + allow_remapping_by_area_matrix, min_area_matrix, max_area_matrix); centerpoint::NetworkParam encoder_param(encoder_onnx_path, encoder_engine_path, trt_precision); centerpoint::NetworkParam head_param(head_onnx_path, head_engine_path, trt_precision); @@ -240,11 +246,12 @@ void PointpaintingFusionNode::postprocess(sensor_msgs::msg::PointCloud2 & painte continue; } autoware_auto_perception_msgs::msg::DetectedObject obj; - centerpoint::box3DToDetectedObject( - box3d, class_names_, rename_car_to_truck_and_bus_, has_twist_, obj); + centerpoint::box3DToDetectedObject(box3d, class_names_, has_twist_, obj); output_obj_msg.objects.emplace_back(obj); } + detection_class_remapper_.mapClasses(output_obj_msg); + obj_pub_ptr_->publish(output_obj_msg); } diff --git a/perception/image_projection_based_fusion/src/utils/geometry.cpp b/perception/image_projection_based_fusion/src/utils/geometry.cpp index a365f6b0decd9..dc35b754d0f01 100644 --- a/perception/image_projection_based_fusion/src/utils/geometry.cpp +++ b/perception/image_projection_based_fusion/src/utils/geometry.cpp @@ -122,7 +122,7 @@ void boundingBoxToVertices( const auto position = Eigen::Vector3d(pose.position.x, pose.position.y, pose.position.z); const auto orientation = Eigen::Quaterniond( - pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w); + pose.orientation.w, pose.orientation.x, pose.orientation.y, pose.orientation.z); for (const auto & corner : corners_template) { Eigen::Vector3d corner_point( diff --git a/perception/lidar_centerpoint/CMakeLists.txt b/perception/lidar_centerpoint/CMakeLists.txt index 206e490b4aec8..646267b0d7a61 100644 --- a/perception/lidar_centerpoint/CMakeLists.txt +++ b/perception/lidar_centerpoint/CMakeLists.txt @@ -125,6 +125,7 @@ if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL) ### centerpoint ### ament_auto_add_library(centerpoint_lib SHARED lib/centerpoint_trt.cpp + lib/detection_class_remapper.cpp lib/utils.cpp lib/ros_utils.cpp lib/network/network_trt.cpp @@ -183,6 +184,11 @@ if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL) data config ) + + install( + TARGETS centerpoint_cuda_lib + DESTINATION lib + ) else() find_package(ament_cmake_auto REQUIRED) ament_auto_find_build_dependencies() diff --git a/perception/lidar_centerpoint/config/centerpoint.param.yaml b/perception/lidar_centerpoint/config/centerpoint.param.yaml index 34dce71975985..3810c864738af 100644 --- a/perception/lidar_centerpoint/config/centerpoint.param.yaml +++ b/perception/lidar_centerpoint/config/centerpoint.param.yaml @@ -1,7 +1,6 @@ /**: ros__parameters: class_names: ["CAR", "PEDESTRIAN", "BICYCLE"] - rename_car_to_truck_and_bus: true point_feature_size: 4 max_voxel_size: 40000 point_cloud_range: [-89.6, -89.6, -3.0, 89.6, 89.6, 5.0] diff --git a/perception/lidar_centerpoint/config/centerpoint_tiny.param.yaml b/perception/lidar_centerpoint/config/centerpoint_tiny.param.yaml index 0b23c48887b6f..c93b59ac0b8f9 100644 --- a/perception/lidar_centerpoint/config/centerpoint_tiny.param.yaml +++ b/perception/lidar_centerpoint/config/centerpoint_tiny.param.yaml @@ -1,7 +1,6 @@ /**: ros__parameters: class_names: ["CAR", "PEDESTRIAN", "BICYCLE"] - rename_car_to_truck_and_bus: true point_feature_size: 4 max_voxel_size: 40000 point_cloud_range: [-89.6, -89.6, -3.0, 89.6, 89.6, 5.0] diff --git a/perception/lidar_centerpoint/config/detection_class_remapper.param.yaml b/perception/lidar_centerpoint/config/detection_class_remapper.param.yaml new file mode 100644 index 0000000000000..ed378ffa44a70 --- /dev/null +++ b/perception/lidar_centerpoint/config/detection_class_remapper.param.yaml @@ -0,0 +1,38 @@ +/**: + ros__parameters: + allow_remapping_by_area_matrix: + # NOTE(kl): We turn all vehicles into trailers if they go over 3x12 [m^2]. + # NOTE(kl): We turn cars into trucks if they have an area between 2.2 x 5.5 and 3.0 * 12.0 [m^2] + # row: original class. column: class to remap to + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE,PEDESTRIAN + [0, 0, 0, 0, 0, 0, 0, 0, #UNKNOWN + 0, 0, 1, 0, 1, 0, 0, 0, #CAR + 0, 0, 0, 0, 1, 0, 0, 0, #TRUCK + 0, 0, 0, 0, 1, 0, 0, 0, #BUS + 0, 0, 0, 0, 0, 0, 0, 0, #TRAILER + 0, 0, 0, 0, 0, 0, 0, 0, #MOTORBIKE + 0, 0, 0, 0, 0, 0, 0, 0, #BICYCLE + 0, 0, 0, 0, 0, 0, 0, 0] #PEDESTRIAN + + min_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, 12.100, 0.000, 36.000, 0.000, 0.000, 0.000, #CAR + 0.000, 0.000, 0.000, 0.000, 36.000, 0.000, 0.000, 0.000, #TRUCK + 0.000, 0.000, 0.000, 0.000, 36.000, 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 + 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000] #PEDESTRIAN + + + 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, 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 + 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000] #PEDESTRIAN diff --git a/perception/lidar_centerpoint/include/lidar_centerpoint/detection_class_remapper.hpp b/perception/lidar_centerpoint/include/lidar_centerpoint/detection_class_remapper.hpp new file mode 100644 index 0000000000000..18a78f122e170 --- /dev/null +++ b/perception/lidar_centerpoint/include/lidar_centerpoint/detection_class_remapper.hpp @@ -0,0 +1,47 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef LIDAR_CENTERPOINT__DETECTION_CLASS_REMAPPER_HPP_ +#define LIDAR_CENTERPOINT__DETECTION_CLASS_REMAPPER_HPP_ + +#include + +#include +#include +#include +#include + +#include + +namespace centerpoint +{ + +class DetectionClassRemapper +{ +public: + void setParameters( + const std::vector & allow_remapping_by_area_matrix, + const std::vector & min_area_matrix, const std::vector & max_area_matrix); + void mapClasses(autoware_auto_perception_msgs::msg::DetectedObjects & msg); + +protected: + Eigen::Matrix allow_remapping_by_area_matrix_; + Eigen::MatrixXd min_area_matrix_; + Eigen::MatrixXd max_area_matrix_; + int num_labels_; +}; + +} // namespace centerpoint + +#endif // LIDAR_CENTERPOINT__DETECTION_CLASS_REMAPPER_HPP_ diff --git a/perception/lidar_centerpoint/include/lidar_centerpoint/node.hpp b/perception/lidar_centerpoint/include/lidar_centerpoint/node.hpp index 66ffdfd400cfd..7477442a1060d 100644 --- a/perception/lidar_centerpoint/include/lidar_centerpoint/node.hpp +++ b/perception/lidar_centerpoint/include/lidar_centerpoint/node.hpp @@ -16,6 +16,7 @@ #define LIDAR_CENTERPOINT__NODE_HPP_ #include +#include #include #include #include @@ -49,9 +50,10 @@ class LidarCenterPointNode : public rclcpp::Node float score_threshold_{0.0}; std::vector class_names_; - bool rename_car_to_truck_and_bus_{false}; bool has_twist_{false}; + DetectionClassRemapper detection_class_remapper_; + std::unique_ptr detector_ptr_{nullptr}; // debugger diff --git a/perception/lidar_centerpoint/include/lidar_centerpoint/ros_utils.hpp b/perception/lidar_centerpoint/include/lidar_centerpoint/ros_utils.hpp index 8204cac708bad..69b75b614498e 100644 --- a/perception/lidar_centerpoint/include/lidar_centerpoint/ros_utils.hpp +++ b/perception/lidar_centerpoint/include/lidar_centerpoint/ros_utils.hpp @@ -31,8 +31,7 @@ namespace centerpoint { void box3DToDetectedObject( - const Box3D & box3d, const std::vector & class_names, - const bool rename_car_to_truck_and_bus, const bool has_twist, + const Box3D & box3d, const std::vector & class_names, const bool has_twist, autoware_auto_perception_msgs::msg::DetectedObject & obj); uint8_t getSemanticType(const std::string & class_name); diff --git a/perception/lidar_centerpoint/launch/lidar_centerpoint.launch.xml b/perception/lidar_centerpoint/launch/lidar_centerpoint.launch.xml index 5ffd93aa05318..4593b6e4f5234 100644 --- a/perception/lidar_centerpoint/launch/lidar_centerpoint.launch.xml +++ b/perception/lidar_centerpoint/launch/lidar_centerpoint.launch.xml @@ -5,6 +5,7 @@ + @@ -23,5 +24,6 @@ + diff --git a/perception/lidar_centerpoint/lib/detection_class_remapper.cpp b/perception/lidar_centerpoint/lib/detection_class_remapper.cpp new file mode 100644 index 0000000000000..147fb360d7c97 --- /dev/null +++ b/perception/lidar_centerpoint/lib/detection_class_remapper.cpp @@ -0,0 +1,70 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +namespace centerpoint +{ + +void DetectionClassRemapper::setParameters( + const std::vector & allow_remapping_by_area_matrix, + const std::vector & min_area_matrix, const std::vector & max_area_matrix) +{ + assert(allow_remapping_by_area_matrix.size() == min_area_matrix.size()); + assert(allow_remapping_by_area_matrix.size() == max_area_matrix.size()); + assert(std::pow(std::sqrt(min_area_matrix.size()), 2) == std::sqrt(min_area_matrix.size())); + + num_labels_ = static_cast(std::sqrt(min_area_matrix.size())); + + Eigen::Map> + allow_remapping_by_area_matrix_tmp( + allow_remapping_by_area_matrix.data(), num_labels_, num_labels_); + allow_remapping_by_area_matrix_ = allow_remapping_by_area_matrix_tmp.transpose() + .cast(); // Eigen is column major by default + + Eigen::Map min_area_matrix_tmp( + min_area_matrix.data(), num_labels_, num_labels_); + min_area_matrix_ = min_area_matrix_tmp.transpose(); // Eigen is column major by default + + Eigen::Map max_area_matrix_tmp( + max_area_matrix.data(), num_labels_, num_labels_); + max_area_matrix_ = max_area_matrix_tmp.transpose(); // Eigen is column major by default + + min_area_matrix_ = min_area_matrix_.unaryExpr( + [](double v) { return std::isfinite(v) ? v : std::numeric_limits::max(); }); + max_area_matrix_ = max_area_matrix_.unaryExpr( + [](double v) { return std::isfinite(v) ? v : std::numeric_limits::max(); }); +} + +void DetectionClassRemapper::mapClasses(autoware_auto_perception_msgs::msg::DetectedObjects & msg) +{ + for (auto & object : msg.objects) { + const float bev_area = object.shape.dimensions.x * object.shape.dimensions.y; + + for (auto & classification : object.classification) { + auto & label = classification.label; + + for (int i = 0; i < num_labels_; ++i) { + if ( + allow_remapping_by_area_matrix_(label, i) && bev_area >= min_area_matrix_(label, i) && + bev_area <= max_area_matrix_(label, i)) { + label = i; + break; + } + } + } + } +} + +} // namespace centerpoint diff --git a/perception/lidar_centerpoint/lib/ros_utils.cpp b/perception/lidar_centerpoint/lib/ros_utils.cpp index 8bc5f955efab4..16998b1eae96d 100644 --- a/perception/lidar_centerpoint/lib/ros_utils.cpp +++ b/perception/lidar_centerpoint/lib/ros_utils.cpp @@ -23,8 +23,7 @@ namespace centerpoint using Label = autoware_auto_perception_msgs::msg::ObjectClassification; void box3DToDetectedObject( - const Box3D & box3d, const std::vector & class_names, - const bool rename_car_to_truck_and_bus, const bool has_twist, + const Box3D & box3d, const std::vector & class_names, const bool has_twist, autoware_auto_perception_msgs::msg::DetectedObject & obj) { // TODO(yukke42): the value of classification confidence of DNN, not probability. @@ -41,17 +40,6 @@ void box3DToDetectedObject( rclcpp::get_logger("lidar_centerpoint"), "Unexpected label: UNKNOWN is set."); } - float l = box3d.length; - float w = box3d.width; - if (classification.label == Label::CAR && rename_car_to_truck_and_bus) { - // Note: object size is referred from multi_object_tracker - if ((w * l > 2.2 * 5.5) && (w * l <= 2.5 * 7.9)) { - classification.label = Label::TRUCK; - } else if (w * l > 2.5 * 7.9) { - classification.label = Label::BUS; - } - } - if (isCarLikeVehicleLabel(classification.label)) { obj.kinematics.orientation_availability = autoware_auto_perception_msgs::msg::DetectedObjectKinematics::SIGN_UNKNOWN; diff --git a/perception/lidar_centerpoint/src/node.cpp b/perception/lidar_centerpoint/src/node.cpp index c8ae4e140533b..9901f9a325953 100644 --- a/perception/lidar_centerpoint/src/node.cpp +++ b/perception/lidar_centerpoint/src/node.cpp @@ -52,7 +52,6 @@ LidarCenterPointNode::LidarCenterPointNode(const rclcpp::NodeOptions & node_opti const std::string head_onnx_path = this->declare_parameter("head_onnx_path"); const std::string head_engine_path = this->declare_parameter("head_engine_path"); class_names_ = this->declare_parameter>("class_names"); - rename_car_to_truck_and_bus_ = this->declare_parameter("rename_car_to_truck_and_bus", false); has_twist_ = this->declare_parameter("has_twist", false); const std::size_t point_feature_size = static_cast(this->declare_parameter("point_feature_size")); @@ -64,6 +63,13 @@ LidarCenterPointNode::LidarCenterPointNode(const rclcpp::NodeOptions & node_opti static_cast(this->declare_parameter("downsample_factor")); const std::size_t encoder_in_feature_size = static_cast(this->declare_parameter("encoder_in_feature_size")); + const auto allow_remapping_by_area_matrix = + this->declare_parameter>("allow_remapping_by_area_matrix"); + const auto min_area_matrix = this->declare_parameter>("min_area_matrix"); + const auto max_area_matrix = this->declare_parameter>("max_area_matrix"); + + detection_class_remapper_.setParameters( + allow_remapping_by_area_matrix, min_area_matrix, max_area_matrix); NetworkParam encoder_param(encoder_onnx_path, encoder_engine_path, trt_precision); NetworkParam head_param(head_onnx_path, head_engine_path, trt_precision); @@ -127,10 +133,12 @@ void LidarCenterPointNode::pointCloudCallback( output_msg.header = input_pointcloud_msg->header; for (const auto & box3d : det_boxes3d) { autoware_auto_perception_msgs::msg::DetectedObject obj; - box3DToDetectedObject(box3d, class_names_, rename_car_to_truck_and_bus_, has_twist_, obj); + box3DToDetectedObject(box3d, class_names_, has_twist_, obj); output_msg.objects.emplace_back(obj); } + detection_class_remapper_.mapClasses(output_msg); + if (objects_sub_count > 0) { objects_pub_->publish(output_msg); } diff --git a/perception/multi_object_tracker/config/data_association_matrix.param.yaml b/perception/multi_object_tracker/config/data_association_matrix.param.yaml index 7439dcfdea42b..1790315ae557d 100644 --- a/perception/multi_object_tracker/config/data_association_matrix.param.yaml +++ b/perception/multi_object_tracker/config/data_association_matrix.param.yaml @@ -25,11 +25,11 @@ # NOTE(yukke42): The size of truck is 12 m length x 3 m width. # NOTE(yukke42): The size of trailer is 20 m length x 3 m width. #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN - [10000.00, 10000.00, 10000.00, 10000.00, 10000.00, 10000.00, 10000.00, 10000.00, #UNKNOWN - 12.10, 12.10, 36.00, 40.00, 60.00, 10000.00, 10000.00, 10000.00, #CAR - 36.00, 12.10, 36.00, 40.00, 60.00, 10000.00, 10000.00, 10000.00, #TRUCK - 40.00, 12.10, 36.00, 40.00, 60.00, 10000.00, 10000.00, 10000.00, #BUS - 60.00, 12.10, 36.00, 40.00, 60.00, 10000.00, 10000.00, 10000.00, #TRAILER + [100.00, 100.00, 100.00, 100.00, 100.00, 100.00, 100.00, 100.00, #UNKNOWN + 12.10, 12.10, 36.00, 60.00, 60.00, 10000.00, 10000.00, 10000.00, #CAR + 36.00, 12.10, 36.00, 60.00, 60.00, 10000.00, 10000.00, 10000.00, #TRUCK + 60.00, 12.10, 36.00, 60.00, 60.00, 10000.00, 10000.00, 10000.00, #BUS + 60.00, 12.10, 36.00, 60.00, 60.00, 10000.00, 10000.00, 10000.00, #TRAILER 2.50, 10000.00, 10000.00, 10000.00, 10000.00, 2.50, 2.50, 1.00, #MOTORBIKE 2.50, 10000.00, 10000.00, 10000.00, 10000.00, 2.50, 2.50, 1.00, #BICYCLE 2.00, 10000.00, 10000.00, 10000.00, 10000.00, 1.50, 1.50, 1.00] #PEDESTRIAN diff --git a/perception/multi_object_tracker/config/default_tracker.param.yaml b/perception/multi_object_tracker/config/default_tracker.param.yaml index 495df3a30d96e..23e88d8274172 100644 --- a/perception/multi_object_tracker/config/default_tracker.param.yaml +++ b/perception/multi_object_tracker/config/default_tracker.param.yaml @@ -3,6 +3,7 @@ car_tracker: "multi_vehicle_tracker" truck_tracker: "multi_vehicle_tracker" bus_tracker: "multi_vehicle_tracker" + trailer_tracker: "multi_vehicle_tracker" pedestrian_tracker: "pedestrian_and_bicycle_tracker" bicycle_tracker: "pedestrian_and_bicycle_tracker" motorcycle_tracker: "pedestrian_and_bicycle_tracker" diff --git a/perception/multi_object_tracker/include/multi_object_tracker/utils/utils.hpp b/perception/multi_object_tracker/include/multi_object_tracker/utils/utils.hpp index 59ea0c747f4d1..a61ac10717f3e 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/utils/utils.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/utils/utils.hpp @@ -71,6 +71,13 @@ enum MSG_COV_IDX { YAW_PITCH = 34, YAW_YAW = 35 }; + +inline bool isLargeVehicleLabel(const uint8_t label) +{ + using Label = autoware_auto_perception_msgs::msg::ObjectClassification; + return label == Label::BUS || label == Label::TRUCK || label == Label::TRAILER; +} + } // namespace utils #endif // MULTI_OBJECT_TRACKER__UTILS__UTILS_HPP_ diff --git a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp index 3873d9a7b7d6b..794b523d817d9 100644 --- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp +++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp @@ -106,7 +106,7 @@ bool isSpecificAlivePattern( tracker->getTotalMeasurementCount() / (tracker->getTotalNoMeasurementCount() + tracker->getTotalMeasurementCount()); - const bool big_vehicle = (label == Label::TRUCK || label == Label::BUS); + const bool big_vehicle = utils::isLargeVehicleLabel(label); const bool slow_velocity = getVelocity(object) < max_velocity; @@ -172,6 +172,8 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options) std::make_pair(Label::TRUCK, this->declare_parameter("truck_tracker"))); tracker_map_.insert( std::make_pair(Label::BUS, this->declare_parameter("bus_tracker"))); + tracker_map_.insert( + std::make_pair(Label::TRAILER, this->declare_parameter("trailer_tracker"))); tracker_map_.insert( std::make_pair(Label::PEDESTRIAN, this->declare_parameter("pedestrian_tracker"))); tracker_map_.insert( 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 c4d003b12ae5c..9473d291d333c 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 @@ -228,7 +228,7 @@ bool BigVehicleTracker::measureWithPose( 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 (label == Label::TRUCK || label == Label::BUS) { + } else if (utils::isLargeVehicleLabel(label)) { r_cov_x = ekf_params_.r_cov_x; r_cov_y = ekf_params_.r_cov_y; } else { diff --git a/perception/multi_object_tracker/src/tracker/model/multiple_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/multiple_vehicle_tracker.cpp index 9d66e713b5fe1..e84006495b420 100644 --- a/perception/multi_object_tracker/src/tracker/model/multiple_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/multiple_vehicle_tracker.cpp @@ -55,7 +55,7 @@ bool MultipleVehicleTracker::getTrackedObject( if (label == Label::CAR) { normal_vehicle_tracker_.getTrackedObject(time, object); - } else if (label == Label::BUS || label == Label::TRUCK) { + } else if (utils::isLargeVehicleLabel(label)) { big_vehicle_tracker_.getTrackedObject(time, object); } object.object_id = getUUID(); 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 5449355800f80..77650de5e2313 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 @@ -226,7 +226,7 @@ bool NormalVehicleTracker::measureWithPose( if (label == Label::CAR) { r_cov_x = ekf_params_.r_cov_x; r_cov_y = ekf_params_.r_cov_y; - } else if (label == Label::TRUCK || label == Label::BUS) { + } else if (utils::isLargeVehicleLabel(label)) { 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); diff --git a/perception/radar_tracks_msgs_converter/src/radar_tracks_msgs_converter_node/radar_tracks_msgs_converter_node.cpp b/perception/radar_tracks_msgs_converter/src/radar_tracks_msgs_converter_node/radar_tracks_msgs_converter_node.cpp index 23c4d1200ccd4..42465ad4d9851 100644 --- a/perception/radar_tracks_msgs_converter/src/radar_tracks_msgs_converter_node/radar_tracks_msgs_converter_node.cpp +++ b/perception/radar_tracks_msgs_converter/src/radar_tracks_msgs_converter_node/radar_tracks_msgs_converter_node.cpp @@ -162,6 +162,8 @@ TrackedObjects RadarTracksMsgsConverterNode::convertRadarTrackToTrackedObjects() TrackedObjects tracked_objects; tracked_objects.header = radar_data_->header; tracked_objects.header.frame_id = node_param_.new_frame_id; + using P_IDX = tier4_autoware_utils::pose_covariance_index::POSE_COV_IDX; + using R_IDX = tier4_autoware_utils::xyz_upper_covariance_index::XYZ_UPPER_COV_IDX; for (auto & radar_track : radar_data_->tracks) { TrackedObject tracked_object; @@ -185,9 +187,6 @@ TrackedObjects RadarTracksMsgsConverterNode::convertRadarTrackToTrackedObjects() kinematics.pose_with_covariance.pose = transformed_pose_stamped.pose; { - using P_IDX = tier4_autoware_utils::pose_covariance_index::POSE_COV_IDX; - using R_IDX = tier4_autoware_utils::xyz_upper_covariance_index::XYZ_UPPER_COV_IDX; - auto & covariance = kinematics.pose_with_covariance.covariance; covariance[P_IDX::X_X] = radar_track.position_covariance[R_IDX::X_X]; covariance[P_IDX::X_Y] = radar_track.position_covariance[R_IDX::X_Y]; @@ -218,25 +217,30 @@ TrackedObjects RadarTracksMsgsConverterNode::convertRadarTrackToTrackedObjects() } } - kinematics.twist_with_covariance.covariance[0] = radar_track.velocity_covariance[0]; - kinematics.twist_with_covariance.covariance[1] = radar_track.velocity_covariance[1]; - kinematics.twist_with_covariance.covariance[2] = radar_track.velocity_covariance[2]; - kinematics.twist_with_covariance.covariance[6] = radar_track.velocity_covariance[1]; - kinematics.twist_with_covariance.covariance[7] = radar_track.velocity_covariance[3]; - kinematics.twist_with_covariance.covariance[8] = radar_track.velocity_covariance[4]; - kinematics.twist_with_covariance.covariance[12] = radar_track.velocity_covariance[2]; - kinematics.twist_with_covariance.covariance[13] = radar_track.velocity_covariance[4]; - kinematics.twist_with_covariance.covariance[14] = radar_track.velocity_covariance[5]; - - kinematics.acceleration_with_covariance.covariance[0] = radar_track.acceleration_covariance[0]; - kinematics.acceleration_with_covariance.covariance[1] = radar_track.acceleration_covariance[1]; - kinematics.acceleration_with_covariance.covariance[2] = radar_track.acceleration_covariance[2]; - kinematics.acceleration_with_covariance.covariance[6] = radar_track.acceleration_covariance[1]; - kinematics.acceleration_with_covariance.covariance[7] = radar_track.acceleration_covariance[3]; - kinematics.acceleration_with_covariance.covariance[8] = radar_track.acceleration_covariance[4]; - kinematics.acceleration_with_covariance.covariance[12] = radar_track.acceleration_covariance[2]; - kinematics.acceleration_with_covariance.covariance[13] = radar_track.acceleration_covariance[4]; - kinematics.acceleration_with_covariance.covariance[14] = radar_track.acceleration_covariance[5]; + { + auto & covariance = kinematics.twist_with_covariance.covariance; + covariance[P_IDX::X_X] = radar_track.velocity_covariance[R_IDX::X_X]; + covariance[P_IDX::X_Y] = radar_track.velocity_covariance[R_IDX::X_Y]; + covariance[P_IDX::X_Z] = radar_track.velocity_covariance[R_IDX::X_Z]; + covariance[P_IDX::Y_X] = radar_track.velocity_covariance[R_IDX::X_Y]; + covariance[P_IDX::Y_Y] = radar_track.velocity_covariance[R_IDX::Y_Y]; + covariance[P_IDX::Y_Z] = radar_track.velocity_covariance[R_IDX::Y_Z]; + covariance[P_IDX::Z_X] = radar_track.velocity_covariance[R_IDX::X_Z]; + covariance[P_IDX::Z_Y] = radar_track.velocity_covariance[R_IDX::Y_Z]; + covariance[P_IDX::Z_Z] = radar_track.velocity_covariance[R_IDX::Z_Z]; + } + { + auto & covariance = kinematics.acceleration_with_covariance.covariance; + covariance[P_IDX::X_X] = radar_track.acceleration_covariance[R_IDX::X_X]; + covariance[P_IDX::X_Y] = radar_track.acceleration_covariance[R_IDX::X_Y]; + covariance[P_IDX::X_Z] = radar_track.acceleration_covariance[R_IDX::X_Z]; + covariance[P_IDX::Y_X] = radar_track.acceleration_covariance[R_IDX::X_Y]; + covariance[P_IDX::Y_Y] = radar_track.acceleration_covariance[R_IDX::Y_Y]; + covariance[P_IDX::Y_Z] = radar_track.acceleration_covariance[R_IDX::Y_Z]; + covariance[P_IDX::Z_X] = radar_track.acceleration_covariance[R_IDX::X_Z]; + covariance[P_IDX::Z_Y] = radar_track.acceleration_covariance[R_IDX::Y_Z]; + covariance[P_IDX::Z_Z] = radar_track.acceleration_covariance[R_IDX::Z_Z]; + } tracked_object.kinematics = kinematics; diff --git a/perception/tensorrt_yolo/README.md b/perception/tensorrt_yolo/README.md index ff147e9256371..efdd26941308e 100644 --- a/perception/tensorrt_yolo/README.md +++ b/perception/tensorrt_yolo/README.md @@ -68,7 +68,9 @@ This package includes multiple licenses. ## Onnx model -All YOLO ONNX models are converted from the officially trained model. If you need information about training datasets and conditions, please refer to the official repositories . +All YOLO ONNX models are converted from the officially trained model. If you need information about training datasets and conditions, please refer to the official repositories. + +All models are downloaded automatically when building. When launching the node with a model for the first time, the model is automatically converted to TensorRT, although this may take some time. ### YOLOv3 diff --git a/planning/behavior_path_planner/CMakeLists.txt b/planning/behavior_path_planner/CMakeLists.txt index 5036e6ae1649b..fa9e610d02026 100644 --- a/planning/behavior_path_planner/CMakeLists.txt +++ b/planning/behavior_path_planner/CMakeLists.txt @@ -12,6 +12,7 @@ ament_auto_add_library(behavior_path_planner_node SHARED src/utilities.cpp src/path_utilities.cpp src/debug_utilities.cpp + src/steering_factor_interface.cpp src/turn_signal_decider.cpp src/scene_module/scene_module_bt_node_interface.cpp src/scene_module/side_shift/side_shift_module.cpp diff --git a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp index 58df76bc78baa..8aabe413c0c5b 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp @@ -23,6 +23,7 @@ #include "behavior_path_planner/scene_module/pull_out/pull_out_module.hpp" #include "behavior_path_planner/scene_module/pull_over/pull_over_module.hpp" #include "behavior_path_planner/scene_module/side_shift/side_shift_module.hpp" +#include "behavior_path_planner/steering_factor_interface.hpp" #include "behavior_path_planner/turn_signal_decider.hpp" #include @@ -72,6 +73,7 @@ using geometry_msgs::msg::TwistStamped; using nav_msgs::msg::OccupancyGrid; using nav_msgs::msg::Odometry; using rcl_interfaces::msg::SetParametersResult; +using steering_factor_interface::SteeringFactorInterface; using tier4_planning_msgs::msg::AvoidanceDebugMsgArray; using tier4_planning_msgs::msg::PathChangeModule; using tier4_planning_msgs::msg::Scenario; @@ -98,6 +100,7 @@ class BehaviorPathPlannerNode : public rclcpp::Node std::shared_ptr planner_data_; std::shared_ptr bt_manager_; + std::unique_ptr steering_factor_interface_ptr_; tier4_autoware_utils::SelfPoseListener self_pose_listener_{this}; Scenario::SharedPtr current_scenario_{nullptr}; @@ -171,6 +174,11 @@ class BehaviorPathPlannerNode : public rclcpp::Node */ bool isForcedCandidatePath() const; + /** + * @brief publish the steering factor from intersection + */ + void publish_steering_factor(const TurnIndicatorsCommand & turn_signal); + template size_t findEgoIndex(const std::vector & points) const { diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp index 1069a4f285223..e3b35ed89405b 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp @@ -124,6 +124,11 @@ class AvoidanceModule : public SceneModuleInterface path.points, ego_position, left_shift.finish_pose.position); rtc_interface_left_.updateCooperateStatus( left_shift.uuid, true, start_distance, finish_distance, clock_->now()); + if (finish_distance > -1.0e-03) { + steering_factor_interface_ptr_->updateSteeringFactor( + {left_shift.start_pose, left_shift.finish_pose}, {start_distance, finish_distance}, + SteeringFactor::AVOIDANCE_PATH_CHANGE, SteeringFactor::LEFT, SteeringFactor::TURNING, ""); + } } for (const auto & right_shift : right_shift_array_) { @@ -133,6 +138,12 @@ class AvoidanceModule : public SceneModuleInterface path.points, ego_position, right_shift.finish_pose.position); rtc_interface_right_.updateCooperateStatus( right_shift.uuid, true, start_distance, finish_distance, clock_->now()); + if (finish_distance > -1.0e-03) { + steering_factor_interface_ptr_->updateSteeringFactor( + {right_shift.start_pose, right_shift.finish_pose}, {start_distance, finish_distance}, + SteeringFactor::AVOIDANCE_PATH_CHANGE, SteeringFactor::RIGHT, SteeringFactor::TURNING, + ""); + } } } diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module.hpp index 5770c16beb42c..a331269871215 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module.hpp @@ -16,6 +16,7 @@ #define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__LANE_CHANGE_MODULE_HPP_ #include "behavior_path_planner/scene_module/lane_change/debug.hpp" +#include "behavior_path_planner/scene_module/lane_change/lane_change_module_data.hpp" #include "behavior_path_planner/scene_module/lane_change/lane_change_path.hpp" #include "behavior_path_planner/scene_module/scene_module_interface.hpp" @@ -36,38 +37,6 @@ namespace behavior_path_planner using autoware_auto_planning_msgs::msg::PathWithLaneId; using marker_utils::CollisionCheckDebug; -struct LaneChangeParameters -{ - double lane_change_prepare_duration; - double lane_changing_duration; - double minimum_lane_change_prepare_distance; - double lane_change_finish_judge_buffer; - double minimum_lane_change_velocity; - double prediction_time_resolution; - double maximum_deceleration; - int lane_change_sampling_num; - double abort_lane_change_velocity_thresh; - double abort_lane_change_angle_thresh; - double abort_lane_change_distance_thresh; - bool enable_abort_lane_change; - bool enable_collision_check_at_prepare_phase; - bool use_predicted_path_outside_lanelet; - bool use_all_predicted_path; - bool publish_debug_marker; -}; - -struct LaneChangeStatus -{ - PathWithLaneId lane_follow_path; - LaneChangePath lane_change_path; - lanelet::ConstLanelets current_lanes; - lanelet::ConstLanelets lane_change_lanes; - std::vector lane_follow_lane_ids; - std::vector lane_change_lane_ids; - bool is_safe; - double start_distance; -}; - class LaneChangeModule : public SceneModuleInterface { public: @@ -159,7 +128,6 @@ class LaneChangeModule : public SceneModuleInterface } PathWithLaneId getReferencePath() const; - lanelet::ConstLanelets getCurrentLanes() const; lanelet::ConstLanelets getLaneChangeLanes( const lanelet::ConstLanelets & current_lanes, const double lane_change_lane_length) const; std::pair getSafePath( diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module_data.hpp new file mode 100644 index 0000000000000..c67f3c17d152b --- /dev/null +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module_data.hpp @@ -0,0 +1,59 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__LANE_CHANGE_MODULE_DATA_HPP_ +#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__LANE_CHANGE_MODULE_DATA_HPP_ + +#include "behavior_path_planner/scene_module/lane_change/lane_change_path.hpp" +#include "lanelet2_core/geometry/Lanelet.h" + +#include "autoware_auto_planning_msgs/msg/path_point_with_lane_id.hpp" + +#include + +namespace behavior_path_planner +{ +struct LaneChangeParameters +{ + double lane_change_prepare_duration; + double lane_changing_duration; + double minimum_lane_change_prepare_distance; + double lane_change_finish_judge_buffer; + double minimum_lane_change_velocity; + double prediction_time_resolution; + double maximum_deceleration; + int lane_change_sampling_num; + double abort_lane_change_velocity_thresh; + double abort_lane_change_angle_thresh; + double abort_lane_change_distance_thresh; + bool enable_abort_lane_change; + bool enable_collision_check_at_prepare_phase; + bool use_predicted_path_outside_lanelet; + bool use_all_predicted_path; + bool publish_debug_marker; +}; + +struct LaneChangeStatus +{ + PathWithLaneId lane_follow_path; + LaneChangePath lane_change_path; + lanelet::ConstLanelets current_lanes; + lanelet::ConstLanelets lane_change_lanes; + std::vector lane_follow_lane_ids; + std::vector lane_change_lane_ids; + bool is_safe; + double start_distance; +}; +} // namespace behavior_path_planner + +#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__LANE_CHANGE_MODULE_DATA_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_path.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_path.hpp index fad9860e830f9..9d24060e60eb7 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_path.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_path.hpp @@ -19,6 +19,8 @@ #include +#include + namespace behavior_path_planner { using autoware_auto_planning_msgs::msg::PathWithLaneId; @@ -31,5 +33,7 @@ struct LaneChangePath double preparation_length{0.0}; double lane_change_length{0.0}; }; +using LaneChangePaths = std::vector; + } // namespace behavior_path_planner #endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__LANE_CHANGE_PATH_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/util.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/util.hpp index dcd9d97c3efc2..7d1664a48eec5 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/util.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/util.hpp @@ -43,47 +43,89 @@ using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; using tier4_autoware_utils::Polygon2d; -PathWithLaneId combineReferencePath(const PathWithLaneId path1, const PathWithLaneId path2); +PathWithLaneId combineReferencePath(const PathWithLaneId & path1, const PathWithLaneId & path2); + bool isPathInLanelets( const PathWithLaneId & path, const lanelet::ConstLanelets & original_lanelets, const lanelet::ConstLanelets & target_lanelets); + double getExpectedVelocityWhenDecelerate( const double & current_velocity, const double & expected_acceleration, const double & lane_change_prepare_duration); + double getDistanceWhenDecelerate( const double & velocity, const double & expected_acceleration, const double & duration, const double & minimum_distance); -std::vector getLaneChangePaths( + +std::optional constructCandidatePath( + const PathWithLaneId & prepare_segment, const PathWithLaneId & lane_changing_segment, + const PathWithLaneId & target_lane_reference_path, const ShiftPoint & shift_point, + const lanelet::ConstLanelets & original_lanelets, const lanelet::ConstLanelets & target_lanelets, + const double & acceleration, const double & prepare_distance, const double & lane_change_distance, + const double & lane_changing_duration, const double & minimum_lane_change_velocity); + +LaneChangePaths getLaneChangePaths( const RouteHandler & route_handler, const lanelet::ConstLanelets & original_lanelets, const lanelet::ConstLanelets & target_lanelets, const Pose & pose, const Twist & twist, - const BehaviorPathPlannerParameters & common_parameter, const LaneChangeParameters & parameter); -std::vector selectValidPaths( - const std::vector & paths, const lanelet::ConstLanelets & current_lanes, + const BehaviorPathPlannerParameters & common_parameter, + const behavior_path_planner::LaneChangeParameters & parameter); + +LaneChangePaths selectValidPaths( + const LaneChangePaths & paths, const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, const lanelet::routing::RoutingGraphContainer & overall_graphs, const Pose & current_pose, const bool isInGoalRouteSection, const Pose & goal_pose); + bool selectSafePath( - const std::vector & paths, const lanelet::ConstLanelets & current_lanes, + const LaneChangePaths & paths, const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, const PredictedObjects::ConstSharedPtr dynamic_objects, const Pose & current_pose, const Twist & current_twist, const BehaviorPathPlannerParameters & common_parameters, const behavior_path_planner::LaneChangeParameters & ros_parameters, LaneChangePath * selected_path, std::unordered_map & debug_data); + bool isLaneChangePathSafe( const PathWithLaneId & path, const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, const PredictedObjects::ConstSharedPtr dynamic_objects, const Pose & current_pose, - const size_t current_seg_idx, const Twist & current_twist, + const size_t & current_seg_idx, const Twist & current_twist, const BehaviorPathPlannerParameters & common_parameters, const behavior_path_planner::LaneChangeParameters & lane_change_parameters, std::unordered_map & debug_data, const bool use_buffer = true, const double acceleration = 0.0); + bool hasEnoughDistance( const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, const Pose & current_pose, const bool isInGoalRouteSection, const Pose & goal_pose, const lanelet::routing::RoutingGraphContainer & overall_graphs); + +ShiftPoint getLaneChangeShiftPoint( + const PathWithLaneId & path1, const PathWithLaneId & path2, + const lanelet::ConstLanelets & target_lanes, const PathWithLaneId & reference_path); + +PathWithLaneId getReferencePathFromTargetLane( + const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes, + const Pose & lane_changing_start_pose, const double & prepare_distance, + const double & lane_changing_distance, const double & forward_path_length); + +PathWithLaneId getReferencePathFromTargetLane( + const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes, + const Pose & in_target_front_pose, const Pose & in_target_end_pose); + +PathWithLaneId getLaneChangePathPrepareSegment( + const RouteHandler & route_handler, const lanelet::ConstLanelets & original_lanelets, + const Pose & current_pose, const double & backward_path_length, const double & prepare_distance, + const double & prepare_duration, const double & minimum_lane_change_velocity); + +PathWithLaneId getLaneChangePathLaneChangingSegment( + const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanelets, + const Pose & current_pose, const double & forward_path_length, const double & prepare_distance, + const double & lane_change_distance, const double & minimum_lane_change_length, + const double & lane_change_distance_buffer, const double & lane_changing_duration, + const double & minimum_lane_change_velocity); + } // namespace behavior_path_planner::lane_change_utils #endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__UTIL_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp index b1e9665eaa46f..d7f47f5391d15 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp @@ -18,10 +18,12 @@ #include "behavior_path_planner/data_manager.hpp" #include "behavior_path_planner/utilities.hpp" +#include #include #include #include +#include #include #include #include @@ -40,10 +42,12 @@ namespace behavior_path_planner { +using autoware_adapi_v1_msgs::msg::SteeringFactor; using autoware_auto_planning_msgs::msg::PathWithLaneId; using autoware_auto_vehicle_msgs::msg::HazardLightsCommand; using autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand; using rtc_interface::RTCInterface; +using steering_factor_interface::SteeringFactorInterface; using tier4_planning_msgs::msg::AvoidanceDebugMsgArray; using unique_identifier_msgs::msg::UUID; using visualization_msgs::msg::MarkerArray; @@ -216,6 +220,14 @@ class SceneModuleInterface return false; } + virtual void publishSteeringFactor() + { + if (!steering_factor_interface_ptr_) { + return; + } + steering_factor_interface_ptr_->publishSteeringFactor(clock_->now()); + } + /** * @brief set planner data */ @@ -249,6 +261,7 @@ class SceneModuleInterface mutable MarkerArray debug_marker_; std::shared_ptr rtc_interface_ptr_; + std::unique_ptr steering_factor_interface_ptr_; UUID uuid_; bool is_waiting_approval_; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/steering_factor_interface.hpp b/planning/behavior_path_planner/include/behavior_path_planner/steering_factor_interface.hpp new file mode 100644 index 0000000000000..8afe0576ec1aa --- /dev/null +++ b/planning/behavior_path_planner/include/behavior_path_planner/steering_factor_interface.hpp @@ -0,0 +1,56 @@ +// Copyright 2022 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef BEHAVIOR_PATH_PLANNER__STEERING_FACTOR_INTERFACE_HPP_ +#define BEHAVIOR_PATH_PLANNER__STEERING_FACTOR_INTERFACE_HPP_ + +#include "rclcpp/rclcpp.hpp" + +#include "autoware_adapi_v1_msgs/msg/steering_factor_array.hpp" +#include "geometry_msgs/msg/pose.hpp" +#include "unique_identifier_msgs/msg/uuid.hpp" + +#include +#include +#include + +namespace steering_factor_interface +{ +using autoware_adapi_v1_msgs::msg::SteeringFactor; +using autoware_adapi_v1_msgs::msg::SteeringFactorArray; +using geometry_msgs::msg::Pose; + +class SteeringFactorInterface +{ +public: + SteeringFactorInterface(rclcpp::Node * node, const std::string & name); + void publishSteeringFactor(const rclcpp::Time & stamp); + void updateSteeringFactor( + const std::array & pose, const std::array distance, const uint16_t type, + const uint16_t direction, const uint16_t status, const std::string detail); + void clearSteeringFactors(); + +private: + rclcpp::Logger getLogger() const; + + rclcpp::Publisher::SharedPtr pub_steering_factors_; + + std::mutex mutex_; + rclcpp::Logger logger_; + SteeringFactorArray registered_steering_factors_; +}; + +} // namespace steering_factor_interface + +#endif // BEHAVIOR_PATH_PLANNER__STEERING_FACTOR_INTERFACE_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/turn_signal_decider.hpp b/planning/behavior_path_planner/include/behavior_path_planner/turn_signal_decider.hpp index 3203b46b17454..ff6f3aed118c2 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/turn_signal_decider.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/turn_signal_decider.hpp @@ -22,6 +22,7 @@ #include +#include #include #include @@ -45,6 +46,9 @@ class TurnSignalDecider intersection_search_distance_ = intersection_search_distance; } + std::pair getIntersectionTurnSignalFlag(); + std::pair getIntersectionPoseAndDistance(); + private: std::pair getIntersectionTurnSignal( const PathWithLaneId & path, const Pose & current_pose, const size_t current_seg_idx, @@ -56,6 +60,10 @@ class TurnSignalDecider // data double intersection_search_distance_{0.0}; double base_link2front_{0.0}; + mutable bool intersection_turn_signal_ = false; + mutable bool approaching_intersection_turn_signal_ = false; + mutable double intersection_distance_ = std::numeric_limits::max(); + mutable Pose intersection_pose_point_ = Pose(); }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp index 0858ac05f9ddd..b399fa5197ba0 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp @@ -74,6 +74,7 @@ using tier4_autoware_utils::Polygon2d; namespace bg = boost::geometry; using geometry_msgs::msg::Pose; using marker_utils::CollisionCheckDebug; +using vehicle_info_util::VehicleInfo; struct FrenetCoordinate3d { @@ -381,8 +382,10 @@ lanelet::ConstLanelets getExtendedCurrentLanes( lanelet::ConstLanelets calcLaneAroundPose( const std::shared_ptr route_handler, const geometry_msgs::msg::Pose & pose, const double forward_length, const double backward_length); + Polygon2d convertBoundingBoxObjectToGeometryPolygon( - const Pose & current_pose, const double & length, const double & width); + const Pose & current_pose, const double & base_to_front, const double & base_to_rear, + const double & base_to_width); Polygon2d convertCylindricalObjectToGeometryPolygon( const Pose & current_pose, const Shape & obj_shape); @@ -399,7 +402,7 @@ Pose projectCurrentPoseToTarget(const Pose & desired_object, const Pose & target bool getEgoExpectedPoseAndConvertToPolygon( const Pose & current_pose, const PredictedPath & pred_path, Pose & expected_pose, tier4_autoware_utils::Polygon2d & ego_polygon, const double & check_current_time, - const double & length, const double & width); + const VehicleInfo & ego_info); bool getObjectExpectedPoseAndConvertToPolygon( const PredictedPath & pred_path, const PredictedObject & object, Pose & expected_pose, @@ -432,16 +435,16 @@ bool isLateralDistanceEnough( bool isSafeInLaneletCollisionCheck( const Pose & ego_current_pose, const Twist & ego_current_twist, - const PredictedPath & ego_predicted_path, const double & ego_vehicle_length, - const double & ego_vehicle_width, const double & check_start_time, const double & check_end_time, + const PredictedPath & ego_predicted_path, const VehicleInfo & ego_info, + const double & check_start_time, const double & check_end_time, const double & check_time_resolution, const PredictedObject & target_object, const PredictedPath & target_object_path, const BehaviorPathPlannerParameters & common_parameters, CollisionCheckDebug & debug); bool isSafeInFreeSpaceCollisionCheck( const Pose & ego_current_pose, const Twist & ego_current_twist, - const PredictedPath & ego_predicted_path, const double & ego_vehicle_length, - const double & ego_vehicle_width, const double & check_start_time, const double & check_end_time, + const PredictedPath & ego_predicted_path, const VehicleInfo & ego_info, + const double & check_start_time, const double & check_end_time, const double & check_time_resolution, const PredictedObject & target_object, const BehaviorPathPlannerParameters & common_parameters, CollisionCheckDebug & debug); } // namespace behavior_path_planner::util diff --git a/planning/behavior_path_planner/package.xml b/planning/behavior_path_planner/package.xml index 29c105f5e0871..54bfa019ee0aa 100644 --- a/planning/behavior_path_planner/package.xml +++ b/planning/behavior_path_planner/package.xml @@ -20,6 +20,11 @@ Takayuki Murooka + + Taiki Tanaka + Tomoya Kimura + Shumpei Wakabayashi + Apache License 2.0 Takamasa Horibe @@ -34,6 +39,7 @@ autoware_cmake + autoware_adapi_v1_msgs autoware_auto_perception_msgs autoware_auto_planning_msgs autoware_auto_vehicle_msgs diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index 5fe8ea5c422ca..34b6f61631a9e 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -149,6 +149,8 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod planner_data_->parameters.base_link2front, intersection_search_distance); } + steering_factor_interface_ptr_ = std::make_unique(this, "intersection"); + // Start timer { const auto planning_hz = declare_parameter("planning_hz", 10.0); @@ -614,6 +616,8 @@ void BehaviorPathPlannerNode::run() hazard_signal.stamp = get_clock()->now(); turn_signal_publisher_->publish(turn_signal); hazard_signal_publisher_->publish(hazard_signal); + + publish_steering_factor(turn_signal); } // for debug @@ -629,6 +633,36 @@ void BehaviorPathPlannerNode::run() RCLCPP_DEBUG(get_logger(), "----- behavior path planner end -----\n\n"); } +void BehaviorPathPlannerNode::publish_steering_factor(const TurnIndicatorsCommand & turn_signal) +{ + const auto [intersection_flag, approaching_intersection_flag] = + turn_signal_decider_.getIntersectionTurnSignalFlag(); + if (intersection_flag || approaching_intersection_flag) { + const uint16_t steering_factor_direction = std::invoke([&turn_signal]() { + if (turn_signal.command == TurnIndicatorsCommand::ENABLE_LEFT) { + return SteeringFactor::LEFT; + } + return SteeringFactor::RIGHT; + }); + + const auto [intersection_pose, intersection_distance] = + turn_signal_decider_.getIntersectionPoseAndDistance(); + const uint16_t steering_factor_state = std::invoke([&intersection_flag]() { + if (intersection_flag) { + return SteeringFactor::TURNING; + } + return SteeringFactor::TRYING; + }); + + steering_factor_interface_ptr_->updateSteeringFactor( + {intersection_pose, intersection_pose}, {intersection_distance, intersection_distance}, + SteeringFactor::INTERSECTION, steering_factor_direction, steering_factor_state, ""); + } else { + steering_factor_interface_ptr_->clearSteeringFactors(); + } + steering_factor_interface_ptr_->publishSteeringFactor(get_clock()->now()); +} + PathWithLaneId::SharedPtr BehaviorPathPlannerNode::getPath( const BehaviorModuleOutput & bt_output, const std::shared_ptr planner_data) { diff --git a/planning/behavior_path_planner/src/behavior_tree_manager.cpp b/planning/behavior_path_planner/src/behavior_tree_manager.cpp index 76bee98960016..232d3f8d42640 100644 --- a/planning/behavior_path_planner/src/behavior_tree_manager.cpp +++ b/planning/behavior_path_planner/src/behavior_tree_manager.cpp @@ -103,6 +103,7 @@ BehaviorModuleOutput BehaviorTreeManager::run(const std::shared_ptr m->onExit(); } m->publishRTCStatus(); + m->publishSteeringFactor(); }); return output; } diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index 3350e3fe40e90..92db1e8e42792 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp @@ -56,6 +56,7 @@ AvoidanceModule::AvoidanceModule( uuid_right_{generateUUID()} { using std::placeholders::_1; + steering_factor_interface_ptr_ = std::make_unique(&node, "avoidance"); } bool AvoidanceModule::isExecutionRequested() const @@ -2082,6 +2083,18 @@ CandidateOutput AvoidanceModule::planCandidate() const output.lateral_shift = new_shift_points->at(i).getRelativeLength(); output.start_distance_to_path_change = new_shift_points->front().start_longitudinal; output.finish_distance_to_path_change = new_shift_points->back().end_longitudinal; + + const uint16_t steering_factor_direction = std::invoke([&output]() { + if (output.lateral_shift > 0.0) { + return SteeringFactor::LEFT; + } + return SteeringFactor::RIGHT; + }); + steering_factor_interface_ptr_->updateSteeringFactor( + {new_shift_points->front().start, new_shift_points->back().end}, + {output.start_distance_to_path_change, output.finish_distance_to_path_change}, + SteeringFactor::AVOIDANCE_PATH_CHANGE, steering_factor_direction, SteeringFactor::APPROACHING, + ""); } const size_t ego_idx = findEgoIndex(shifted_path.path.points); @@ -2229,6 +2242,11 @@ boost::optional AvoidanceModule::findNewShiftPoint( throw std::logic_error("prev_reference_ and prev_linear_shift_path_ must have same size."); } + // new shift points must exist in front of Ego + if (candidate.start_longitudinal < 0.0) { + continue; + } + // TODO(Horibe): this code prohibits the changes on ego pose. Think later. // if (candidate.start_idx < avoidance_data_.ego_closest_path_index) { // DEBUG_PRINT("%s, start_idx is behind ego. skip.", pfx); @@ -2483,6 +2501,7 @@ void AvoidanceModule::onExit() current_state_ = BT::NodeStatus::IDLE; clearWaitingApproval(); removeRTCStatus(); + steering_factor_interface_ptr_->clearSteeringFactors(); } void AvoidanceModule::initVariables() diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/debug.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/debug.cpp index 908087baeeef9..28dcaccd090f7 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/debug.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/debug.cpp @@ -51,7 +51,8 @@ MarkerArray showObjectInfo( std::ostringstream ss; ss << info.failed_reason << "\nLon: " << std::setprecision(4) << info.relative_to_ego.position.x - << "\nLat: " << info.relative_to_ego.position.y; + << "\nLat: " << info.relative_to_ego.position.y + << "\nPosition: " << (info.is_front ? "front" : "back"); marker.text = ss.str(); diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp index 304aada523602..fc1b99b05fa4d 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp @@ -46,6 +46,7 @@ LaneChangeModule::LaneChangeModule( uuid_left_{generateUUID()}, uuid_right_{generateUUID()} { + steering_factor_interface_ptr_ = std::make_unique(&node, "lane_change"); } BehaviorModuleOutput LaneChangeModule::run() @@ -53,7 +54,7 @@ BehaviorModuleOutput LaneChangeModule::run() RCLCPP_DEBUG(getLogger(), "Was waiting approval, and now approved. Do plan()."); current_state_ = BT::NodeStatus::RUNNING; is_activated_ = isActivated(); - const auto output = plan(); + auto output = plan(); const auto turn_signal_info = output.turn_signal_info; const auto current_pose = planner_data_->self_pose->pose; const double start_distance = motion_utils::calcSignedArcLength( @@ -61,11 +62,24 @@ BehaviorModuleOutput LaneChangeModule::run() status_.lane_change_path.shift_point.start.position); const double finish_distance = motion_utils::calcSignedArcLength( output.path->points, current_pose.position, status_.lane_change_path.shift_point.end.position); - if (turn_signal_info.turn_signal.command == TurnIndicatorsCommand::ENABLE_LEFT) { - waitApprovalLeft(start_distance, finish_distance); - } else if (turn_signal_info.turn_signal.command == TurnIndicatorsCommand::ENABLE_RIGHT) { - waitApprovalRight(start_distance, finish_distance); - } + + const uint16_t steering_factor_direction = + std::invoke([this, &start_distance, &finish_distance, &turn_signal_info]() { + if (turn_signal_info.turn_signal.command == TurnIndicatorsCommand::ENABLE_LEFT) { + waitApprovalLeft(start_distance, finish_distance); + return SteeringFactor::LEFT; + } + if (turn_signal_info.turn_signal.command == TurnIndicatorsCommand::ENABLE_RIGHT) { + waitApprovalRight(start_distance, finish_distance); + return SteeringFactor::RIGHT; + } + return SteeringFactor::UNKNOWN; + }); + // TODO(tkhmy) add handle status TRYING + steering_factor_interface_ptr_->updateSteeringFactor( + {status_.lane_change_path.shift_point.start, status_.lane_change_path.shift_point.end}, + {start_distance, finish_distance}, SteeringFactor::LANE_CHANGE, steering_factor_direction, + SteeringFactor::TURNING, ""); return output; } @@ -85,6 +99,7 @@ void LaneChangeModule::onExit() { clearWaitingApproval(); removeRTCStatus(); + steering_factor_interface_ptr_->clearSteeringFactors(); debug_marker_.markers.clear(); current_state_ = BT::NodeStatus::IDLE; RCLCPP_DEBUG(getLogger(), "LANE_CHANGE onExit"); @@ -97,7 +112,7 @@ bool LaneChangeModule::isExecutionRequested() const } // Get lane change lanes - const auto current_lanes = getCurrentLanes(); + const auto current_lanes = util::getCurrentLanes(planner_data_); const auto lane_change_lanes = getLaneChangeLanes(current_lanes, lane_change_lane_length_); // Find lane change path @@ -115,7 +130,7 @@ bool LaneChangeModule::isExecutionReady() const } // Get lane change lanes - const auto current_lanes = getCurrentLanes(); + const auto current_lanes = util::getCurrentLanes(planner_data_); const auto lane_change_lanes = getLaneChangeLanes(current_lanes, lane_change_lane_length_); LaneChangePath selected_path; @@ -147,16 +162,17 @@ BT::NodeStatus LaneChangeModule::updateState() BehaviorModuleOutput LaneChangeModule::plan() { - constexpr double RESAMPLE_INTERVAL = 1.0; + constexpr double RESAMPLE_INTERVAL{1.0}; auto path = util::resamplePathWithSpline(status_.lane_change_path.path, RESAMPLE_INTERVAL); // Generate drivable area { - const auto common_parameters = planner_data_->parameters; + const auto & common_parameters = planner_data_->parameters; lanelet::ConstLanelets lanes; + lanes.reserve(status_.current_lanes.size() + status_.lane_change_lanes.size()); lanes.insert(lanes.end(), status_.current_lanes.begin(), status_.current_lanes.end()); lanes.insert(lanes.end(), status_.lane_change_lanes.begin(), status_.lane_change_lanes.end()); - const double resolution = common_parameters.drivable_area_resolution; + const double & resolution = common_parameters.drivable_area_resolution; path.drivable_area = util::generateDrivableArea( path, lanes, resolution, common_parameters.vehicle_length, planner_data_); } @@ -183,7 +199,7 @@ CandidateOutput LaneChangeModule::planCandidate() const CandidateOutput output; // Get lane change lanes - const auto current_lanes = getCurrentLanes(); + const auto current_lanes = util::getCurrentLanes(planner_data_); const auto lane_change_lanes = getLaneChangeLanes(current_lanes, lane_change_lane_length_); LaneChangePath selected_path; @@ -195,8 +211,8 @@ CandidateOutput LaneChangeModule::planCandidate() const return output; } - const auto start_idx = selected_path.shift_point.start_idx; - const auto end_idx = selected_path.shift_point.end_idx; + const auto & start_idx = selected_path.shift_point.start_idx; + const auto & end_idx = selected_path.shift_point.end_idx; output.path_candidate = selected_path.path; output.lateral_shift = selected_path.shifted_path.shift_length.at(end_idx) - @@ -208,6 +224,18 @@ CandidateOutput LaneChangeModule::planCandidate() const selected_path.path.points, planner_data_->self_pose->pose.position, selected_path.shift_point.end.position); + const uint16_t steering_factor_direction = std::invoke([&output]() { + if (output.lateral_shift > 0.0) { + return SteeringFactor::LEFT; + } + return SteeringFactor::RIGHT; + }); + + steering_factor_interface_ptr_->updateSteeringFactor( + {selected_path.shift_point.start, selected_path.shift_point.end}, + {output.start_distance_to_path_change, output.finish_distance_to_path_change}, + SteeringFactor::LANE_CHANGE, steering_factor_direction, SteeringFactor::APPROACHING, ""); + return output; } @@ -224,7 +252,7 @@ BehaviorModuleOutput LaneChangeModule::planWaitingApproval() void LaneChangeModule::updateLaneChangeStatus() { - const auto current_lanes = getCurrentLanes(); + const auto current_lanes = util::getCurrentLanes(planner_data_); status_.current_lanes = current_lanes; // Get lane change lanes @@ -256,12 +284,12 @@ PathWithLaneId LaneChangeModule::getReferencePath() const const auto & route_handler = planner_data_->route_handler; const auto current_pose = planner_data_->self_pose->pose; - const auto common_parameters = planner_data_->parameters; + const auto & common_parameters = planner_data_->parameters; // Set header reference_path.header = route_handler->getRouteHeader(); - const auto current_lanes = getCurrentLanes(); + const auto current_lanes = util::getCurrentLanes(planner_data_); if (current_lanes.empty()) { return reference_path; @@ -284,7 +312,7 @@ PathWithLaneId LaneChangeModule::getReferencePath() const *route_handler, current_lanes, current_pose, common_parameters.backward_path_length, common_parameters.forward_path_length, common_parameters, optional_lengths); } - const double buffer = + const double & buffer = common_parameters.backward_length_buffer_for_end_of_lane; // buffer for min_lane_change_length const double lane_change_buffer = num_lane_change * (common_parameters.minimum_lane_change_length + buffer) + optional_lengths; @@ -300,30 +328,13 @@ PathWithLaneId LaneChangeModule::getReferencePath() const return reference_path; } -lanelet::ConstLanelets LaneChangeModule::getCurrentLanes() const -{ - const auto & route_handler = planner_data_->route_handler; - const auto current_pose = planner_data_->self_pose->pose; - const auto common_parameters = planner_data_->parameters; - - lanelet::ConstLanelet current_lane; - if (!route_handler->getClosestLaneletWithinRoute(current_pose, ¤t_lane)) { - RCLCPP_ERROR_THROTTLE( - getLogger(), *clock_, 5000, "failed to find closest lanelet within route!!!"); - return {}; // TODO(Horibe) what should be returned? - } - - // For current_lanes with desired length - return route_handler->getLaneletSequence( - current_lane, current_pose, common_parameters.backward_path_length, - common_parameters.forward_path_length); -} - lanelet::ConstLanelets LaneChangeModule::getLaneChangeLanes( const lanelet::ConstLanelets & current_lanes, const double lane_change_lane_length) const { lanelet::ConstLanelets lane_change_lanes; const auto & route_handler = planner_data_->route_handler; + const auto & minimum_lane_change_length = planner_data_->parameters.minimum_lane_change_length; + const auto & lane_change_prepare_duration = parameters_->lane_change_prepare_duration; const auto current_pose = planner_data_->self_pose->pose; const auto current_twist = planner_data_->self_odometry->twist.twist; @@ -333,11 +344,9 @@ lanelet::ConstLanelets LaneChangeModule::getLaneChangeLanes( // Get lane change lanes lanelet::ConstLanelet current_lane; - lanelet::utils::query::getClosestLanelet( - current_lanes, planner_data_->self_pose->pose, ¤t_lane); - const double lane_change_prepare_length = std::max( - current_twist.linear.x * parameters_->lane_change_prepare_duration, - planner_data_->parameters.minimum_lane_change_length); + lanelet::utils::query::getClosestLanelet(current_lanes, current_pose, ¤t_lane); + const double lane_change_prepare_length = + std::max(current_twist.linear.x * lane_change_prepare_duration, minimum_lane_change_length); lanelet::ConstLanelets current_check_lanes = route_handler->getLaneletSequence(current_lane, current_pose, 0.0, lane_change_prepare_length); lanelet::ConstLanelet lane_change_lane; @@ -355,14 +364,12 @@ std::pair LaneChangeModule::getSafePath( const lanelet::ConstLanelets & lane_change_lanes, const double check_distance, LaneChangePath & safe_path) const { - std::vector valid_paths; - const auto & route_handler = planner_data_->route_handler; const auto current_pose = planner_data_->self_pose->pose; const auto current_twist = planner_data_->self_odometry->twist.twist; - const auto common_parameters = planner_data_->parameters; + const auto & common_parameters = planner_data_->parameters; - const auto current_lanes = getCurrentLanes(); + const auto current_lanes = util::getCurrentLanes(planner_data_); if (!lane_change_lanes.empty()) { // find candidate paths @@ -382,7 +389,7 @@ std::pair LaneChangeModule::getSafePath( } // select valid path - valid_paths = lane_change_utils::selectValidPaths( + const LaneChangePaths valid_paths = lane_change_utils::selectValidPaths( lane_change_paths, current_lanes, check_lanes, *route_handler->getOverallGraphPtr(), current_pose, route_handler->isInGoalRouteSection(current_lanes.back()), route_handler->getGoalPose()); @@ -394,7 +401,7 @@ std::pair LaneChangeModule::getSafePath( // select safe path object_debug_.clear(); - bool found_safe_path = lane_change_utils::selectSafePath( + const bool found_safe_path = lane_change_utils::selectSafePath( valid_paths, current_lanes, check_lanes, planner_data_->dynamic_object, current_pose, current_twist, common_parameters, *parameters_, &safe_path, object_debug_); @@ -414,8 +421,8 @@ bool LaneChangeModule::isSafe() const { return status_.is_safe; } bool LaneChangeModule::isNearEndOfLane() const { - const auto current_pose = planner_data_->self_pose->pose; - const auto common_parameters = planner_data_->parameters; + const auto & current_pose = planner_data_->self_pose->pose; + const auto & common_parameters = planner_data_->parameters; const double threshold = 5 + common_parameters.minimum_lane_change_length; return std::max(0.0, util::getDistanceToEndOfLane(current_pose, status_.current_lanes)) < @@ -424,8 +431,8 @@ bool LaneChangeModule::isNearEndOfLane() const bool LaneChangeModule::isCurrentSpeedLow() const { - const auto current_twist = planner_data_->self_odometry->twist.twist; - const double threshold_kmph = 10; + const auto & current_twist = planner_data_->self_odometry->twist.twist; + constexpr double threshold_kmph = 10.0; return util::l2Norm(current_twist.linear) < threshold_kmph * 1000 / 3600; } @@ -434,10 +441,10 @@ bool LaneChangeModule::isAbortConditionSatisfied() const const auto & route_handler = planner_data_->route_handler; const auto current_pose = planner_data_->self_pose->pose; const auto current_twist = planner_data_->self_odometry->twist.twist; - const auto objects = planner_data_->dynamic_object; - const auto common_parameters = planner_data_->parameters; + const auto & dynamic_objects = planner_data_->dynamic_object; + const auto & common_parameters = planner_data_->parameters; - const auto current_lanes = status_.current_lanes; + const auto & current_lanes = status_.current_lanes; // check abort enable flag if (!parameters_->enable_abort_lane_change) { @@ -459,8 +466,8 @@ bool LaneChangeModule::isAbortConditionSatisfied() const } // check if lane change path is still safe - bool is_path_safe{false}; - { + const bool is_path_safe = std::invoke([this, &route_handler, &dynamic_objects, ¤t_lanes, + ¤t_pose, ¤t_twist, &common_parameters]() { constexpr double check_distance = 100.0; // get lanes used for detection const auto & path = status_.lane_change_path; @@ -469,53 +476,58 @@ bool LaneChangeModule::isAbortConditionSatisfied() const const auto check_lanes = route_handler->getCheckTargetLanesFromPath( path.path, status_.lane_change_lanes, check_distance_with_path); - const size_t current_seg_idx = findEgoSegmentIndex(path.path.points); std::unordered_map debug_data; - is_path_safe = lane_change_utils::isLaneChangePathSafe( - path.path, current_lanes, check_lanes, objects, current_pose, current_seg_idx, current_twist, - common_parameters, *parameters_, debug_data, false, status_.lane_change_path.acceleration); - } - - // check vehicle velocity thresh - const bool is_velocity_low = - util::l2Norm(current_twist.linear) < parameters_->abort_lane_change_velocity_thresh; - - // check if vehicle is within lane - bool is_within_original_lane = false; - { - const auto lane_length = lanelet::utils::getLaneletLength2d(current_lanes); - const auto lane_poly = lanelet::utils::getPolygonFromArcLength(current_lanes, 0, lane_length); - const auto vehicle_poly = util::getVehiclePolygon( - current_pose, common_parameters.vehicle_width, common_parameters.base_link2front); - is_within_original_lane = boost::geometry::within( - lanelet::utils::to2D(vehicle_poly).basicPolygon(), - lanelet::utils::to2D(lane_poly).basicPolygon()); - } - - // check distance from original lane's centerline - bool is_distance_small = false; - { - const auto centerline2d = lanelet::utils::to2D(closest_lanelet.centerline()).basicLineString(); - lanelet::BasicPoint2d vehicle_pose2d(current_pose.position.x, current_pose.position.y); - const double distance = lanelet::geometry::distance2d(centerline2d, vehicle_pose2d); - is_distance_small = distance < parameters_->abort_lane_change_distance_thresh; - } - // check angle thresh from original lane - bool is_angle_diff_small = false; - { - const double lane_angle = - lanelet::utils::getLaneletAngle(closest_lanelet, current_pose.position); - const double vehicle_yaw = tf2::getYaw(current_pose.orientation); - const double yaw_diff = tier4_autoware_utils::normalizeRadian(lane_angle - vehicle_yaw); - is_angle_diff_small = std::abs(yaw_diff) < parameters_->abort_lane_change_angle_thresh; - } + const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.path.points, current_pose, common_parameters.ego_nearest_dist_threshold, + common_parameters.ego_nearest_yaw_threshold); + return lane_change_utils::isLaneChangePathSafe( + path.path, current_lanes, check_lanes, dynamic_objects, current_pose, current_seg_idx, + current_twist, common_parameters, *parameters_, debug_data, false, + status_.lane_change_path.acceleration); + }); // abort only if velocity is low or vehicle pose is close enough if (!is_path_safe) { + // check vehicle velocity thresh + const bool is_velocity_low = + util::l2Norm(current_twist.linear) < parameters_->abort_lane_change_velocity_thresh; + + // check if vehicle is within lane + const bool is_within_original_lane = + std::invoke([¤t_lanes, current_pose, common_parameters]() { + const auto lane_length = lanelet::utils::getLaneletLength2d(current_lanes); + const auto lane_poly = + lanelet::utils::getPolygonFromArcLength(current_lanes, 0, lane_length); + const auto vehicle_poly = util::getVehiclePolygon( + current_pose, common_parameters.vehicle_width, common_parameters.base_link2front); + return boost::geometry::within( + lanelet::utils::to2D(vehicle_poly).basicPolygon(), + lanelet::utils::to2D(lane_poly).basicPolygon()); + }); + if (is_velocity_low && is_within_original_lane) { return true; } + + // check distance from original lane's centerline + const bool is_distance_small = std::invoke([this, &closest_lanelet, ¤t_pose]() { + const auto centerline2d = + lanelet::utils::to2D(closest_lanelet.centerline()).basicLineString(); + lanelet::BasicPoint2d vehicle_pose2d(current_pose.position.x, current_pose.position.y); + const double distance = lanelet::geometry::distance2d(centerline2d, vehicle_pose2d); + return distance < parameters_->abort_lane_change_distance_thresh; + }); + + // check angle thresh from original lane + const bool is_angle_diff_small = std::invoke([this, closest_lanelet, ¤t_pose]() { + const double lane_angle = + lanelet::utils::getLaneletAngle(closest_lanelet, current_pose.position); + const double vehicle_yaw = tf2::getYaw(current_pose.orientation); + const double yaw_diff = tier4_autoware_utils::normalizeRadian(lane_angle - vehicle_yaw); + return std::abs(yaw_diff) < parameters_->abort_lane_change_angle_thresh; + }); + if (is_distance_small && is_angle_diff_small) { return true; } @@ -530,7 +542,7 @@ bool LaneChangeModule::isAbortConditionSatisfied() const bool LaneChangeModule::hasFinishedLaneChange() const { - const auto current_pose = planner_data_->self_pose->pose; + const auto & current_pose = planner_data_->self_pose->pose; const auto arclength_current = lanelet::utils::getArcCoordinates(status_.lane_change_lanes, current_pose); const double travel_distance = arclength_current.length - status_.start_distance; diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp index 7a8a4d27cd19f..44a521734934b 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp @@ -33,7 +33,10 @@ namespace behavior_path_planner::lane_change_utils { -PathWithLaneId combineReferencePath(const PathWithLaneId path1, const PathWithLaneId path2) +using autoware_auto_planning_msgs::msg::PathPointWithLaneId; +using lanelet::ArcCoordinates; + +PathWithLaneId combineReferencePath(const PathWithLaneId & path1, const PathWithLaneId & path2) { PathWithLaneId path; path.points.insert(path.points.end(), path1.points.begin(), path1.points.end()); @@ -79,12 +82,85 @@ double getDistanceWhenDecelerate( const auto distance = velocity * duration + 0.5 * expected_acceleration * std::pow(duration, 2); return std::max(distance, minimum_distance); } -std::vector getLaneChangePaths( + +std::optional constructCandidatePath( + const PathWithLaneId & prepare_segment, const PathWithLaneId & lane_changing_segment, + const PathWithLaneId & target_lane_reference_path, const ShiftPoint & shift_point, + const lanelet::ConstLanelets & original_lanelets, const lanelet::ConstLanelets & target_lanelets, + const double & acceleration, const double & prepare_distance, const double & lane_change_distance, + const double & lane_changing_duration, const double & minimum_lane_change_velocity) +{ + PathShifter path_shifter; + path_shifter.setPath(target_lane_reference_path); + path_shifter.addShiftPoint(shift_point); + ShiftedPath shifted_path; + + // offset front side + bool offset_back = false; + if (!path_shifter.generate(&shifted_path, offset_back)) { + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("behavior_path_planner").get_child("lane_change").get_child("util"), + "failed to generate shifted path."); + } + + LaneChangePath candidate_path; + candidate_path.acceleration = acceleration; + candidate_path.preparation_length = prepare_distance; + candidate_path.lane_change_length = lane_change_distance; + candidate_path.shift_point = shift_point; + + const PathPointWithLaneId & lane_changing_start_point = prepare_segment.points.back(); + const PathPointWithLaneId & lane_changing_end_point = lane_changing_segment.points.front(); + const Pose & lane_changing_end_pose = lane_changing_end_point.point.pose; + const auto lanechange_end_idx = + motion_utils::findNearestIndex(shifted_path.path.points, lane_changing_end_pose); + if (lanechange_end_idx) { + for (size_t i = 0; i < shifted_path.path.points.size(); ++i) { + auto & point = shifted_path.path.points.at(i); + if (i < *lanechange_end_idx) { + point.lane_ids.insert( + point.lane_ids.end(), lane_changing_start_point.lane_ids.begin(), + lane_changing_start_point.lane_ids.end()); + point.lane_ids.insert( + point.lane_ids.end(), lane_changing_end_point.lane_ids.begin(), + lane_changing_end_point.lane_ids.end()); + point.point.longitudinal_velocity_mps = std::min( + point.point.longitudinal_velocity_mps, + lane_changing_start_point.point.longitudinal_velocity_mps); + continue; + } + point.point.longitudinal_velocity_mps = std::min( + point.point.longitudinal_velocity_mps, + static_cast( + std::max(lane_change_distance / lane_changing_duration, minimum_lane_change_velocity))); + const auto nearest_idx = + motion_utils::findNearestIndex(lane_changing_segment.points, point.point.pose); + point.lane_ids = lane_changing_segment.points.at(*nearest_idx).lane_ids; + } + + candidate_path.path = combineReferencePath(prepare_segment, shifted_path.path); + candidate_path.shifted_path = shifted_path; + } else { + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("behavior_path_planner").get_child("lane_change").get_child("util"), + "lane change end idx not found on target path."); + return std::nullopt; + } + + // check candidate path is in lanelet + if (!isPathInLanelets(candidate_path.path, original_lanelets, target_lanelets)) { + return std::nullopt; + } + + return std::optional{candidate_path}; +} + +LaneChangePaths getLaneChangePaths( const RouteHandler & route_handler, const lanelet::ConstLanelets & original_lanelets, const lanelet::ConstLanelets & target_lanelets, const Pose & pose, const Twist & twist, const BehaviorPathPlannerParameters & common_parameter, const LaneChangeParameters & parameter) { - std::vector candidate_paths; + LaneChangePaths candidate_paths{}; if (original_lanelets.empty() || target_lanelets.empty()) { return candidate_paths; @@ -113,13 +189,13 @@ std::vector getLaneChangePaths( for (double acceleration = 0.0; acceleration >= -maximum_deceleration; acceleration -= acceleration_resolution) { - const double lane_change_prepare_velocity = getExpectedVelocityWhenDecelerate( + const double prepare_speed = getExpectedVelocityWhenDecelerate( current_velocity, acceleration, lane_change_prepare_duration); - const double lane_changing_velocity = getExpectedVelocityWhenDecelerate( - lane_change_prepare_velocity, acceleration, lane_changing_duration); + const double lane_changing_speed = + getExpectedVelocityWhenDecelerate(prepare_speed, acceleration, lane_changing_duration); // skip if velocity becomes less than zero before finishing lane change - if (lane_changing_velocity < 0.0) { + if (lane_changing_speed < 0.0) { continue; } @@ -127,150 +203,63 @@ std::vector getLaneChangePaths( const double prepare_distance = getDistanceWhenDecelerate( current_velocity, acceleration, lane_change_prepare_duration, minimum_lane_change_prepare_distance); + const double lane_changing_distance = getDistanceWhenDecelerate( - lane_change_prepare_velocity, acceleration, lane_changing_duration, - minimum_lane_change_length + buffer); + prepare_speed, acceleration, lane_changing_duration, minimum_lane_change_length); if (prepare_distance < target_distance) { continue; } - PathWithLaneId reference_path1; - { - const auto arc_position = lanelet::utils::getArcCoordinates(original_lanelets, pose); - const double s_start = arc_position.length - backward_path_length; - const double s_end = arc_position.length + prepare_distance; - reference_path1 = route_handler.getCenterLinePath(original_lanelets, s_start, s_end); - } - - reference_path1.points.back().point.longitudinal_velocity_mps = std::min( - reference_path1.points.back().point.longitudinal_velocity_mps, - static_cast( - std::max(prepare_distance / lane_change_prepare_duration, minimum_lane_change_velocity))); - - PathWithLaneId reference_path2{}; - { - const double lane_length = lanelet::utils::getLaneletLength2d(target_lanelets); - const auto arc_position = lanelet::utils::getArcCoordinates(target_lanelets, pose); - const int num = std::abs(route_handler.getNumLaneToPreferredLane(target_lanelets.back())); - double s_start = arc_position.length + prepare_distance + lane_changing_distance; - s_start = std::min(s_start, lane_length - num * minimum_lane_change_length); - double s_end = s_start + forward_path_length; - s_end = std::min(s_end, lane_length - num * (minimum_lane_change_length + buffer)); - s_end = std::max(s_end, s_start + std::numeric_limits::epsilon()); - reference_path2 = route_handler.getCenterLinePath(target_lanelets, s_start, s_end); - } + const PathWithLaneId prepare_segment_reference = getLaneChangePathPrepareSegment( + route_handler, original_lanelets, pose, backward_path_length, prepare_distance, + lane_change_prepare_duration, minimum_lane_change_velocity); - for (auto & point : reference_path2.points) { - point.point.longitudinal_velocity_mps = std::min( - point.point.longitudinal_velocity_mps, - static_cast( - std::max(lane_changing_distance / lane_changing_duration, minimum_lane_change_velocity))); - } + const PathWithLaneId lane_changing_segment_reference = getLaneChangePathLaneChangingSegment( + route_handler, target_lanelets, pose, forward_path_length, prepare_distance, + lane_changing_distance, minimum_lane_change_length, buffer, lane_changing_duration, + minimum_lane_change_velocity); - if (reference_path1.points.empty() || reference_path2.points.empty()) { + if ( + prepare_segment_reference.points.empty() || lane_changing_segment_reference.points.empty()) { RCLCPP_ERROR_STREAM( rclcpp::get_logger("behavior_path_planner").get_child("lane_change").get_child("util"), "reference path is empty!! something wrong..."); continue; } - LaneChangePath candidate_path; - candidate_path.acceleration = acceleration; - candidate_path.preparation_length = prepare_distance; - candidate_path.lane_change_length = lane_changing_distance; - - PathWithLaneId target_lane_reference_path; - { - const lanelet::ArcCoordinates lane_change_start_arc_position = - lanelet::utils::getArcCoordinates( - target_lanelets, reference_path1.points.back().point.pose); - double s_start = lane_change_start_arc_position.length; - double s_end = s_start + prepare_distance + lane_changing_distance + forward_path_length; - target_lane_reference_path = route_handler.getCenterLinePath(target_lanelets, s_start, s_end); - } + const Pose & lane_changing_start_pose = prepare_segment_reference.points.back().point.pose; - ShiftPoint shift_point; - { - const Pose lane_change_start_on_self_lane = reference_path1.points.back().point.pose; - const Pose lane_change_end_on_target_lane = reference_path2.points.front().point.pose; - const lanelet::ArcCoordinates lane_change_start_on_self_lane_arc = - lanelet::utils::getArcCoordinates(target_lanelets, lane_change_start_on_self_lane); - shift_point.length = lane_change_start_on_self_lane_arc.distance; - shift_point.start = lane_change_start_on_self_lane; - shift_point.end = lane_change_end_on_target_lane; - shift_point.start_idx = motion_utils::findNearestIndex( - target_lane_reference_path.points, lane_change_start_on_self_lane.position); - shift_point.end_idx = motion_utils::findNearestIndex( - target_lane_reference_path.points, lane_change_end_on_target_lane.position); - } - - PathShifter path_shifter; - path_shifter.setPath(target_lane_reference_path); - path_shifter.addShiftPoint(shift_point); - ShiftedPath shifted_path; + const PathWithLaneId target_lane_reference_path = getReferencePathFromTargetLane( + route_handler, target_lanelets, lane_changing_start_pose, prepare_distance, + lane_changing_distance, forward_path_length); - // offset front side - bool offset_back = false; - if (!path_shifter.generate(&shifted_path, offset_back)) { - RCLCPP_ERROR_STREAM( - rclcpp::get_logger("behavior_path_planner").get_child("lane_change").get_child("util"), - "failed to generate shifted path."); - } - const auto lanechange_end_idx = motion_utils::findNearestIndex( - shifted_path.path.points, reference_path2.points.front().point.pose); - if (lanechange_end_idx) { - for (size_t i = 0; i < shifted_path.path.points.size(); ++i) { - auto & point = shifted_path.path.points.at(i); - if (i < *lanechange_end_idx) { - point.lane_ids.insert( - point.lane_ids.end(), reference_path1.points.back().lane_ids.begin(), - reference_path1.points.back().lane_ids.end()); - point.lane_ids.insert( - point.lane_ids.end(), reference_path2.points.front().lane_ids.begin(), - reference_path2.points.front().lane_ids.end()); - point.point.longitudinal_velocity_mps = std::min( - point.point.longitudinal_velocity_mps, - reference_path1.points.back().point.longitudinal_velocity_mps); - continue; - } - point.point.longitudinal_velocity_mps = std::min( - point.point.longitudinal_velocity_mps, - static_cast(std::max( - lane_changing_distance / lane_changing_duration, minimum_lane_change_velocity))); - const auto nearest_idx = - motion_utils::findNearestIndex(reference_path2.points, point.point.pose); - point.lane_ids = reference_path2.points.at(*nearest_idx).lane_ids; - } + const ShiftPoint shift_point = getLaneChangeShiftPoint( + prepare_segment_reference, lane_changing_segment_reference, target_lanelets, + target_lane_reference_path); - candidate_path.path = combineReferencePath(reference_path1, shifted_path.path); - candidate_path.shifted_path = shifted_path; - candidate_path.shift_point = shift_point; - } else { - RCLCPP_ERROR_STREAM( - rclcpp::get_logger("behavior_path_planner").get_child("lane_change").get_child("util"), - "lane change end idx not found on target path."); - continue; - } + const auto candidate_path = constructCandidatePath( + prepare_segment_reference, lane_changing_segment_reference, target_lane_reference_path, + shift_point, original_lanelets, target_lanelets, acceleration, prepare_distance, + lane_changing_distance, lane_changing_duration, minimum_lane_change_velocity); - // check candidate path is in lanelet - if (!isPathInLanelets(candidate_path.path, original_lanelets, target_lanelets)) { + if (!candidate_path) { continue; } - candidate_paths.push_back(candidate_path); + candidate_paths.push_back(*candidate_path); } return candidate_paths; } -std::vector selectValidPaths( - const std::vector & paths, const lanelet::ConstLanelets & current_lanes, +LaneChangePaths selectValidPaths( + const LaneChangePaths & paths, const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, const lanelet::routing::RoutingGraphContainer & overall_graphs, const Pose & current_pose, const bool isInGoalRouteSection, const Pose & goal_pose) { - std::vector available_paths; + LaneChangePaths available_paths; for (const auto & path : paths) { if (hasEnoughDistance( @@ -284,7 +273,7 @@ std::vector selectValidPaths( } bool selectSafePath( - const std::vector & paths, const lanelet::ConstLanelets & current_lanes, + const LaneChangePaths & paths, const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, const PredictedObjects::ConstSharedPtr dynamic_objects, const Pose & current_pose, const Twist & current_twist, const BehaviorPathPlannerParameters & common_parameters, @@ -316,8 +305,8 @@ bool hasEnoughDistance( const bool isInGoalRouteSection, const Pose & goal_pose, const lanelet::routing::RoutingGraphContainer & overall_graphs) { - const double lane_change_prepare_distance = path.preparation_length; - const double lane_changing_distance = path.lane_change_length; + const double & lane_change_prepare_distance = path.preparation_length; + const double & lane_changing_distance = path.lane_change_length; const double lane_change_total_distance = lane_change_prepare_distance + lane_changing_distance; if (lane_change_total_distance > util::getDistanceToEndOfLane(current_pose, current_lanes)) { @@ -340,6 +329,12 @@ bool hasEnoughDistance( util::getDistanceToCrosswalk(current_pose, current_lanes, overall_graphs)) { return false; } + + // return is there is no target lanes. Else continue checking + if (target_lanes.empty()) { + return true; + } + return true; } @@ -347,7 +342,7 @@ bool isLaneChangePathSafe( const PathWithLaneId & path, const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, const PredictedObjects::ConstSharedPtr dynamic_objects, const Pose & current_pose, - const size_t current_seg_idx, const Twist & current_twist, + const size_t & current_seg_idx, const Twist & current_twist, const BehaviorPathPlannerParameters & common_parameters, const LaneChangeParameters & lane_change_parameters, std::unordered_map & debug_data, const bool use_buffer, @@ -363,19 +358,16 @@ bool isLaneChangePathSafe( const double time_resolution = lane_change_parameters.prediction_time_resolution; const auto & lane_change_prepare_duration = lane_change_parameters.lane_change_prepare_duration; + const auto & enable_collision_check_at_prepare_phase = + lane_change_parameters.enable_collision_check_at_prepare_phase; const auto & lane_changing_duration = lane_change_parameters.lane_changing_duration; - const double current_lane_check_start_time = - (!lane_change_parameters.enable_collision_check_at_prepare_phase) ? lane_change_prepare_duration - : 0.0; - const double current_lane_check_end_time = lane_change_prepare_duration + lane_changing_duration; - double target_lane_check_start_time = - (!lane_change_parameters.enable_collision_check_at_prepare_phase) ? lane_change_prepare_duration - : 0.0; - const double target_lane_check_end_time = lane_change_prepare_duration + lane_changing_duration; + const double check_start_time = + (enable_collision_check_at_prepare_phase) ? 0.0 : lane_change_prepare_duration; + const double check_end_time = lane_change_prepare_duration + lane_changing_duration; constexpr double ego_predicted_path_min_speed{1.0}; const auto vehicle_predicted_path = util::convertToPredictedPath( - path, current_twist, current_pose, current_seg_idx, target_lane_check_end_time, time_resolution, - acceleration, ego_predicted_path_min_speed); + path, current_twist, current_pose, static_cast(current_seg_idx), check_end_time, + time_resolution, acceleration, ego_predicted_path_min_speed); const auto arc = lanelet::utils::getArcCoordinates(current_lanes, current_pose); @@ -390,8 +382,8 @@ bool isLaneChangePathSafe( *dynamic_objects, current_lanes, arc.length, arc.length + check_distance); const double lateral_buffer = (use_buffer) ? 0.5 : 0.0; + const auto & vehicle_info = common_parameters.vehicle_info; const auto & vehicle_width = common_parameters.vehicle_width; - const auto & vehicle_length = common_parameters.vehicle_length; const auto current_lane_object_indices = util::filterObjectsIndicesByPath( *dynamic_objects, current_lane_object_indices_lanelet, path, vehicle_width / 2 + lateral_buffer); @@ -423,9 +415,9 @@ bool isLaneChangePathSafe( util::getPredictedPathFromObj(obj, lane_change_parameters.use_all_predicted_path); for (const auto & obj_path : predicted_paths) { if (!util::isSafeInLaneletCollisionCheck( - current_pose, current_twist, vehicle_predicted_path, vehicle_length, vehicle_width, - current_lane_check_start_time, current_lane_check_end_time, time_resolution, obj, - obj_path, common_parameters, current_debug_data.second)) { + current_pose, current_twist, vehicle_predicted_path, vehicle_info, check_start_time, + check_end_time, time_resolution, obj, obj_path, common_parameters, + current_debug_data.second)) { appendDebugInfo(current_debug_data, false); return false; } @@ -454,18 +446,17 @@ bool isLaneChangePathSafe( if (is_object_in_target) { for (const auto & obj_path : predicted_paths) { if (!util::isSafeInLaneletCollisionCheck( - current_pose, current_twist, vehicle_predicted_path, vehicle_length, vehicle_width, - target_lane_check_start_time, target_lane_check_end_time, time_resolution, obj, - obj_path, common_parameters, current_debug_data.second)) { + current_pose, current_twist, vehicle_predicted_path, vehicle_info, check_start_time, + check_end_time, time_resolution, obj, obj_path, common_parameters, + current_debug_data.second)) { appendDebugInfo(current_debug_data, false); return false; } } } else { if (!util::isSafeInFreeSpaceCollisionCheck( - current_pose, current_twist, vehicle_predicted_path, vehicle_length, vehicle_width, - target_lane_check_start_time, target_lane_check_end_time, time_resolution, obj, - common_parameters, current_debug_data.second)) { + current_pose, current_twist, vehicle_predicted_path, vehicle_info, check_start_time, + check_end_time, time_resolution, obj, common_parameters, current_debug_data.second)) { appendDebugInfo(current_debug_data, false); return false; } @@ -474,4 +465,120 @@ bool isLaneChangePathSafe( } return true; } + +PathWithLaneId getReferencePathFromTargetLane( + const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes, + const Pose & lane_changing_start_pose, const double & prepare_distance, + const double & lane_changing_distance, const double & forward_path_length) +{ + const ArcCoordinates lane_change_start_arc_position = + lanelet::utils::getArcCoordinates(target_lanes, lane_changing_start_pose); + const double & s_start = lane_change_start_arc_position.length; + const double s_end = s_start + prepare_distance + lane_changing_distance + forward_path_length; + return route_handler.getCenterLinePath(target_lanes, s_start, s_end); +} + +PathWithLaneId getReferencePathFromTargetLane( + const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes, + const Pose & in_target_front_pose, const Pose & in_target_end_pose) +{ + const ArcCoordinates lane_change_start_arc_position = + lanelet::utils::getArcCoordinates(target_lanes, in_target_front_pose); + + const ArcCoordinates lane_change_end_arc_position = + lanelet::utils::getArcCoordinates(target_lanes, in_target_end_pose); + + const double & s_start = lane_change_start_arc_position.length; + const double & s_end = lane_change_end_arc_position.length; + return route_handler.getCenterLinePath(target_lanes, s_start, s_end); +} + +ShiftPoint getLaneChangeShiftPoint( + const PathWithLaneId & path1, const PathWithLaneId & path2, + const lanelet::ConstLanelets & target_lanes, const PathWithLaneId & reference_path) +{ + const Pose & lane_change_start_on_self_lane = path1.points.back().point.pose; + const Pose & lane_change_end_on_target_lane = path2.points.front().point.pose; + const ArcCoordinates lane_change_start_on_self_lane_arc = + lanelet::utils::getArcCoordinates(target_lanes, lane_change_start_on_self_lane); + + ShiftPoint shift_point; + shift_point.length = lane_change_start_on_self_lane_arc.distance; + shift_point.start = lane_change_start_on_self_lane; + shift_point.end = lane_change_end_on_target_lane; + shift_point.start_idx = + motion_utils::findNearestIndex(reference_path.points, lane_change_start_on_self_lane.position); + shift_point.end_idx = + motion_utils::findNearestIndex(reference_path.points, lane_change_end_on_target_lane.position); + + return shift_point; +} + +PathWithLaneId getLaneChangePathPrepareSegment( + const RouteHandler & route_handler, const lanelet::ConstLanelets & original_lanelets, + const Pose & current_pose, const double & backward_path_length, const double & prepare_distance, + const double & prepare_duration, const double & minimum_lane_change_velocity) +{ + if (original_lanelets.empty()) { + return PathWithLaneId(); + } + + const ArcCoordinates arc_position = + lanelet::utils::getArcCoordinates(original_lanelets, current_pose); + const double s_start = arc_position.length - backward_path_length; + const double s_end = arc_position.length + prepare_distance; + + PathWithLaneId prepare_segment = + route_handler.getCenterLinePath(original_lanelets, s_start, s_end); + + prepare_segment.points.back().point.longitudinal_velocity_mps = std::min( + prepare_segment.points.back().point.longitudinal_velocity_mps, + static_cast( + std::max(prepare_distance / prepare_duration, minimum_lane_change_velocity))); + + return prepare_segment; +} + +PathWithLaneId getLaneChangePathLaneChangingSegment( + const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanelets, + const Pose & current_pose, const double & forward_path_length, const double & prepare_distance, + const double & lane_change_distance, const double & minimum_lane_change_length, + const double & lane_change_distance_buffer, const double & lane_changing_duration, + const double & minimum_lane_change_velocity) +{ + if (target_lanelets.empty()) { + return PathWithLaneId(); + } + + const ArcCoordinates arc_position = + lanelet::utils::getArcCoordinates(target_lanelets, current_pose); + const double lane_length = lanelet::utils::getLaneletLength2d(target_lanelets); + const int num = std::abs(route_handler.getNumLaneToPreferredLane(target_lanelets.back())); + + const double s_start = std::invoke([&arc_position, &prepare_distance, &lane_change_distance, + &lane_length, &num, &minimum_lane_change_length]() { + const double s_start = arc_position.length + prepare_distance + lane_change_distance; + return std::min(s_start, lane_length - num * minimum_lane_change_length); + }); + + const double s_end = std::invoke([&s_start, &forward_path_length, &lane_length, &num, + &minimum_lane_change_length, &lane_change_distance_buffer]() { + const double s_end = std::min( + s_start + forward_path_length, + lane_length - num * (minimum_lane_change_length + lane_change_distance_buffer)); + return std::max(s_end, s_start + std::numeric_limits::epsilon()); + }); + + PathWithLaneId lane_changing_segment = + route_handler.getCenterLinePath(target_lanelets, s_start, s_end); + for (auto & point : lane_changing_segment.points) { + point.point.longitudinal_velocity_mps = std::min( + point.point.longitudinal_velocity_mps, + static_cast( + std::max(lane_change_distance / lane_changing_duration, minimum_lane_change_velocity))); + } + + return lane_changing_segment; +} + } // namespace behavior_path_planner::lane_change_utils diff --git a/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp b/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp index d7f2df9dea01f..901378416493c 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp @@ -40,6 +40,7 @@ PullOutModule::PullOutModule( vehicle_info_{vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo()} { rtc_interface_ptr_ = std::make_shared(&node, "pull_out"); + steering_factor_interface_ptr_ = std::make_unique(&node, "pull_out"); lane_departure_checker_ = std::make_shared(); lane_departure_checker_->setVehicleInfo(vehicle_info_); @@ -99,6 +100,7 @@ void PullOutModule::onExit() { clearWaitingApproval(); removeRTCStatus(); + steering_factor_interface_ptr_->clearSteeringFactors(); current_state_ = BT::NodeStatus::IDLE; RCLCPP_DEBUG(getLogger(), "PULL_OUT onExit"); } @@ -176,6 +178,15 @@ BehaviorModuleOutput PullOutModule::plan() output.turn_signal_info = calcTurnSignalInfo(status_.pull_out_path.start_pose, status_.pull_out_path.end_pose); + const uint16_t steering_factor_direction = std::invoke([&output]() { + if (output.turn_signal_info.turn_signal.command == TurnIndicatorsCommand::ENABLE_LEFT) { + return SteeringFactor::LEFT; + } else if (output.turn_signal_info.turn_signal.command == TurnIndicatorsCommand::ENABLE_RIGHT) { + return SteeringFactor::RIGHT; + } + return SteeringFactor::STRAIGHT; + }); + if (status_.back_finished) { const double start_distance = motion_utils::calcSignedArcLength( path.points, planner_data_->self_pose->pose.position, @@ -184,11 +195,20 @@ BehaviorModuleOutput PullOutModule::plan() path.points, planner_data_->self_pose->pose.position, status_.pull_out_path.end_pose.position); updateRTCStatus(start_distance, finish_distance); + // TODO(tkhmy) add handle status TRYING + steering_factor_interface_ptr_->updateSteeringFactor( + {status_.pull_out_path.start_pose, status_.pull_out_path.end_pose}, + {start_distance, finish_distance}, SteeringFactor::PULL_OUT, steering_factor_direction, + SteeringFactor::TURNING, ""); } else { const double distance = motion_utils::calcSignedArcLength( path.points, planner_data_->self_pose->pose.position, status_.pull_out_path.start_pose.position); updateRTCStatus(0.0, distance); + // TODO(tkhmy) add handle status TRYING + steering_factor_interface_ptr_->updateSteeringFactor( + {status_.pull_out_path.start_pose, status_.pull_out_path.end_pose}, {0.0, distance}, + SteeringFactor::PULL_OUT, steering_factor_direction, SteeringFactor::TURNING, ""); } setDebugData(); @@ -257,6 +277,16 @@ BehaviorModuleOutput PullOutModule::planWaitingApproval() output.path_candidate = std::make_shared(candidate_path); waitApproval(); + + const uint16_t steering_factor_direction = std::invoke([&output]() { + if (output.turn_signal_info.turn_signal.command == TurnIndicatorsCommand::ENABLE_LEFT) { + return SteeringFactor::LEFT; + } else if (output.turn_signal_info.turn_signal.command == TurnIndicatorsCommand::ENABLE_RIGHT) { + return SteeringFactor::RIGHT; + } + return SteeringFactor::STRAIGHT; + }); + if (status_.back_finished) { const double start_distance = motion_utils::calcSignedArcLength( candidate_path.points, planner_data_->self_pose->pose.position, @@ -265,14 +295,22 @@ BehaviorModuleOutput PullOutModule::planWaitingApproval() candidate_path.points, planner_data_->self_pose->pose.position, status_.pull_out_path.end_pose.position); updateRTCStatus(start_distance, finish_distance); + steering_factor_interface_ptr_->updateSteeringFactor( + {status_.pull_out_path.start_pose, status_.pull_out_path.end_pose}, + {start_distance, finish_distance}, SteeringFactor::PULL_OUT, steering_factor_direction, + SteeringFactor::APPROACHING, ""); } else { const double distance = motion_utils::calcSignedArcLength( candidate_path.points, planner_data_->self_pose->pose.position, status_.pull_out_path.start_pose.position); updateRTCStatus(0.0, distance); + steering_factor_interface_ptr_->updateSteeringFactor( + {status_.pull_out_path.start_pose, status_.pull_out_path.end_pose}, {0.0, distance}, + SteeringFactor::PULL_OUT, steering_factor_direction, SteeringFactor::APPROACHING, ""); } setDebugData(); + return output; } diff --git a/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp b/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp index 28dd33170ba6c..cce0105f52f33 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp @@ -52,6 +52,7 @@ PullOverModule::PullOverModule( vehicle_info_{vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo()} { rtc_interface_ptr_ = std::make_shared(&node, "pull_over"); + steering_factor_interface_ptr_ = std::make_unique(&node, "pull_over"); goal_pose_pub_ = node.create_publisher("/planning/scenario_planning/modified_goal", 1); @@ -160,6 +161,7 @@ void PullOverModule::onExit() RCLCPP_DEBUG(getLogger(), "PULL_OVER onExit"); clearWaitingApproval(); removeRTCStatus(); + steering_factor_interface_ptr_->clearSteeringFactors(); // A child node must never return IDLE // https://github.com/BehaviorTree/BehaviorTree.CPP/blob/master/include/behaviortree_cpp_v3/basic_types.h#L34 @@ -566,6 +568,7 @@ BehaviorModuleOutput PullOverModule::plan() // request approval again one the final path is decided waitApproval(); removeRTCStatus(); + steering_factor_interface_ptr_->clearSteeringFactors(); uuid_ = generateUUID(); current_state_ = BT::NodeStatus::SUCCESS; // for breaking loop status_.has_requested_approval_ = true; @@ -679,6 +682,21 @@ BehaviorModuleOutput PullOverModule::plan() goal_pose_pub_->publish(goal_pose_stamped); } + const uint16_t steering_factor_direction = std::invoke([this]() { + if (getTurnInfo().first.command == TurnIndicatorsCommand::ENABLE_LEFT) { + return SteeringFactor::LEFT; + } else if (getTurnInfo().first.command == TurnIndicatorsCommand::ENABLE_RIGHT) { + return SteeringFactor::RIGHT; + } + return SteeringFactor::STRAIGHT; + }); + + // TODO(tkhmy) add handle status TRYING + steering_factor_interface_ptr_->updateSteeringFactor( + {getParkingStartPose(), modified_goal_pose_}, + {distance_to_path_change.first, distance_to_path_change.second}, SteeringFactor::PULL_OVER, + steering_factor_direction, SteeringFactor::TURNING, ""); + // For evaluations if (parameters_.print_debug_info) { printParkingPositionError(); @@ -705,6 +723,20 @@ BehaviorModuleOutput PullOverModule::planWaitingApproval() const auto distance_to_path_change = calcDistanceToPathChange(); updateRTCStatus(distance_to_path_change.first, distance_to_path_change.second); + + const uint16_t steering_factor_direction = std::invoke([this]() { + if (getTurnInfo().first.command == TurnIndicatorsCommand::ENABLE_LEFT) { + return SteeringFactor::LEFT; + } else if (getTurnInfo().first.command == TurnIndicatorsCommand::ENABLE_RIGHT) { + return SteeringFactor::RIGHT; + } + return SteeringFactor::STRAIGHT; + }); + + steering_factor_interface_ptr_->updateSteeringFactor( + {getParkingStartPose(), modified_goal_pose_}, + {distance_to_path_change.first, distance_to_path_change.second}, SteeringFactor::PULL_OVER, + steering_factor_direction, SteeringFactor::APPROACHING, ""); waitApproval(); return out; diff --git a/planning/behavior_path_planner/src/steering_factor_interface.cpp b/planning/behavior_path_planner/src/steering_factor_interface.cpp new file mode 100644 index 0000000000000..31c55af9b1603 --- /dev/null +++ b/planning/behavior_path_planner/src/steering_factor_interface.cpp @@ -0,0 +1,59 @@ +// Copyright 2022 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "behavior_path_planner/steering_factor_interface.hpp" + +namespace steering_factor_interface +{ +SteeringFactorInterface::SteeringFactorInterface(rclcpp::Node * node, const std::string & name) +: logger_{node->get_logger().get_child("PlanningAPI[" + name + "]")} +{ + // Publisher + pub_steering_factors_ = + node->create_publisher("/planning/steering_factor/" + name, 1); +} + +void SteeringFactorInterface::publishSteeringFactor(const rclcpp::Time & stamp) +{ + std::lock_guard lock(mutex_); + registered_steering_factors_.header.stamp = stamp; + pub_steering_factors_->publish(registered_steering_factors_); +} + +void SteeringFactorInterface::updateSteeringFactor( + const std::array & pose, const std::array distance, const uint16_t type, + const uint16_t direction, const uint16_t status, const std::string detail) +{ + std::lock_guard lock(mutex_); + SteeringFactor factor; + factor.pose = pose; + std::array converted_distance; + for (int i = 0; i < 2; ++i) converted_distance[i] = static_cast(distance[i]); + factor.distance = converted_distance; + factor.type = type; + factor.direction = direction; + factor.status = status; + factor.detail = detail; + registered_steering_factors_.factors = {factor}; +} + +void SteeringFactorInterface::clearSteeringFactors() +{ + std::lock_guard lock(mutex_); + registered_steering_factors_.factors.clear(); +} + +rclcpp::Logger SteeringFactorInterface::getLogger() const { return logger_; } + +} // namespace steering_factor_interface diff --git a/planning/behavior_path_planner/src/turn_signal_decider.cpp b/planning/behavior_path_planner/src/turn_signal_decider.cpp index 40dbf83f31b0a..9fbe0d5a5f997 100644 --- a/planning/behavior_path_planner/src/turn_signal_decider.cpp +++ b/planning/behavior_path_planner/src/turn_signal_decider.cpp @@ -80,6 +80,10 @@ std::pair TurnSignalDecider::getIntersectionTurnS if (distance_from_vehicle_front > intersection_search_distance_) { if (turn_signal.command == TurnIndicatorsCommand::DISABLE) { distance = std::numeric_limits::max(); + approaching_intersection_turn_signal_ = false; + } else { + intersection_distance_ = distance; + approaching_intersection_turn_signal_ = true; } return std::make_pair(turn_signal, distance); } @@ -112,12 +116,29 @@ std::pair TurnSignalDecider::getIntersectionTurnS turn_signal.command = TurnIndicatorsCommand::ENABLE_RIGHT; distance = distance_from_vehicle_front; } + intersection_pose_point_ = path_point.point.pose; } } } if (turn_signal.command == TurnIndicatorsCommand::DISABLE) { distance = std::numeric_limits::max(); + intersection_turn_signal_ = false; + approaching_intersection_turn_signal_ = false; + } else { + intersection_turn_signal_ = true; } + intersection_distance_ = distance; return std::make_pair(turn_signal, distance); } + +std::pair TurnSignalDecider::getIntersectionTurnSignalFlag() +{ + return std::make_pair(intersection_turn_signal_, approaching_intersection_turn_signal_); +} + +std::pair TurnSignalDecider::getIntersectionPoseAndDistance() +{ + return std::make_pair(intersection_pose_point_, intersection_distance_); +} + } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/src/utilities.cpp b/planning/behavior_path_planner/src/utilities.cpp index fb659bd9d70bd..1d8ef3d98fe07 100644 --- a/planning/behavior_path_planner/src/utilities.cpp +++ b/planning/behavior_path_planner/src/utilities.cpp @@ -775,10 +775,10 @@ PredictedObjects filterObjectsByLanelets( bool calcObjectPolygon(const PredictedObject & object, Polygon2d * object_polygon) { if (object.shape.type == Shape::BOUNDING_BOX) { - const double & len_x = object.shape.dimensions.x; - const double & len_y = object.shape.dimensions.y; + const double & length_m = object.shape.dimensions.x / 2; + const double & width_m = object.shape.dimensions.y / 2; *object_polygon = convertBoundingBoxObjectToGeometryPolygon( - object.kinematics.initial_pose_with_covariance.pose, len_x, len_y); + object.kinematics.initial_pose_with_covariance.pose, length_m, length_m, width_m); } else if (object.shape.type == Shape::CYLINDER) { *object_polygon = convertCylindricalObjectToGeometryPolygon( @@ -1965,7 +1965,7 @@ lanelet::ConstLanelets getCurrentLanes(const std::shared_ptr { const auto & route_handler = planner_data->route_handler; const auto current_pose = planner_data->self_pose->pose; - const auto common_parameters = planner_data->parameters; + const auto & common_parameters = planner_data->parameters; lanelet::ConstLanelet current_lane; if (!route_handler->getClosestLaneletWithinRoute(current_pose, ¤t_lane)) { @@ -2037,46 +2037,34 @@ lanelet::ConstLanelets calcLaneAroundPose( } Polygon2d convertBoundingBoxObjectToGeometryPolygon( - const Pose & current_pose, const double & length, const double & width) -{ - Polygon2d object_polygon; - - tf2::Transform tf_map2obj; - tf2::fromMsg(current_pose, tf_map2obj); + const Pose & current_pose, const double & base_to_front, const double & base_to_rear, + const double & base_to_width) +{ + const auto mapped_point = [](const double & length_scalar, const double & width_scalar) { + tf2::Vector3 map; + map.setX(length_scalar); + map.setY(width_scalar); + map.setZ(0.0); + map.setW(1.0); + return map; + }; // set vertices at map coordinate - tf2::Vector3 p1_map; - p1_map.setX(length / 2); - p1_map.setY(width / 2); - p1_map.setZ(0.0); - p1_map.setW(1.0); - - tf2::Vector3 p2_map; - p2_map.setX(-length / 2); - p2_map.setY(width / 2); - p2_map.setZ(0.0); - p2_map.setW(1.0); - - tf2::Vector3 p3_map; - p3_map.setX(-length / 2); - p3_map.setY(-width / 2); - p3_map.setZ(0.0); - p3_map.setW(1.0); - - tf2::Vector3 p4_map; - p4_map.setX(length / 2); - p4_map.setY(-width / 2); - p4_map.setZ(0.0); - p4_map.setW(1.0); + const tf2::Vector3 p1_map = std::invoke(mapped_point, base_to_front, -base_to_width); + const tf2::Vector3 p2_map = std::invoke(mapped_point, base_to_front, base_to_width); + const tf2::Vector3 p3_map = std::invoke(mapped_point, -base_to_rear, base_to_width); + const tf2::Vector3 p4_map = std::invoke(mapped_point, -base_to_rear, -base_to_width); // transform vertices from map coordinate to object coordinate - tf2::Vector3 p1_obj = tf_map2obj * p1_map; - tf2::Vector3 p2_obj = tf_map2obj * p2_map; - tf2::Vector3 p3_obj = tf_map2obj * p3_map; - tf2::Vector3 p4_obj = tf_map2obj * p4_map; + tf2::Transform tf_map2obj; + tf2::fromMsg(current_pose, tf_map2obj); + const tf2::Vector3 p1_obj = tf_map2obj * p1_map; + const tf2::Vector3 p2_obj = tf_map2obj * p2_map; + const tf2::Vector3 p3_obj = tf_map2obj * p3_map; + const tf2::Vector3 p4_obj = tf_map2obj * p4_map; + Polygon2d object_polygon; object_polygon.outer().reserve(5); - object_polygon.outer().emplace_back(p1_obj.x(), p1_obj.y()); object_polygon.outer().emplace_back(p2_obj.x(), p2_obj.y()); object_polygon.outer().emplace_back(p3_obj.x(), p3_obj.y()); @@ -2209,13 +2197,20 @@ void getProjectedDistancePointFromPolygons( bool getEgoExpectedPoseAndConvertToPolygon( const Pose & current_pose, const PredictedPath & pred_path, Pose & expected_pose, tier4_autoware_utils::Polygon2d & ego_polygon, const double & check_current_time, - const double & length, const double & width) + const VehicleInfo & ego_info) { if (!util::lerpByTimeStamp(pred_path, check_current_time, &expected_pose)) { return false; } expected_pose.orientation = current_pose.orientation; - ego_polygon = util::convertBoundingBoxObjectToGeometryPolygon(expected_pose, length, width); + + const auto & i = ego_info; + const auto & front_m = i.max_longitudinal_offset_m; + const auto & width_m = i.vehicle_width_m / 2.0; + const auto & back_m = i.rear_overhang_m; + + ego_polygon = + util::convertBoundingBoxObjectToGeometryPolygon(current_pose, front_m, back_m, width_m); return true; } @@ -2271,10 +2266,13 @@ Pose projectCurrentPoseToTarget(const Pose & desired_object, const Pose & target bool isObjectFront(const Pose & ego_pose, const Pose & obj_pose) { const auto obj_from_ego = projectCurrentPoseToTarget(ego_pose, obj_pose); - return obj_from_ego.position.x > 0; + return obj_from_ego.position.x > -1e-3; } -bool isObjectFront(const Pose & projected_ego_pose) { return projected_ego_pose.position.x > 0; } +bool isObjectFront(const Pose & projected_ego_pose) +{ + return projected_ego_pose.position.x > -1e-3; +} double stoppingDistance(const double & vehicle_velocity, const double & vehicle_accel) { @@ -2301,7 +2299,7 @@ double rearVehicleStopDistance( bool isLongitudinalDistanceEnough( const double & rear_vehicle_stop_threshold, const double & front_vehicle_stop_threshold) { - return rear_vehicle_stop_threshold < front_vehicle_stop_threshold; + return rear_vehicle_stop_threshold <= front_vehicle_stop_threshold; } bool hasEnoughDistance( @@ -2351,8 +2349,8 @@ bool isLateralDistanceEnough( bool isSafeInLaneletCollisionCheck( const Pose & ego_current_pose, const Twist & ego_current_twist, - const PredictedPath & ego_predicted_path, const double & ego_vehicle_length, - const double & ego_vehicle_width, const double & check_start_time, const double & check_end_time, + const PredictedPath & ego_predicted_path, const VehicleInfo & ego_info, + const double & check_start_time, const double & check_end_time, const double & check_time_resolution, const PredictedObject & target_object, const PredictedPath & target_object_path, const BehaviorPathPlannerParameters & common_parameters, CollisionCheckDebug & debug) @@ -2374,8 +2372,7 @@ bool isSafeInLaneletCollisionCheck( Pose expected_ego_pose; tier4_autoware_utils::Polygon2d ego_polygon; if (!util::getEgoExpectedPoseAndConvertToPolygon( - ego_current_pose, ego_predicted_path, expected_ego_pose, ego_polygon, t, - ego_vehicle_length, ego_vehicle_width)) { + ego_current_pose, ego_predicted_path, expected_ego_pose, ego_polygon, t, ego_info)) { debug.failed_reason = "get_ego_info_failed"; continue; } @@ -2407,8 +2404,8 @@ bool isSafeInLaneletCollisionCheck( bool isSafeInFreeSpaceCollisionCheck( const Pose & ego_current_pose, const Twist & ego_current_twist, - const PredictedPath & ego_predicted_path, const double & ego_vehicle_length, - const double & ego_vehicle_width, const double & check_start_time, const double & check_end_time, + const PredictedPath & ego_predicted_path, const VehicleInfo & ego_info, + const double & check_start_time, const double & check_end_time, const double & check_time_resolution, const PredictedObject & target_object, const BehaviorPathPlannerParameters & common_parameters, CollisionCheckDebug & debug) { @@ -2420,8 +2417,7 @@ bool isSafeInFreeSpaceCollisionCheck( Pose expected_ego_pose; tier4_autoware_utils::Polygon2d ego_polygon; if (!util::getEgoExpectedPoseAndConvertToPolygon( - ego_current_pose, ego_predicted_path, expected_ego_pose, ego_polygon, t, - ego_vehicle_length, ego_vehicle_width)) { + ego_current_pose, ego_predicted_path, expected_ego_pose, ego_polygon, t, ego_info)) { debug.failed_reason = "get_ego_info_failed"; continue; } diff --git a/planning/behavior_velocity_planner/config/intersection.param.yaml b/planning/behavior_velocity_planner/config/intersection.param.yaml index 0ea817ffefc81..79eb257de6889 100644 --- a/planning/behavior_velocity_planner/config/intersection.param.yaml +++ b/planning/behavior_velocity_planner/config/intersection.param.yaml @@ -14,6 +14,7 @@ detection_area_right_margin: 0.5 # [m] detection_area_left_margin: 0.5 # [m] detection_area_length: 200.0 # [m] + detection_area_angle_threshold: 0.785 # [rad] min_predicted_path_confidence: 0.05 collision_start_margin_time: 5.0 # [s] collision_end_margin_time: 2.0 # [s] diff --git a/planning/behavior_velocity_planner/docs/intersection/intersection.drawio.svg b/planning/behavior_velocity_planner/docs/intersection/intersection.drawio.svg index a5cc48a88bd42..e6bd97a3cdebb 100644 --- a/planning/behavior_velocity_planner/docs/intersection/intersection.drawio.svg +++ b/planning/behavior_velocity_planner/docs/intersection/intersection.drawio.svg @@ -1,4 +1,4 @@ -
Pass Judge Line
Pass Judge Line
Stop Line
Stop Line
Driving Lane
Driving Lane

Attention lane (Yellow)

Objects exist in this lane become the target object to judge if the ego vehicle can enter the intersection

Attention lane (Yellow)...

Stuck Vehicle

If there is a stuck vehicle on an exit place in the intersection, wait at the entrance of the intersection

Stuck Vehicle...

Front vehicle in a same lane

Objects exist in this lane become the target object to judge if the ego vehicle can enter the intersection

Front vehicle in a same lane...

Consider collisions with vehicles on the attention area

Consider collisions with vehicles on the at...
Behind Vehicle in a same lane
Behind Vehicle in a s...

stuck_vehicle_detect_dist_(5.0m)

Detection distance for the stuck vehicle

stuck_vehicle_detect_dist_(5.0m)...

Stop Line Margin

Distance between stop line and crossing lane  when stop line is generated.

Stop Line Margin...
Ignore None Attention Area Collision
Ignore None Attention Area Collis...Text is not SVG - cannot display \ No newline at end of file +
Pass Judge Line
Pass Judge Line
Stop Line
Stop Line
Driving Lane
Driving Lane

Attention lane (Yellow)

Objects exist in this lane become the target object to judge if the ego vehicle can enter the intersection

Attention lane (Yellow)...

Stuck Vehicle

If there is a stuck vehicle on an exit place in the intersection, wait at the entrance of the intersection

Stuck Vehicle...

Front vehicle in a same lane

Objects exist in this lane become the target object to judge if the ego vehicle can enter the intersection

Front vehicle in a same lane...

Consider collisions with vehicles on the attention area

Consider collisions with vehicles on the at...
Behind Vehicle in a same lane
Behind Vehicle in a sa...

stuck_vehicle_detect_dist_(5.0m)

Detection distance for the stuck vehicle

stuck_vehicle_detect_dist_(5.0m)...

Stop Line Margin

Distance between stop line and crossing lane  when stop line is generated.

Stop Line Margin...
Ignore None Attention Area Collision
Ignore None Attention Area Collis...

Intersection Area (Green)

Objects exist in this area is also the target for attention

Intersection Area (Green)...Text is not SVG - cannot display \ No newline at end of file diff --git a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp b/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp index 996e2d901d18c..16a24641ae3b7 100644 --- a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp @@ -53,12 +53,13 @@ class IntersectionModule : public SceneModuleInterface geometry_msgs::msg::Pose stop_point_pose; geometry_msgs::msg::Pose judge_point_pose; geometry_msgs::msg::Polygon ego_lane_polygon; - std::vector detection_area_with_margin; geometry_msgs::msg::Polygon stuck_vehicle_detect_area; geometry_msgs::msg::Polygon candidate_collision_ego_lane_polygon; std::vector candidate_collision_object_polygons; std::vector intersection_detection_lanelets; std::vector detection_area; + geometry_msgs::msg::Polygon intersection_area; + std::vector adjacent_area; autoware_auto_perception_msgs::msg::PredictedObjects conflicting_targets; autoware_auto_perception_msgs::msg::PredictedObjects stuck_targets; geometry_msgs::msg::Pose predicted_obj_pose; @@ -129,6 +130,7 @@ class IntersectionModule : public SceneModuleInterface * @param path ego-car lane * @param detection_area_lanelet_ids angle check is performed for obstacles using this lanelet * ids + * @param intersection_area associated intersection_area if exists * @param objects_ptr target objects * @param closest_idx ego-car position index on the lane * @return true if collision is detected @@ -137,6 +139,8 @@ class IntersectionModule : public SceneModuleInterface lanelet::LaneletMapConstPtr lanelet_map_ptr, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const std::vector & detection_area_lanelet_ids, + const std::vector & adjacent_lanelet_ids, + const std::optional & intersection_area, const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr objects_ptr, const int closest_idx, const Polygon2d & stuck_vehicle_detect_area); @@ -218,7 +222,8 @@ class IntersectionModule : public SceneModuleInterface * @return true if the given pose belongs to any target lanelet */ bool checkAngleForTargetLanelets( - const geometry_msgs::msg::Pose & pose, const std::vector & target_lanelet_ids); + const geometry_msgs::msg::Pose & pose, const std::vector & target_lanelet_ids, + const double margin = 0); /** * @brief Get lanes including ego lanelet and next lanelet diff --git a/planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp b/planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp index 94a5d2fac2095..1992adbf34c80 100644 --- a/planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp @@ -27,6 +27,7 @@ #endif #include +#include #include #include @@ -48,11 +49,9 @@ bool getDuplicatedPointIdx( */ bool getObjectiveLanelets( lanelet::LaneletMapConstPtr lanelet_map_ptr, lanelet::routing::RoutingGraphPtr routing_graph_ptr, - const int lane_id, const double detection_area_length, const double right_margin, - const double left_margin, std::vector * conflicting_lanelets_result, - lanelet::ConstLanelets * objective_lanelets_result, - std::vector * objective_lanelets_with_margin_result, - const rclcpp::Logger logger); + const int lane_id, const double detection_area_length, + std::vector * conflicting_lanelets_result, + lanelet::ConstLanelets * objective_lanelets_result, const rclcpp::Logger logger); struct StopLineIdx { @@ -143,6 +142,13 @@ bool isBeforeTargetIndex( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int closest_idx, const geometry_msgs::msg::Pose & current_pose, const int target_idx); +std::vector extendedAdjacentDirectionLanes( + const lanelet::LaneletMapPtr map, const lanelet::routing::RoutingGraphPtr routing_graph, + lanelet::ConstLanelet lane); + +std::optional getIntersectionArea( + lanelet::ConstLanelet assigned_lane, lanelet::LaneletMapConstPtr lanelet_map_ptr); + } // namespace util } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/intersection-design.md b/planning/behavior_velocity_planner/intersection-design.md index f499a1ed194eb..3959a5ebfd83d 100644 --- a/planning/behavior_velocity_planner/intersection-design.md +++ b/planning/behavior_velocity_planner/intersection-design.md @@ -25,11 +25,12 @@ Objects that satisfy all of the following conditions are considered as target ob - The type of object type is **car**, **truck**, **bus** or **motorbike**. (Bicycle, pedestrian, animal, unknown are not.) - The center of gravity of object is **located within a certain distance** from the attention lane (threshold = `detection_area_margin`) . - - In the past, the decision was made based on whether any points of the object's polygon exists in the attention lane, but since there were many false positives, the logic has changed to the current one. + - (Optional condition) The center of gravity is in the **intersection area**. + - To deal with objects that is in the area not covered by the lanelets in the intersection. - The posture of object is **the same direction as the attention lane** (threshold = `detection_area_angle_threshold`). - The orientation of the target is recalculated in this module according to the `orientation_reliable` and the sign of the velocity of the target. -- Not being **in the same lane as the ego vehicle**. - - To avoid judging the vehicle ahead as a collision target. This logic needs to be improved. +- Not being **in the neighbouring lanes of the ego vehicle**. + - neighbouring lanes include the ego lane of the vehicle and the adjacent lanes of it with turn_direction as the ego lane. #### How to Define Attention Lanes diff --git a/planning/behavior_velocity_planner/package.xml b/planning/behavior_velocity_planner/package.xml index 70d85b2e5d340..035835d2e2ed0 100644 --- a/planning/behavior_velocity_planner/package.xml +++ b/planning/behavior_velocity_planner/package.xml @@ -26,6 +26,10 @@ Tomohito Ando + + Tomoya Kimura + Shumpei Wakabayashi + Apache License 2.0 Taiki Tanaka diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/debug.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/debug.cpp index 94364e928bbda..c7bbb10395af8 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/debug.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/debug.cpp @@ -30,14 +30,14 @@ using tier4_autoware_utils::createMarkerColor; using tier4_autoware_utils::createMarkerOrientation; using tier4_autoware_utils::createMarkerScale; -visualization_msgs::msg::MarkerArray createLaneletPolygonsMarkerArray( +static visualization_msgs::msg::MarkerArray createLaneletPolygonsMarkerArray( const std::vector & polygons, const std::string & ns, - const int64_t module_id) + const int64_t lane_id, const double r, const double g, const double b) { visualization_msgs::msg::MarkerArray msg; int32_t i = 0; - int32_t uid = planning_utils::bitShift(module_id); + int32_t uid = planning_utils::bitShift(lane_id); for (const auto & polygon : polygons) { visualization_msgs::msg::Marker marker{}; marker.header.frame_id = "map"; @@ -49,7 +49,7 @@ visualization_msgs::msg::MarkerArray createLaneletPolygonsMarkerArray( marker.action = visualization_msgs::msg::Marker::ADD; marker.pose.orientation = createMarkerOrientation(0, 0, 0, 1.0); marker.scale = createMarkerScale(0.1, 0.0, 0.0); - marker.color = createMarkerColor(0.0, 1.0, 0.0, 0.999); + marker.color = createMarkerColor(r, g, b, 0.999); for (const auto & p : polygon) { geometry_msgs::msg::Point point; point.x = p.x(); @@ -118,22 +118,29 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( &debug_marker_array, now); appendMarkerArray( - createLaneletPolygonsMarkerArray(debug_data_.detection_area, "detection_area", module_id_), - &debug_marker_array, now); + createLaneletPolygonsMarkerArray( + debug_data_.detection_area, "detection_area", lane_id_, 0.0, 1.0, 0.0), + &debug_marker_array); appendMarkerArray( createLaneletPolygonsMarkerArray( - debug_data_.detection_area_with_margin, "detection_area_with_margin", module_id_), - &debug_marker_array, now); + debug_data_.adjacent_area, "adjacent_area", lane_id_, 0.913, 0.639, 0.149), + &debug_marker_array); appendMarkerArray( debug::createPolygonMarkerArray( - debug_data_.ego_lane_polygon, "ego_lane", module_id_, now, 0.3, 0.0, 0.0, 0.0, 0.3, 0.7), - &debug_marker_array, now); + debug_data_.intersection_area, "intersection_area", lane_id_, now, 0.3, 0.0, 0.0, 0.0, 1.0, + 0.0), + &debug_marker_array); + + appendMarkerArray( + debug::createPolygonMarkerArray( + debug_data_.ego_lane_polygon, "ego_lane", lane_id_, now, 0.3, 0.0, 0.0, 0.0, 0.3, 0.7), + &debug_marker_array); appendMarkerArray( debug::createPolygonMarkerArray( - debug_data_.stuck_vehicle_detect_area, "stuck_vehicle_detect_area", module_id_, now, 0.3, 0.0, + debug_data_.stuck_vehicle_detect_area, "stuck_vehicle_detect_area", lane_id_, now, 0.3, 0.0, 0.0, 0.0, 0.5, 0.5), &debug_marker_array, now); @@ -147,7 +154,7 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( for (const auto & p : debug_data_.candidate_collision_object_polygons) { appendMarkerArray( debug::createPolygonMarkerArray( - p, "candidate_collision_object_polygons", module_id_ + i++, now, 0.3, 0.0, 0.0, 0.0, 0.5, + p, "candidate_collision_object_polygons", lane_id_ + i++, now, 0.3, 0.0, 0.0, 0.0, 0.5, 0.5), &debug_marker_array, now); } diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp index 2ff0ac7e8c848..d3bd968842adb 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp @@ -94,20 +94,14 @@ bool IntersectionModule::modifyPathVelocity( /* get detection area and conflicting area */ lanelet::ConstLanelets detection_area_lanelets; std::vector conflicting_area_lanelets; - std::vector detection_area_lanelets_with_margin; util::getObjectiveLanelets( lanelet_map_ptr, routing_graph_ptr, lane_id_, planner_param_.detection_area_length, - planner_param_.detection_area_right_margin, planner_param_.detection_area_left_margin, - &conflicting_area_lanelets, &detection_area_lanelets, &detection_area_lanelets_with_margin, - logger_); + &conflicting_area_lanelets, &detection_area_lanelets, logger_); std::vector conflicting_areas = util::getPolygon3dFromLaneletsVec( conflicting_area_lanelets, planner_param_.detection_area_length); std::vector detection_areas = util::getPolygon3dFromLanelets(detection_area_lanelets, planner_param_.detection_area_length); - std::vector detection_areas_with_margin = - util::getPolygon3dFromLaneletsVec( - detection_area_lanelets_with_margin, planner_param_.detection_area_length); std::vector conflicting_area_lanelet_ids = util::getLaneletIdsFromLaneletsVec(conflicting_area_lanelets); std::vector detection_area_lanelet_ids = @@ -120,7 +114,24 @@ bool IntersectionModule::modifyPathVelocity( return true; } debug_data_.detection_area = detection_areas; - debug_data_.detection_area_with_margin = detection_areas_with_margin; + + /* get intersection area */ + const auto & assigned_lanelet = + planner_data_->route_handler_->getLaneletMapPtr()->laneletLayer.get(lane_id_); + const auto intersection_area = util::getIntersectionArea(assigned_lanelet, lanelet_map_ptr); + if (intersection_area) { + const auto intersection_area_2d = intersection_area.value(); + debug_data_.intersection_area = toGeomPoly(intersection_area_2d); + } + + /* get adjacent lanelets */ + const auto adjacent_lanelet_ids = + util::extendedAdjacentDirectionLanes(lanelet_map_ptr, routing_graph_ptr, assigned_lanelet); + std::vector adjacent_lanes; + for (auto && adjacent_lanelet_id : adjacent_lanelet_ids) { + adjacent_lanes.push_back(lanelet_map_ptr->laneletLayer.get(adjacent_lanelet_id).polygon3d()); + } + debug_data_.adjacent_area = adjacent_lanes; /* set stop-line and stop-judgement-line for base_link */ util::StopLineIdx stop_line_idxs; @@ -198,8 +209,8 @@ bool IntersectionModule::modifyPathVelocity( planner_param_.stuck_vehicle_detect_dist); bool is_stuck = checkStuckVehicleInIntersection(objects_ptr, stuck_vehicle_detect_area); bool has_collision = checkCollision( - lanelet_map_ptr, *path, detection_area_lanelet_ids, objects_ptr, closest_idx, - stuck_vehicle_detect_area); + lanelet_map_ptr, *path, detection_area_lanelet_ids, adjacent_lanelet_ids, intersection_area, + objects_ptr, closest_idx, stuck_vehicle_detect_area); bool is_entry_prohibited = (has_collision || is_stuck); if (external_go) { is_entry_prohibited = false; @@ -284,6 +295,7 @@ bool IntersectionModule::checkCollision( lanelet::LaneletMapConstPtr lanelet_map_ptr, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const std::vector & detection_area_lanelet_ids, + const std::vector & adjacent_lanelet_ids, const std::optional & intersection_area, const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr objects_ptr, const int closest_idx, const Polygon2d & stuck_vehicle_detect_area) { @@ -326,7 +338,24 @@ bool IntersectionModule::checkCollision( // check direction of objects const auto object_direction = getObjectPoseWithVelocityDirection(object.kinematics); - if (checkAngleForTargetLanelets(object_direction, detection_area_lanelet_ids)) { + if (intersection_area) { + const auto obj_poly = tier4_autoware_utils::toPolygon2d(object); + const auto intersection_area_2d = intersection_area.value(); + const auto is_in_intersection_area = bg::within(obj_poly, intersection_area_2d); + const auto is_in_adjacent_lanelets = checkAngleForTargetLanelets( + object_direction, adjacent_lanelet_ids, planner_param_.detection_area_margin); + if (is_in_adjacent_lanelets) continue; + if (is_in_intersection_area) { + target_objects.objects.push_back(object); + } else if (checkAngleForTargetLanelets( + object_direction, detection_area_lanelet_ids, + planner_param_.detection_area_margin)) { + target_objects.objects.push_back(object); + } + } else if (checkAngleForTargetLanelets( + object_direction, detection_area_lanelet_ids, + planner_param_.detection_area_margin)) { + // intersection_area is not available, use detection_area_with_margin as before target_objects.objects.push_back(object); } } @@ -610,11 +639,12 @@ bool IntersectionModule::isTargetExternalInputStatus(const int target_status) } bool IntersectionModule::checkAngleForTargetLanelets( - const geometry_msgs::msg::Pose & pose, const std::vector & target_lanelet_ids) + const geometry_msgs::msg::Pose & pose, const std::vector & target_lanelet_ids, + const double margin) { for (const int lanelet_id : target_lanelet_ids) { const auto ll = planner_data_->route_handler_->getLaneletMapPtr()->laneletLayer.get(lanelet_id); - if (!lanelet::utils::isInLanelet(pose, ll, planner_param_.detection_area_margin)) { + if (!lanelet::utils::isInLanelet(pose, ll, margin)) { continue; } const double ll_angle = lanelet::utils::getLaneletAngle(ll, pose.position); diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp index 6e3d3169a6d8d..61f587396d444 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp @@ -68,13 +68,10 @@ bool MergeFromPrivateRoadModule::modifyPathVelocity( /* get detection area */ lanelet::ConstLanelets detection_area_lanelets; std::vector conflicting_area_lanelets; - std::vector detection_area_lanelets_with_margin; util::getObjectiveLanelets( lanelet_map_ptr, routing_graph_ptr, lane_id_, planner_param_.detection_area_length, - planner_param_.detection_area_right_margin, planner_param_.detection_area_left_margin, - &conflicting_area_lanelets, &detection_area_lanelets, &detection_area_lanelets_with_margin, - logger_); + &conflicting_area_lanelets, &detection_area_lanelets, logger_); std::vector conflicting_areas = util::getPolygon3dFromLaneletsVec( conflicting_area_lanelets, planner_param_.detection_area_length); if (conflicting_areas.empty()) { diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp index 2acd007f7e759..2ea4c3b870b87 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp @@ -26,6 +26,7 @@ #include #include +#include #include #include @@ -310,11 +311,9 @@ bool getStopPoseIndexFromMap( bool getObjectiveLanelets( lanelet::LaneletMapConstPtr lanelet_map_ptr, lanelet::routing::RoutingGraphPtr routing_graph_ptr, - const int lane_id, const double detection_area_length, double right_margin, double left_margin, + const int lane_id, const double detection_area_length, std::vector * conflicting_lanelets_result, - lanelet::ConstLanelets * objective_lanelets_result, - std::vector * objective_lanelets_with_margin_result, - const rclcpp::Logger logger) + lanelet::ConstLanelets * objective_lanelets_result, const rclcpp::Logger logger) { const auto & assigned_lanelet = lanelet_map_ptr->laneletLayer.get(lane_id); @@ -355,7 +354,6 @@ bool getObjectiveLanelets( std::vector // conflicting lanes with "lane_id" conflicting_lanelets_ex_yield_ego; // excluding ego lanes and yield lanes std::vector objective_lanelets; // final objective lanelets - std::vector objective_lanelets_with_margin; // final objective lanelets // exclude yield lanelets and ego lanelets from objective_lanelets for (const auto & conflicting_lanelet : conflicting_lanelets) { @@ -365,11 +363,8 @@ bool getObjectiveLanelets( if (lanelet::utils::contains(ego_lanelets, conflicting_lanelet)) { continue; } - const auto objective_lanelet_with_margin = - generateOffsetLanelet(conflicting_lanelet, right_margin, left_margin); conflicting_lanelets_ex_yield_ego.push_back({conflicting_lanelet}); objective_lanelets.push_back({conflicting_lanelet}); - objective_lanelets_with_margin.push_back({objective_lanelet_with_margin}); } // get possible lanelet path that reaches conflicting_lane longer than given length @@ -396,7 +391,6 @@ bool getObjectiveLanelets( *conflicting_lanelets_result = conflicting_lanelets_ex_yield_ego; *objective_lanelets_result = objective_and_preceding_lanelets; - *objective_lanelets_with_margin_result = objective_lanelets_with_margin; // set this flag true when debugging const bool is_debug = false; @@ -582,6 +576,11 @@ geometry_msgs::msg::Pose toPose(const geometry_msgs::msg::Point & p) return pose; } +static std::string getTurnDirection(lanelet::ConstLanelet lane) +{ + return lane.attributeOr("turn_direction", "else"); +} + bool isOverTargetIndex( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int closest_idx, const geometry_msgs::msg::Pose & current_pose, const int target_idx) @@ -604,5 +603,78 @@ bool isBeforeTargetIndex( return static_cast(target_idx > closest_idx); } +static std::vector getAllAdjacentLanelets( + const lanelet::routing::RoutingGraphPtr routing_graph, lanelet::ConstLanelet lane) +{ + std::set results; + + results.insert(lane.id()); + + auto it = routing_graph->adjacentRight(lane); + // take all lane on the right side + while (!!it) { + results.insert(it.get().id()); + it = routing_graph->adjacentRight(it.get()); + } + // take all lane on the left side + it = routing_graph->adjacentLeft(lane); + while (!!it) { + results.insert(it.get().id()); + it = routing_graph->adjacentLeft(it.get()); + } + + return std::vector(results.begin(), results.end()); +} + +std::vector extendedAdjacentDirectionLanes( + const lanelet::LaneletMapPtr map, const lanelet::routing::RoutingGraphPtr routing_graph, + lanelet::ConstLanelet lane) +{ + // some of the intersections are not well-formed, and "adjacent" turning + // lanelets are not sharing the LineStrings + const std::string turn_direction = getTurnDirection(lane); + if (turn_direction != "left" && turn_direction != "right" && turn_direction != "straight") + return std::vector(); + + std::set previous_lanelet_ids; + for (auto && previous_lanelet : routing_graph->previous(lane)) { + previous_lanelet_ids.insert(previous_lanelet.id()); + } + + std::set besides_previous_lanelet_ids; + for (auto && previous_lanelet_id : previous_lanelet_ids) { + lanelet::ConstLanelet previous_lanelet = map->laneletLayer.get(previous_lanelet_id); + for (auto && beside_lanelet : getAllAdjacentLanelets(routing_graph, previous_lanelet)) { + besides_previous_lanelet_ids.insert(beside_lanelet); + } + } + + std::set following_turning_lanelets; + following_turning_lanelets.insert(lane.id()); + for (auto && besides_previous_lanelet_id : besides_previous_lanelet_ids) { + lanelet::ConstLanelet besides_previous_lanelet = + map->laneletLayer.get(besides_previous_lanelet_id); + for (auto && following_lanelet : routing_graph->following(besides_previous_lanelet)) { + // if this has {"turn_direction", "${turn_direction}"}, take this + if (getTurnDirection(following_lanelet) == turn_direction) + following_turning_lanelets.insert(following_lanelet.id()); + } + } + return std::vector(following_turning_lanelets.begin(), following_turning_lanelets.end()); +} + +std::optional getIntersectionArea( + lanelet::ConstLanelet assigned_lane, lanelet::LaneletMapConstPtr lanelet_map_ptr) +{ + const std::string area_id_str = assigned_lane.attributeOr("intersection_area", "else"); + if (area_id_str == "else") return std::nullopt; + + const int area_id = std::atoi(area_id_str.c_str()); + const auto poly_3d = lanelet_map_ptr->polygonLayer.get(area_id); + Polygon2d poly{}; + for (const auto & p : poly_3d) poly.outer().emplace_back(p.x(), p.y()); + return std::make_optional(poly); +} + } // namespace util } // namespace behavior_velocity_planner diff --git a/planning/obstacle_avoidance_planner/src/node.cpp b/planning/obstacle_avoidance_planner/src/node.cpp index 5a5c6fe4f23b0..424ddab56849a 100644 --- a/planning/obstacle_avoidance_planner/src/node.cpp +++ b/planning/obstacle_avoidance_planner/src/node.cpp @@ -1141,6 +1141,19 @@ Trajectories ObstacleAvoidancePlanner::optimizeTrajectory( return getPrevTrajs(p.path.points); } + // NOTE: Elastic band sometimes diverges with status = "OSQP_SOLVED". + constexpr double max_path_change_diff = 1.0e4; + for (size_t i = 0; i < eb_traj->size(); ++i) { + const auto & eb_pos = eb_traj->at(i).pose.position; + const auto & path_pos = p.path.points.at(std::min(i, p.path.points.size() - 1)).pose.position; + + const double diff_x = eb_pos.x - path_pos.x; + const double diff_y = eb_pos.y - path_pos.y; + if (max_path_change_diff < std::abs(diff_x) || max_path_change_diff < std::abs(diff_y)) { + return getPrevTrajs(p.path.points); + } + } + // EB has to be solved twice before solving MPT with fixed points // since the result of EB is likely to change with/without fixing (1st/2nd EB) // that makes MPT fixing points worse. diff --git a/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml b/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml index 240ea8def4db8..f8f9a85deed5c 100644 --- a/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml +++ b/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml @@ -11,8 +11,6 @@ min_object_accel_for_rss: -1.0 # front obstacle's acceleration to calculate RSS distance [m/ss] safe_distance_margin : 6.0 # This is also used as a stop margin [m] - lpf_gain_for_accel: 0.2 # gain of low pass filter for ego acceleration [-] - nearest_dist_deviation_threshold: 3.0 # [m] for finding nearest index nearest_yaw_deviation_threshold: 1.57 # [rad] for finding nearest index min_behavior_stop_margin: 3.0 # [m] diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp index e47d99f6520aa..7570b1ad12274 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp @@ -26,6 +26,8 @@ #include "autoware_auto_perception_msgs/msg/predicted_object.hpp" #include "autoware_auto_perception_msgs/msg/predicted_objects.hpp" #include "autoware_auto_planning_msgs/msg/trajectory.hpp" +#include "geometry_msgs/msg/accel_stamped.hpp" +#include "geometry_msgs/msg/accel_with_covariance_stamped.hpp" #include "geometry_msgs/msg/point_stamped.hpp" #include "geometry_msgs/msg/transform_stamped.hpp" #include "geometry_msgs/msg/twist_stamped.hpp" @@ -48,6 +50,8 @@ using autoware_auto_perception_msgs::msg::PredictedObjects; using autoware_auto_perception_msgs::msg::PredictedPath; using autoware_auto_planning_msgs::msg::Trajectory; using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using geometry_msgs::msg::AccelStamped; +using geometry_msgs::msg::AccelWithCovarianceStamped; using nav_msgs::msg::Odometry; using tier4_debug_msgs::msg::Float32Stamped; using tier4_planning_msgs::msg::StopReasonArray; @@ -68,6 +72,7 @@ class ObstacleCruisePlannerNode : public rclcpp::Node const std::vector & parameters); void onObjects(const PredictedObjects::ConstSharedPtr msg); void onOdometry(const Odometry::ConstSharedPtr); + void onAccel(const AccelWithCovarianceStamped::ConstSharedPtr); void onTrajectory(const Trajectory::ConstSharedPtr msg); void onSmoothedTrajectory(const Trajectory::ConstSharedPtr msg); @@ -78,7 +83,6 @@ class ObstacleCruisePlannerNode : public rclcpp::Node ObstacleCruisePlannerData createStopData( const Trajectory & trajectory, const geometry_msgs::msg::Pose & current_pose, const std::vector & obstacles, const bool is_driving_forward); - double calcCurrentAccel() const; std::vector getTargetObstacles( const Trajectory & trajectory, const geometry_msgs::msg::Pose & current_pose, const double current_vel, const bool is_driving_forward, DebugData & debug_data); @@ -101,6 +105,8 @@ class ObstacleCruisePlannerNode : public rclcpp::Node bool isCruiseObstacle(const uint8_t label); bool isStopObstacle(const uint8_t label); + bool isFrontCollideObstacle( + const Trajectory & traj, const PredictedObject & object, const size_t first_collision_idx); bool is_showing_debug_info_; double min_behavior_stop_margin_; @@ -129,6 +135,7 @@ class ObstacleCruisePlannerNode : public rclcpp::Node rclcpp::Subscription::SharedPtr smoothed_trajectory_sub_; rclcpp::Subscription::SharedPtr objects_sub_; rclcpp::Subscription::SharedPtr odom_sub_; + rclcpp::Subscription::SharedPtr acc_sub_; // self pose listener tier4_autoware_utils::SelfPoseListener self_pose_listener_; @@ -136,10 +143,8 @@ class ObstacleCruisePlannerNode : public rclcpp::Node // data for callback functions PredictedObjects::ConstSharedPtr in_objects_ptr_; geometry_msgs::msg::TwistStamped::SharedPtr current_twist_ptr_; - geometry_msgs::msg::TwistStamped::SharedPtr prev_twist_ptr_; - // low pass filter of acceleration - std::shared_ptr lpf_acc_ptr_; + geometry_msgs::msg::AccelStamped::SharedPtr current_accel_ptr_; // Vehicle Parameters VehicleInfo vehicle_info_; diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/polygon_utils.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/polygon_utils.hpp index e4b38b490d3f3..651c63d4593a5 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/polygon_utils.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/polygon_utils.hpp @@ -48,7 +48,7 @@ std::vector getCollisionPoints( const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, const autoware_auto_perception_msgs::msg::Shape & shape, const rclcpp::Time & current_time, const double vehicle_max_longitudinal_offset, const bool is_driving_forward, - const double max_dist = std::numeric_limits::max(), + std::vector & collision_index, const double max_dist = std::numeric_limits::max(), const double max_prediction_time_for_collision_check = std::numeric_limits::max()); std::vector willCollideWithSurroundObstacle( @@ -57,7 +57,7 @@ std::vector willCollideWithSurroundObstacle( const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, const autoware_auto_perception_msgs::msg::Shape & shape, const rclcpp::Time & current_time, const double max_dist, const double ego_obstacle_overlap_time_threshold, - const double max_prediction_time_for_collision_check, + const double max_prediction_time_for_collision_check, std::vector & collision_index, const double vehicle_max_longitudinal_offset, const bool is_driving_forward); std::vector createOneStepPolygons( diff --git a/planning/obstacle_cruise_planner/src/node.cpp b/planning/obstacle_cruise_planner/src/node.cpp index 790e4897cf94c..0b970f35d2069 100644 --- a/planning/obstacle_cruise_planner/src/node.cpp +++ b/planning/obstacle_cruise_planner/src/node.cpp @@ -62,12 +62,13 @@ Trajectory trimTrajectoryFrom(const Trajectory & input, const double nearest_idx } bool isFrontObstacle( - const Trajectory & traj, const size_t ego_idx, const geometry_msgs::msg::Point & obj_pos) + const std::vector & traj_points, const size_t ego_idx, + const geometry_msgs::msg::Point & obj_pos) { - size_t obj_idx = motion_utils::findNearestSegmentIndex(traj.points, obj_pos); + size_t obj_idx = motion_utils::findNearestSegmentIndex(traj_points, obj_pos); const double ego_to_obj_distance = - motion_utils::calcSignedArcLength(traj.points, ego_idx, obj_idx); + motion_utils::calcSignedArcLength(traj_points, ego_idx, obj_idx); if (ego_to_obj_distance < 0) { return false; @@ -195,7 +196,6 @@ ObstacleCruisePlannerNode::ObstacleCruisePlannerNode(const rclcpp::NodeOptions & : Node("obstacle_cruise_planner", node_options), self_pose_listener_(this), in_objects_ptr_(nullptr), - lpf_acc_ptr_(nullptr), vehicle_info_(vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo()) { using std::placeholders::_1; @@ -212,6 +212,9 @@ ObstacleCruisePlannerNode::ObstacleCruisePlannerNode(const rclcpp::NodeOptions & odom_sub_ = create_subscription( "~/input/odometry", rclcpp::QoS{1}, std::bind(&ObstacleCruisePlannerNode::onOdometry, this, std::placeholders::_1)); + acc_sub_ = create_subscription( + "~/input/acceleration", rclcpp::QoS{1}, + std::bind(&ObstacleCruisePlannerNode::onAccel, this, std::placeholders::_1)); // publisher trajectory_pub_ = create_publisher("~/output/trajectory", 1); @@ -260,10 +263,6 @@ ObstacleCruisePlannerNode::ObstacleCruisePlannerNode(const rclcpp::NodeOptions & is_showing_debug_info_ = declare_parameter("common.is_showing_debug_info"); - // low pass filter for ego acceleration - const double lpf_gain_for_accel = declare_parameter("common.lpf_gain_for_accel"); - lpf_acc_ptr_ = std::make_shared(lpf_gain_for_accel); - { // Obstacle filtering parameters obstacle_filtering_param_.rough_detection_area_expand_width = declare_parameter("obstacle_filtering.rough_detection_area_expand_width"); @@ -502,15 +501,18 @@ void ObstacleCruisePlannerNode::onObjects(const PredictedObjects::ConstSharedPtr void ObstacleCruisePlannerNode::onOdometry(const Odometry::ConstSharedPtr msg) { - if (current_twist_ptr_) { - prev_twist_ptr_ = current_twist_ptr_; - } - current_twist_ptr_ = std::make_unique(); current_twist_ptr_->header = msg->header; current_twist_ptr_->twist = msg->twist.twist; } +void ObstacleCruisePlannerNode::onAccel(const AccelWithCovarianceStamped::ConstSharedPtr msg) +{ + current_accel_ptr_ = std::make_unique(); + current_accel_ptr_->header = msg->header; + current_accel_ptr_->accel = msg->accel.accel; +} + void ObstacleCruisePlannerNode::onSmoothedTrajectory(const Trajectory::ConstSharedPtr msg) { planner_ptr_->setSmoothedTrajectory(msg); @@ -521,9 +523,7 @@ void ObstacleCruisePlannerNode::onTrajectory(const Trajectory::ConstSharedPtr ms const auto current_pose_ptr = self_pose_listener_.getCurrentPose(); // check if subscribed variables are ready - if ( - msg->points.empty() || !current_twist_ptr_ || !prev_twist_ptr_ || !in_objects_ptr_ || - !current_pose_ptr) { + if (msg->points.empty() || !current_twist_ptr_ || !in_objects_ptr_ || !current_pose_ptr) { return; } @@ -588,7 +588,7 @@ ObstacleCruisePlannerData ObstacleCruisePlannerNode::createStopData( { const auto current_time = now(); const double current_vel = current_twist_ptr_->twist.linear.x; - const double current_accel = calcCurrentAccel(); + const double current_accel = current_accel_ptr_->accel.linear.x; // create planner_stop data ObstacleCruisePlannerData planner_data; @@ -608,13 +608,27 @@ ObstacleCruisePlannerData ObstacleCruisePlannerNode::createStopData( return planner_data; } +bool ObstacleCruisePlannerNode::isFrontCollideObstacle( + const Trajectory & traj, const PredictedObject & object, const size_t first_collision_idx) +{ + const auto object_pose = object.kinematics.initial_pose_with_covariance.pose; + const auto obj_idx = motion_utils::findNearestIndex(traj.points, object_pose.position); + + const double obj_to_col_points_distance = + motion_utils::calcSignedArcLength(traj.points, obj_idx, first_collision_idx); + const double obj_max_length = calcObjectMaxLength(object.shape); + + // If the object is far in front of the collision point, the object is behind the ego. + return obj_to_col_points_distance > -obj_max_length; +} + ObstacleCruisePlannerData ObstacleCruisePlannerNode::createCruiseData( const Trajectory & trajectory, const geometry_msgs::msg::Pose & current_pose, const std::vector & obstacles, const bool is_driving_forward) { const auto current_time = now(); const double current_vel = current_twist_ptr_->twist.linear.x; - const double current_accel = calcCurrentAccel(); + const double current_accel = current_accel_ptr_->accel.linear.x; // create planner_stop data ObstacleCruisePlannerData planner_data; @@ -633,19 +647,6 @@ ObstacleCruisePlannerData ObstacleCruisePlannerNode::createCruiseData( return planner_data; } -double ObstacleCruisePlannerNode::calcCurrentAccel() const -{ - const double diff_vel = current_twist_ptr_->twist.linear.x - prev_twist_ptr_->twist.linear.x; - const double diff_time = std::max( - (rclcpp::Time(current_twist_ptr_->header.stamp) - rclcpp::Time(prev_twist_ptr_->header.stamp)) - .seconds(), - 1e-03); - - const double accel = diff_vel / diff_time; - - return lpf_acc_ptr_->filter(accel); -} - std::vector ObstacleCruisePlannerNode::getTargetObstacles( const Trajectory & trajectory, const geometry_msgs::msg::Pose & current_pose, const double current_vel, const bool is_driving_forward, DebugData & debug_data) @@ -710,8 +711,9 @@ std::vector ObstacleCruisePlannerNode::filterObstacles( const auto & object_velocity = predicted_object.kinematics.initial_twist_with_covariance.twist.linear.x; - const bool is_front_obstacle = - isFrontObstacle(traj, ego_idx, current_object_pose.pose.position); + const bool is_front_obstacle = isFrontObstacle( + motion_utils::convertToTrajectoryPointArray(traj), ego_idx, + current_object_pose.pose.position); if (!is_front_obstacle) { RCLCPP_INFO_EXPRESSION( get_logger(), is_showing_debug_info_, @@ -749,12 +751,13 @@ std::vector ObstacleCruisePlannerNode::filterObstacles( // precise detection area filtering with polygons std::vector collision_points; + std::vector collision_index; if (first_within_idx) { // obstacles inside the trajectory // calculate nearest collision point collision_points = polygon_utils::getCollisionPoints( extended_traj, extended_traj_polygons, predicted_objects.header, resampled_predicted_path, predicted_object.shape, current_time, vehicle_info_.max_longitudinal_offset_m, - is_driving_forward); + is_driving_forward, collision_index); const bool is_angle_aligned = isAngleAlignedWithTrajectory( extended_traj, current_object_pose.pose, @@ -809,12 +812,13 @@ std::vector ObstacleCruisePlannerNode::filterObstacles( continue; } + std::vector collision_index; collision_points = polygon_utils::willCollideWithSurroundObstacle( extended_traj, extended_traj_polygons, predicted_objects.header, resampled_predicted_path, predicted_object.shape, current_time, vehicle_info_.vehicle_width_m + obstacle_filtering_param_.rough_detection_area_expand_width, obstacle_filtering_param_.ego_obstacle_overlap_time_threshold, - obstacle_filtering_param_.max_prediction_time_for_collision_check, + obstacle_filtering_param_.max_prediction_time_for_collision_check, collision_index, vehicle_info_.max_longitudinal_offset_m, is_driving_forward); if (collision_points.empty()) { @@ -827,6 +831,14 @@ std::vector ObstacleCruisePlannerNode::filterObstacles( debug_data.intentionally_ignored_obstacles.push_back(predicted_object); continue; } + + // Ignore obstacles behind the ego vehicle. + // Note: Only using isFrontObstacle(), behind obstacles cannot be filtered + // properly when the trajectory is crossing or overlapping. + const size_t first_collision_index = collision_index.front(); + if (!isFrontCollideObstacle(extended_traj, predicted_object, first_collision_index)) { + continue; + } } // For debug diff --git a/planning/obstacle_cruise_planner/src/polygon_utils.cpp b/planning/obstacle_cruise_planner/src/polygon_utils.cpp index 02bec1daead1a..d0b635951e074 100644 --- a/planning/obstacle_cruise_planner/src/polygon_utils.cpp +++ b/planning/obstacle_cruise_planner/src/polygon_utils.cpp @@ -133,7 +133,8 @@ std::vector getCollisionPoints( const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, const autoware_auto_perception_msgs::msg::Shape & shape, const rclcpp::Time & current_time, const double vehicle_max_longitudinal_offset, const bool is_driving_forward, - const double max_dist, const double max_prediction_time_for_collision_check) + std::vector & collision_index, const double max_dist, + const double max_prediction_time_for_collision_check) { std::vector collision_points; for (size_t i = 0; i < predicted_path.path.size(); ++i) { @@ -163,6 +164,7 @@ std::vector getCollisionPoints( *collision_idx, current_collision_points, traj, vehicle_max_longitudinal_offset, is_driving_forward); collision_points.push_back(nearest_collision_point); + collision_index.push_back(*collision_idx); } } @@ -175,12 +177,12 @@ std::vector willCollideWithSurroundObstacle( const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, const autoware_auto_perception_msgs::msg::Shape & shape, const rclcpp::Time & current_time, const double max_dist, const double ego_obstacle_overlap_time_threshold, - const double max_prediction_time_for_collision_check, + const double max_prediction_time_for_collision_check, std::vector & collision_index, const double vehicle_max_longitudinal_offset, const bool is_driving_forward) { const auto collision_points = getCollisionPoints( traj, traj_polygons, obj_header, predicted_path, shape, current_time, - vehicle_max_longitudinal_offset, is_driving_forward, max_dist, + vehicle_max_longitudinal_offset, is_driving_forward, collision_index, max_dist, max_prediction_time_for_collision_check); if (collision_points.empty()) { diff --git a/planning/obstacle_stop_planner/CMakeLists.txt b/planning/obstacle_stop_planner/CMakeLists.txt index 6886824dd5808..94048c6cbea65 100644 --- a/planning/obstacle_stop_planner/CMakeLists.txt +++ b/planning/obstacle_stop_planner/CMakeLists.txt @@ -11,6 +11,7 @@ find_package(PCL REQUIRED) ament_auto_add_library(obstacle_stop_planner SHARED src/debug_marker.cpp src/node.cpp + src/planner_utils.cpp src/adaptive_cruise_control.cpp ) diff --git a/planning/obstacle_stop_planner/README.md b/planning/obstacle_stop_planner/README.md index 3b0a38d194a78..a76b8a9b814f4 100644 --- a/planning/obstacle_stop_planner/README.md +++ b/planning/obstacle_stop_planner/README.md @@ -36,7 +36,6 @@ | `enable_slow_down` | bool | enable slow down planner [-] | | `max_velocity` | double | max velocity [m/s] | | `hunting_threshold` | double | even if the obstacle disappears, the stop judgment continues for hunting_threshold [s] | -| `lowpass_gain` | double | low pass gain for calculating acceleration [-] | ## Obstacle Stop Planner diff --git a/planning/obstacle_stop_planner/config/obstacle_stop_planner.param.yaml b/planning/obstacle_stop_planner/config/obstacle_stop_planner.param.yaml index ed4e632ebd6b3..ea506d349dc34 100644 --- a/planning/obstacle_stop_planner/config/obstacle_stop_planner.param.yaml +++ b/planning/obstacle_stop_planner/config/obstacle_stop_planner.param.yaml @@ -1,7 +1,6 @@ /**: ros__parameters: hunting_threshold: 0.5 # even if the obstacle disappears, the stop judgment continues for hunting_threshold [s] - lowpass_gain: 0.9 # gain parameter for low pass filter [-] max_velocity: 20.0 # max velocity [m/s] enable_slow_down: False # whether to use slow down planner [-] diff --git a/planning/obstacle_stop_planner/include/obstacle_stop_planner/debug_marker.hpp b/planning/obstacle_stop_planner/include/obstacle_stop_planner/debug_marker.hpp index 4b703e8812627..2b34a92c4c747 100644 --- a/planning/obstacle_stop_planner/include/obstacle_stop_planner/debug_marker.hpp +++ b/planning/obstacle_stop_planner/include/obstacle_stop_planner/debug_marker.hpp @@ -36,7 +36,17 @@ #include namespace motion_planning { + +using geometry_msgs::msg::Point; +using geometry_msgs::msg::Pose; +using std_msgs::msg::Header; +using visualization_msgs::msg::Marker; +using visualization_msgs::msg::MarkerArray; + using tier4_debug_msgs::msg::Float32MultiArrayStamped; +using tier4_planning_msgs::msg::StopFactor; +using tier4_planning_msgs::msg::StopReason; +using tier4_planning_msgs::msg::StopReasonArray; enum class PolygonType : int8_t { Vehicle = 0, Collision, SlowDownRange, SlowDown }; @@ -97,12 +107,12 @@ class ObstacleStopPlannerDebugNode bool pushPolygon( const std::vector & polygon, const double z, const PolygonType & type); bool pushPolygon(const std::vector & polygon, const PolygonType & type); - bool pushPose(const geometry_msgs::msg::Pose & pose, const PoseType & type); - bool pushObstaclePoint(const geometry_msgs::msg::Point & obstacle_point, const PointType & type); + bool pushPose(const Pose & pose, const PoseType & type); + bool pushObstaclePoint(const Point & obstacle_point, const PointType & type); bool pushObstaclePoint(const pcl::PointXYZ & obstacle_point, const PointType & type); - visualization_msgs::msg::MarkerArray makeVirtualWallMarker(); - visualization_msgs::msg::MarkerArray makeVisualizationMarker(); - tier4_planning_msgs::msg::StopReasonArray makeStopReasonArray(); + MarkerArray makeVirtualWallMarker(); + MarkerArray makeVisualizationMarker(); + StopReasonArray makeStopReasonArray(); void setDebugValues(const DebugValues::TYPE type, const double val) { @@ -111,19 +121,19 @@ class ObstacleStopPlannerDebugNode void publish(); private: - rclcpp::Publisher::SharedPtr virtual_wall_pub_; - rclcpp::Publisher::SharedPtr debug_viz_pub_; - rclcpp::Publisher::SharedPtr stop_reason_pub_; + rclcpp::Publisher::SharedPtr virtual_wall_pub_; + rclcpp::Publisher::SharedPtr debug_viz_pub_; + rclcpp::Publisher::SharedPtr stop_reason_pub_; rclcpp::Publisher::SharedPtr pub_debug_values_; rclcpp::Node * node_; double base_link2front_; - std::shared_ptr stop_pose_ptr_; - std::shared_ptr target_stop_pose_ptr_; - std::shared_ptr slow_down_start_pose_ptr_; - std::shared_ptr slow_down_end_pose_ptr_; - std::shared_ptr stop_obstacle_point_ptr_; - std::shared_ptr slow_down_obstacle_point_ptr_; + std::shared_ptr stop_pose_ptr_; + std::shared_ptr target_stop_pose_ptr_; + std::shared_ptr slow_down_start_pose_ptr_; + std::shared_ptr slow_down_end_pose_ptr_; + std::shared_ptr stop_obstacle_point_ptr_; + std::shared_ptr slow_down_obstacle_point_ptr_; std::vector> vehicle_polygons_; std::vector> slow_down_range_polygons_; std::vector> collision_polygons_; diff --git a/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp b/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp index 1294e6dccd5a1..2312400065eaf 100644 --- a/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp +++ b/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp @@ -26,7 +26,6 @@ #include #include #include -#include #include #include #include @@ -34,6 +33,8 @@ #include #include #include +#include +#include #include #include #include @@ -45,7 +46,6 @@ #include #include -#include #include #include @@ -62,11 +62,22 @@ namespace motion_planning { + namespace bg = boost::geometry; + +using diagnostic_msgs::msg::DiagnosticStatus; +using diagnostic_msgs::msg::KeyValue; +using geometry_msgs::msg::AccelWithCovarianceStamped; +using geometry_msgs::msg::Point; +using geometry_msgs::msg::Pose; +using geometry_msgs::msg::TransformStamped; +using nav_msgs::msg::Odometry; +using sensor_msgs::msg::PointCloud2; +using std_msgs::msg::Header; + using autoware_auto_perception_msgs::msg::PredictedObjects; using autoware_auto_planning_msgs::msg::Trajectory; using autoware_auto_planning_msgs::msg::TrajectoryPoint; -using TrajectoryPoints = std::vector; using tier4_autoware_utils::Point2d; using tier4_autoware_utils::Polygon2d; using tier4_debug_msgs::msg::BoolStamped; @@ -77,41 +88,48 @@ using tier4_planning_msgs::msg::VelocityLimit; using tier4_planning_msgs::msg::VelocityLimitClearCommand; using vehicle_info_util::VehicleInfo; +using TrajectoryPoints = std::vector; +using PointCloud = pcl::PointCloud; + class ObstacleStopPlannerNode : public rclcpp::Node { public: explicit ObstacleStopPlannerNode(const rclcpp::NodeOptions & node_options); private: - /* - * ROS - */ - // publisher and subscriber - rclcpp::Subscription::SharedPtr path_sub_; - rclcpp::Subscription::SharedPtr obstacle_pointcloud_sub_; - rclcpp::Subscription::SharedPtr current_velocity_sub_; - rclcpp::Subscription::SharedPtr dynamic_object_sub_; - rclcpp::Subscription::SharedPtr expand_stop_range_sub_; - rclcpp::Publisher::SharedPtr path_pub_; - rclcpp::Publisher::SharedPtr stop_reason_diag_pub_; + rclcpp::Subscription::SharedPtr sub_trajectory_; + + rclcpp::Subscription::SharedPtr sub_point_cloud_; + + rclcpp::Subscription::SharedPtr sub_odometry_; + + rclcpp::Subscription::SharedPtr sub_acceleration_; + + rclcpp::Subscription::SharedPtr sub_dynamic_objects_; + + rclcpp::Subscription::SharedPtr sub_expand_stop_range_; + + rclcpp::Publisher::SharedPtr pub_trajectory_; + + rclcpp::Publisher::SharedPtr pub_stop_reason_; + rclcpp::Publisher::SharedPtr pub_clear_velocity_limit_; + rclcpp::Publisher::SharedPtr pub_velocity_limit_; - std::unique_ptr acc_controller_; + std::unique_ptr acc_controller_; std::shared_ptr debug_ptr_; - std::shared_ptr lpf_acc_{nullptr}; boost::optional latest_stop_point_{boost::none}; boost::optional latest_slow_down_section_{boost::none}; tf2_ros::Buffer tf_buffer_{get_clock()}; tf2_ros::TransformListener tf_listener_{tf_buffer_}; - sensor_msgs::msg::PointCloud2::SharedPtr obstacle_ros_pointcloud_ptr_{nullptr}; + PointCloud2::SharedPtr obstacle_ros_pointcloud_ptr_{nullptr}; PredictedObjects::ConstSharedPtr object_ptr_{nullptr}; rclcpp::Time last_detect_time_collision_point_; rclcpp::Time last_detect_time_slowdown_point_; - nav_msgs::msg::Odometry::ConstSharedPtr current_velocity_ptr_{nullptr}; - nav_msgs::msg::Odometry::ConstSharedPtr prev_velocity_ptr_{nullptr}; - double current_acc_{0.0}; + Odometry::ConstSharedPtr current_velocity_ptr_{nullptr}; + AccelWithCovarianceStamped::ConstSharedPtr current_acceleration_ptr_{nullptr}; bool is_driving_forward_{true}; bool set_velocity_limit_{false}; @@ -121,85 +139,34 @@ class ObstacleStopPlannerNode : public rclcpp::Node StopParam stop_param_; SlowDownParam slow_down_param_; - // mutex for vehicle_info_, stop_param_, current_acc_, lpf_acc_, obstacle_ros_pointcloud_ptr_ + // mutex for vehicle_info_, stop_param_, current_acc_, obstacle_ros_pointcloud_ptr_ // NOTE: shared_ptr itself is thread safe so we do not have to care if *ptr is not used - // (current_velocity_ptr_, prev_velocity_ptr_) + // (current_velocity_ptr_) std::mutex mutex_; - /* - * Callback - */ - void obstaclePointcloudCallback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr input_msg); - void pathCallback(const Trajectory::ConstSharedPtr input_msg); - void dynamicObjectCallback(const PredictedObjects::ConstSharedPtr input_msg); - void currentVelocityCallback(const nav_msgs::msg::Odometry::ConstSharedPtr input_msg); - void externalExpandStopRangeCallback(const ExpandStopRange::ConstSharedPtr input_msg); - -private: - bool withinPolygon( - const std::vector & cv_polygon, const double radius, const Point2d & prev_point, - const Point2d & next_point, pcl::PointCloud::Ptr candidate_points_ptr, - pcl::PointCloud::Ptr within_points_ptr); - - bool convexHull( - const std::vector & pointcloud, std::vector & polygon_points); - void searchObstacle( const TrajectoryPoints & decimate_trajectory, TrajectoryPoints & output, - PlannerData & planner_data, const std_msgs::msg::Header & trajectory_header, - const VehicleInfo & vehicle_info, const StopParam & stop_param, - const sensor_msgs::msg::PointCloud2::SharedPtr obstacle_ros_pointcloud_ptr); + PlannerData & planner_data, const Header & trajectory_header, const VehicleInfo & vehicle_info, + const StopParam & stop_param, const PointCloud2::SharedPtr obstacle_ros_pointcloud_ptr); void insertVelocity( - TrajectoryPoints & trajectory, PlannerData & planner_data, - const std_msgs::msg::Header & trajectory_header, const VehicleInfo & vehicle_info, - const double current_acc, const double current_vel, const StopParam & stop_param); - - TrajectoryPoints decimateTrajectory( - const TrajectoryPoints & input, const double step_length, std::map & index_map); - - TrajectoryPoints trimTrajectoryWithIndexFromSelfPose( - const TrajectoryPoints & input, const geometry_msgs::msg::Pose & self_pose, size_t & index); - - bool searchPointcloudNearTrajectory( - const TrajectoryPoints & trajectory, - const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_points_ptr, - pcl::PointCloud::Ptr output_points_ptr, - const std_msgs::msg::Header & trajectory_header, const VehicleInfo & vehicle_info, + TrajectoryPoints & trajectory, PlannerData & planner_data, const Header & trajectory_header, + const VehicleInfo & vehicle_info, const double current_acc, const double current_vel, const StopParam & stop_param); - void createOneStepPolygon( - const geometry_msgs::msg::Pose & base_step_pose, - const geometry_msgs::msg::Pose & next_step_pose, std::vector & polygon, - const VehicleInfo & vehicle_info, const double expand_width = 0.0); - - bool getSelfPose( - const std_msgs::msg::Header & header, const tf2_ros::Buffer & tf_buffer, - geometry_msgs::msg::Pose & self_pose); - - void getNearestPoint( - const pcl::PointCloud & pointcloud, const geometry_msgs::msg::Pose & base_pose, - pcl::PointXYZ * nearest_collision_point, rclcpp::Time * nearest_collision_point_time); - - void getLateralNearestPoint( - const pcl::PointCloud & pointcloud, const geometry_msgs::msg::Pose & base_pose, - pcl::PointXYZ * lateral_nearest_point, double * deviation); - - geometry_msgs::msg::Pose getVehicleCenterFromBase( - const geometry_msgs::msg::Pose & base_pose, const VehicleInfo & vehicle_info); + bool searchPointcloudNearTrajectory( + const TrajectoryPoints & trajectory, const PointCloud2::ConstSharedPtr & input_points_ptr, + PointCloud::Ptr output_points_ptr, const Header & trajectory_header, + const VehicleInfo & vehicle_info, const StopParam & stop_param); - void insertStopPoint( - const StopPoint & stop_point, TrajectoryPoints & output, - diagnostic_msgs::msg::DiagnosticStatus & stop_reason_diag); + StopPoint createTargetPoint( + const int idx, const double margin, const TrajectoryPoints & base_trajectory, + const double dist_remain); StopPoint searchInsertPoint( const int idx, const TrajectoryPoints & base_trajectory, const double dist_remain, const StopParam & stop_param); - StopPoint createTargetPoint( - const int idx, const double margin, const TrajectoryPoints & base_trajectory, - const double dist_remain); - SlowDownSection createSlowDownSection( const int idx, const TrajectoryPoints & base_trajectory, const double lateral_deviation, const double dist_remain, const double dist_vehicle_to_obstacle, @@ -211,10 +178,8 @@ class ObstacleStopPlannerNode : public rclcpp::Node void insertSlowDownSection(const SlowDownSection & slow_down_section, TrajectoryPoints & output); - TrajectoryPoints extendTrajectory(const TrajectoryPoints & input, const double extend_distance); - - TrajectoryPoint getExtendTrajectoryPoint( - double extend_distance, const TrajectoryPoint & goal_point); + TrajectoryPoints trimTrajectoryWithIndexFromSelfPose( + const TrajectoryPoints & input, const Pose & self_pose, size_t & index); void setExternalVelocityLimit(); @@ -222,6 +187,19 @@ class ObstacleStopPlannerNode : public rclcpp::Node void publishDebugData( const PlannerData & planner_data, const double current_acc, const double current_vel); + + // Callback + void onTrigger(const Trajectory::ConstSharedPtr input_msg); + + void onOdometry(const Odometry::ConstSharedPtr input_msg); + + void onAcceleration(const AccelWithCovarianceStamped::ConstSharedPtr input_msg); + + void onPointCloud(const PointCloud2::ConstSharedPtr input_msg); + + void onDynamicObjects(const PredictedObjects::ConstSharedPtr input_msg); + + void onExpandStopRange(const ExpandStopRange::ConstSharedPtr input_msg); }; } // namespace motion_planning diff --git a/planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_data.hpp b/planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_data.hpp index 11fcdcdf56828..9ce3bd6213b88 100644 --- a/planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_data.hpp +++ b/planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_data.hpp @@ -17,7 +17,12 @@ #include +#include #include +#include + +#include +#include #include @@ -25,6 +30,9 @@ namespace motion_planning { using diagnostic_msgs::msg::DiagnosticStatus; +using geometry_msgs::msg::Pose; + +using autoware_auto_planning_msgs::msg::TrajectoryPoint; struct StopPoint { @@ -54,9 +62,6 @@ struct NodeParam // max velocity [m/s] double max_velocity; - // smoothing calculated current acceleration [-] - double lowpass_gain; - // keep slow down or stop state if obstacle vanished [s] double hunting_threshold; @@ -210,7 +215,7 @@ struct PlannerData { DiagnosticStatus stop_reason_diag{}; - geometry_msgs::msg::Pose current_pose{}; + Pose current_pose{}; pcl::PointXYZ nearest_collision_point; diff --git a/planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_utils.hpp b/planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_utils.hpp new file mode 100644 index 0000000000000..e3d3ea8a1952e --- /dev/null +++ b/planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_utils.hpp @@ -0,0 +1,173 @@ + +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef OBSTACLE_STOP_PLANNER__PLANNER_UTILS_HPP_ +#define OBSTACLE_STOP_PLANNER__PLANNER_UTILS_HPP_ + +#include "obstacle_stop_planner/planner_data.hpp" + +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include + +#include +#include +#include +#include + +namespace motion_planning +{ + +namespace bg = boost::geometry; + +using diagnostic_msgs::msg::DiagnosticStatus; +using diagnostic_msgs::msg::KeyValue; +using geometry_msgs::msg::Point; +using geometry_msgs::msg::Pose; +using geometry_msgs::msg::TransformStamped; +using std_msgs::msg::Header; + +using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using tier4_autoware_utils::Point2d; +using tier4_autoware_utils::Polygon2d; +using vehicle_info_util::VehicleInfo; + +using TrajectoryPoints = std::vector; +using PointCloud = pcl::PointCloud; + +bool validCheckDecelPlan( + const double v_end, const double a_end, const double v_target, const double a_target, + const double v_margin, const double a_margin); + +/** + * @brief calculate distance until velocity is reached target velocity (TYPE1) + * @param (v0) current velocity [m/s] + * @param (vt) target velocity [m/s] + * @param (a0) current acceleration [m/ss] + * @param (am) minimum deceleration [m/ss] + * @param (ja) maximum jerk [m/sss] + * @param (jd) minimum jerk [m/sss] + * @param (t_min) duration of constant deceleration [s] + * @return moving distance until velocity is reached vt [m] + * @detail TODO(Satoshi Ota) + */ +boost::optional calcDecelDistPlanType1( + const double v0, const double vt, const double a0, const double am, const double ja, + const double jd, const double t_min); + +/** + * @brief calculate distance until velocity is reached target velocity (TYPE2) + * @param (v0) current velocity [m/s] + * @param (vt) target velocity [m/s] + * @param (a0) current acceleration [m/ss] + * @param (am) minimum deceleration [m/ss] + * @param (ja) maximum jerk [m/sss] + * @param (jd) minimum jerk [m/sss] + * @return moving distance until velocity is reached vt [m] + * @detail TODO(Satoshi Ota) + */ +boost::optional calcDecelDistPlanType2( + const double v0, const double vt, const double a0, const double ja, const double jd); + +/** + * @brief calculate distance until velocity is reached target velocity (TYPE3) + * @param (v0) current velocity [m/s] + * @param (vt) target velocity [m/s] + * @param (a0) current acceleration [m/ss] + * @param (ja) maximum jerk [m/sss] + * @return moving distance until velocity is reached vt [m] + * @detail TODO(Satoshi Ota) + */ +boost::optional calcDecelDistPlanType3( + const double v0, const double vt, const double a0, const double ja); + +boost::optional calcDecelDistWithJerkAndAccConstraints( + const double current_vel, const double target_vel, const double current_acc, const double acc_min, + const double jerk_acc, const double jerk_dec); + +boost::optional> calcFeasibleMarginAndVelocity( + const SlowDownParam & slow_down_param, const double dist_baselink_to_obstacle, + const double current_vel, const double current_acc); + +boost::optional> getForwardInsertPointFromBasePoint( + const size_t base_idx, const TrajectoryPoints & trajectory, const double margin); + +boost::optional> getBackwardInsertPointFromBasePoint( + const size_t base_idx, const TrajectoryPoints & trajectory, const double margin); + +boost::optional> findNearestFrontIndex( + const size_t start_idx, const TrajectoryPoints & trajectory, const Point & point); + +void insertStopPoint( + const StopPoint & stop_point, TrajectoryPoints & output, DiagnosticStatus & stop_reason_diag); + +bool isInFrontOfTargetPoint(const Pose & pose, const Point & point); + +bool checkValidIndex(const Pose & p_base, const Pose & p_next, const Pose & p_target); + +bool withinPolygon( + const std::vector & cv_polygon, const double radius, const Point2d & prev_point, + const Point2d & next_point, PointCloud::Ptr candidate_points_ptr, + PointCloud::Ptr within_points_ptr); + +bool convexHull( + const std::vector & pointcloud, std::vector & polygon_points); + +void createOneStepPolygon( + const Pose & base_step_pose, const Pose & next_step_pose, std::vector & polygon, + const VehicleInfo & vehicle_info, const double expand_width = 0.0); + +bool getSelfPose(const Header & header, const tf2_ros::Buffer & tf_buffer, Pose & self_pose); + +void getNearestPoint( + const PointCloud & pointcloud, const Pose & base_pose, pcl::PointXYZ * nearest_collision_point, + rclcpp::Time * nearest_collision_point_time); + +void getLateralNearestPoint( + const PointCloud & pointcloud, const Pose & base_pose, pcl::PointXYZ * lateral_nearest_point, + double * deviation); + +Pose getVehicleCenterFromBase(const Pose & base_pose, const VehicleInfo & vehicle_info); + +std::string jsonDumpsPose(const Pose & pose); + +DiagnosticStatus makeStopReasonDiag(const std::string stop_reason, const Pose & stop_pose); + +TrajectoryPoint getBackwardPointFromBasePoint( + const TrajectoryPoint & p_from, const TrajectoryPoint & p_to, const TrajectoryPoint & p_base, + const double backward_length); + +TrajectoryPoints decimateTrajectory( + const TrajectoryPoints & input, const double step_length, std::map & index_map); + +TrajectoryPoints extendTrajectory(const TrajectoryPoints & input, const double extend_distance); + +TrajectoryPoint getExtendTrajectoryPoint( + double extend_distance, const TrajectoryPoint & goal_point); + +rclcpp::SubscriptionOptions createSubscriptionOptions(rclcpp::Node * node_ptr); + +} // namespace motion_planning + +#endif // OBSTACLE_STOP_PLANNER__PLANNER_UTILS_HPP_ diff --git a/planning/obstacle_stop_planner/src/debug_marker.cpp b/planning/obstacle_stop_planner/src/debug_marker.cpp index d880e940acca3..813c4c3b9c7dc 100644 --- a/planning/obstacle_stop_planner/src/debug_marker.cpp +++ b/planning/obstacle_stop_planner/src/debug_marker.cpp @@ -34,7 +34,6 @@ using tier4_autoware_utils::appendMarkerArray; using tier4_autoware_utils::calcOffsetPose; using tier4_autoware_utils::createDefaultMarker; using tier4_autoware_utils::createMarkerColor; -using tier4_autoware_utils::createMarkerOrientation; using tier4_autoware_utils::createMarkerScale; using tier4_autoware_utils::createPoint; @@ -44,12 +43,9 @@ ObstacleStopPlannerDebugNode::ObstacleStopPlannerDebugNode( rclcpp::Node * node, const double base_link2front) : node_(node), base_link2front_(base_link2front) { - virtual_wall_pub_ = - node_->create_publisher("~/virtual_wall", 1); - debug_viz_pub_ = - node_->create_publisher("~/debug/marker", 1); - stop_reason_pub_ = - node_->create_publisher("~/output/stop_reasons", 1); + virtual_wall_pub_ = node_->create_publisher("~/virtual_wall", 1); + debug_viz_pub_ = node_->create_publisher("~/debug/marker", 1); + stop_reason_pub_ = node_->create_publisher("~/output/stop_reasons", 1); pub_debug_values_ = node_->create_publisher("~/obstacle_stop/debug_values", 1); } @@ -95,21 +91,20 @@ bool ObstacleStopPlannerDebugNode::pushPolygon( } } -bool ObstacleStopPlannerDebugNode::pushPose( - const geometry_msgs::msg::Pose & pose, const PoseType & type) +bool ObstacleStopPlannerDebugNode::pushPose(const Pose & pose, const PoseType & type) { switch (type) { case PoseType::Stop: - stop_pose_ptr_ = std::make_shared(pose); + stop_pose_ptr_ = std::make_shared(pose); return true; case PoseType::TargetStop: - target_stop_pose_ptr_ = std::make_shared(pose); + target_stop_pose_ptr_ = std::make_shared(pose); return true; case PoseType::SlowDownStart: - slow_down_start_pose_ptr_ = std::make_shared(pose); + slow_down_start_pose_ptr_ = std::make_shared(pose); return true; case PoseType::SlowDownEnd: - slow_down_end_pose_ptr_ = std::make_shared(pose); + slow_down_end_pose_ptr_ = std::make_shared(pose); return true; default: return false; @@ -117,14 +112,14 @@ bool ObstacleStopPlannerDebugNode::pushPose( } bool ObstacleStopPlannerDebugNode::pushObstaclePoint( - const geometry_msgs::msg::Point & obstacle_point, const PointType & type) + const Point & obstacle_point, const PointType & type) { switch (type) { case PointType::Stop: - stop_obstacle_point_ptr_ = std::make_shared(obstacle_point); + stop_obstacle_point_ptr_ = std::make_shared(obstacle_point); return true; case PointType::SlowDown: - slow_down_obstacle_point_ptr_ = std::make_shared(obstacle_point); + slow_down_obstacle_point_ptr_ = std::make_shared(obstacle_point); return true; default: return false; @@ -134,7 +129,7 @@ bool ObstacleStopPlannerDebugNode::pushObstaclePoint( bool ObstacleStopPlannerDebugNode::pushObstaclePoint( const pcl::PointXYZ & obstacle_point, const PointType & type) { - geometry_msgs::msg::Point ros_point; + Point ros_point; ros_point.x = obstacle_point.x; ros_point.y = obstacle_point.y; ros_point.z = obstacle_point.z; @@ -156,7 +151,7 @@ void ObstacleStopPlannerDebugNode::publish() stop_reason_pub_->publish(stop_reason_msg); // publish debug values - tier4_debug_msgs::msg::Float32MultiArrayStamped debug_msg{}; + Float32MultiArrayStamped debug_msg{}; debug_msg.stamp = node_->now(); for (const auto & v : debug_values_.getValues()) { debug_msg.data.push_back(v); @@ -175,9 +170,9 @@ void ObstacleStopPlannerDebugNode::publish() slow_down_obstacle_point_ptr_ = nullptr; } -visualization_msgs::msg::MarkerArray ObstacleStopPlannerDebugNode::makeVirtualWallMarker() +MarkerArray ObstacleStopPlannerDebugNode::makeVirtualWallMarker() { - visualization_msgs::msg::MarkerArray msg; + MarkerArray msg; rclcpp::Time current_time = node_->now(); if (stop_pose_ptr_ != nullptr) { @@ -220,15 +215,15 @@ visualization_msgs::msg::MarkerArray ObstacleStopPlannerDebugNode::makeVirtualWa return msg; } -visualization_msgs::msg::MarkerArray ObstacleStopPlannerDebugNode::makeVisualizationMarker() +MarkerArray ObstacleStopPlannerDebugNode::makeVisualizationMarker() { - visualization_msgs::msg::MarkerArray msg; + MarkerArray msg; rclcpp::Time current_time = node_->now(); // polygon if (!vehicle_polygons_.empty()) { auto marker = createDefaultMarker( - "map", current_time, "detection_polygons", 0, visualization_msgs::msg::Marker::LINE_LIST, + "map", current_time, "detection_polygons", 0, Marker::LINE_LIST, createMarkerScale(0.01, 0.0, 0.0), createMarkerColor(0.0, 1.0, 0.0, 0.999)); for (size_t i = 0; i < vehicle_polygons_.size(); ++i) { @@ -251,7 +246,7 @@ visualization_msgs::msg::MarkerArray ObstacleStopPlannerDebugNode::makeVisualiza if (!collision_polygons_.empty()) { auto marker = createDefaultMarker( - "map", current_time, "collision_polygons", 0, visualization_msgs::msg::Marker::LINE_LIST, + "map", current_time, "collision_polygons", 0, Marker::LINE_LIST, createMarkerScale(0.05, 0.0, 0.0), createMarkerColor(1.0, 0.0, 0.0, 0.999)); for (size_t i = 0; i < collision_polygons_.size(); ++i) { @@ -274,9 +269,8 @@ visualization_msgs::msg::MarkerArray ObstacleStopPlannerDebugNode::makeVisualiza if (!slow_down_range_polygons_.empty()) { auto marker = createDefaultMarker( - "map", current_time, "slow_down_detection_polygons", 0, - visualization_msgs::msg::Marker::LINE_LIST, createMarkerScale(0.01, 0.0, 0.0), - createMarkerColor(0.0, 1.0, 0.0, 0.999)); + "map", current_time, "slow_down_detection_polygons", 0, Marker::LINE_LIST, + createMarkerScale(0.01, 0.0, 0.0), createMarkerColor(0.0, 1.0, 0.0, 0.999)); for (size_t i = 0; i < slow_down_range_polygons_.size(); ++i) { for (size_t j = 0; j < slow_down_range_polygons_.at(i).size(); ++j) { @@ -298,7 +292,7 @@ visualization_msgs::msg::MarkerArray ObstacleStopPlannerDebugNode::makeVisualiza if (!slow_down_polygons_.empty()) { auto marker = createDefaultMarker( - "map", current_time, "slow_down_polygons", 0, visualization_msgs::msg::Marker::LINE_LIST, + "map", current_time, "slow_down_polygons", 0, Marker::LINE_LIST, createMarkerScale(0.05, 0.0, 0.0), createMarkerColor(1.0, 1.0, 0.0, 0.999)); for (size_t i = 0; i < slow_down_polygons_.size(); ++i) { @@ -331,7 +325,7 @@ visualization_msgs::msg::MarkerArray ObstacleStopPlannerDebugNode::makeVisualiza if (stop_obstacle_point_ptr_ != nullptr) { auto marker = createDefaultMarker( - "map", current_time, "stop_obstacle_point", 0, visualization_msgs::msg::Marker::SPHERE, + "map", current_time, "stop_obstacle_point", 0, Marker::SPHERE, createMarkerScale(0.25, 0.25, 0.25), createMarkerColor(1.0, 0.0, 0.0, 0.999)); marker.pose.position = *stop_obstacle_point_ptr_; msg.markers.push_back(marker); @@ -339,9 +333,8 @@ visualization_msgs::msg::MarkerArray ObstacleStopPlannerDebugNode::makeVisualiza if (stop_obstacle_point_ptr_ != nullptr) { auto marker = createDefaultMarker( - "map", current_time, "stop_obstacle_text", 0, - visualization_msgs::msg::Marker::TEXT_VIEW_FACING, createMarkerScale(0.0, 0.0, 1.0), - createMarkerColor(1.0, 1.0, 1.0, 0.999)); + "map", current_time, "stop_obstacle_text", 0, Marker::TEXT_VIEW_FACING, + createMarkerScale(0.0, 0.0, 1.0), createMarkerColor(1.0, 1.0, 1.0, 0.999)); marker.pose.position = *stop_obstacle_point_ptr_; marker.pose.position.z += 2.0; marker.text = "!"; @@ -350,7 +343,7 @@ visualization_msgs::msg::MarkerArray ObstacleStopPlannerDebugNode::makeVisualiza if (slow_down_obstacle_point_ptr_ != nullptr) { auto marker = createDefaultMarker( - "map", current_time, "slow_down_obstacle_point", 0, visualization_msgs::msg::Marker::SPHERE, + "map", current_time, "slow_down_obstacle_point", 0, Marker::SPHERE, createMarkerScale(0.25, 0.25, 0.25), createMarkerColor(1.0, 0.0, 0.0, 0.999)); marker.pose.position = *slow_down_obstacle_point_ptr_; msg.markers.push_back(marker); @@ -358,9 +351,8 @@ visualization_msgs::msg::MarkerArray ObstacleStopPlannerDebugNode::makeVisualiza if (slow_down_obstacle_point_ptr_ != nullptr) { auto marker = createDefaultMarker( - "map", current_time, "slow_down_obstacle_text", 0, - visualization_msgs::msg::Marker::TEXT_VIEW_FACING, createMarkerScale(0.0, 0.0, 1.0), - createMarkerColor(1.0, 1.0, 1.0, 0.999)); + "map", current_time, "slow_down_obstacle_text", 0, Marker::TEXT_VIEW_FACING, + createMarkerScale(0.0, 0.0, 1.0), createMarkerColor(1.0, 1.0, 1.0, 0.999)); marker.pose.position = *slow_down_obstacle_point_ptr_; marker.pose.position.z += 2.0; marker.text = "!"; @@ -370,17 +362,17 @@ visualization_msgs::msg::MarkerArray ObstacleStopPlannerDebugNode::makeVisualiza return msg; } -tier4_planning_msgs::msg::StopReasonArray ObstacleStopPlannerDebugNode::makeStopReasonArray() +StopReasonArray ObstacleStopPlannerDebugNode::makeStopReasonArray() { // create header - std_msgs::msg::Header header; + Header header; header.frame_id = "map"; header.stamp = node_->now(); // create stop reason stamped - tier4_planning_msgs::msg::StopReason stop_reason_msg; - stop_reason_msg.reason = tier4_planning_msgs::msg::StopReason::OBSTACLE_STOP; - tier4_planning_msgs::msg::StopFactor stop_factor; + StopReason stop_reason_msg; + stop_reason_msg.reason = StopReason::OBSTACLE_STOP; + StopFactor stop_factor; if (stop_pose_ptr_ != nullptr) { stop_factor.stop_pose = *stop_pose_ptr_; @@ -391,7 +383,7 @@ tier4_planning_msgs::msg::StopReasonArray ObstacleStopPlannerDebugNode::makeStop } // create stop reason array - tier4_planning_msgs::msg::StopReasonArray stop_reason_array; + StopReasonArray stop_reason_array; stop_reason_array.header = header; stop_reason_array.stop_reasons.emplace_back(stop_reason_msg); return stop_reason_array; diff --git a/planning/obstacle_stop_planner/src/node.cpp b/planning/obstacle_stop_planner/src/node.cpp index da485a4e52cdf..0166a56d3c322 100644 --- a/planning/obstacle_stop_planner/src/node.cpp +++ b/planning/obstacle_stop_planner/src/node.cpp @@ -22,12 +22,11 @@ #define EIGEN_MPL2_ONLY #include "obstacle_stop_planner/node.hpp" +#include "obstacle_stop_planner/planner_utils.hpp" #include #include -#include - #include #include @@ -47,402 +46,12 @@ using motion_utils::calcLongitudinalOffsetToSegment; using motion_utils::calcSignedArcLength; using motion_utils::findFirstNearestIndexWithSoftConstraints; using motion_utils::findFirstNearestSegmentIndexWithSoftConstraints; -using tier4_autoware_utils::calcAzimuthAngle; using tier4_autoware_utils::calcDistance2d; using tier4_autoware_utils::createPoint; using tier4_autoware_utils::getPoint; using tier4_autoware_utils::getPose; using tier4_autoware_utils::getRPY; -namespace -{ -rclcpp::SubscriptionOptions createSubscriptionOptions(rclcpp::Node * node_ptr) -{ - rclcpp::CallbackGroup::SharedPtr callback_group = - node_ptr->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - - auto sub_opt = rclcpp::SubscriptionOptions(); - sub_opt.callback_group = callback_group; - - return sub_opt; -} - -bool validCheckDecelPlan( - const double v_end, const double a_end, const double v_target, const double a_target, - const double v_margin, const double a_margin) -{ - const double v_min = v_target - std::abs(v_margin); - const double v_max = v_target + std::abs(v_margin); - const double a_min = a_target - std::abs(a_margin); - const double a_max = a_target + std::abs(a_margin); - - if (v_end < v_min || v_max < v_end) { - RCLCPP_DEBUG_STREAM( - rclcpp::get_logger("validCheckDecelPlan"), - "[validCheckDecelPlan] valid check error! v_target = " << v_target << ", v_end = " << v_end); - return false; - } - if (a_end < a_min || a_max < a_end) { - RCLCPP_DEBUG_STREAM( - rclcpp::get_logger("validCheckDecelPlan"), - "[validCheckDecelPlan] valid check error! a_target = " << a_target << ", a_end = " << a_end); - return false; - } - - return true; -} -/** - * @brief calculate distance until velocity is reached target velocity (TYPE1) - * @param (v0) current velocity [m/s] - * @param (vt) target velocity [m/s] - * @param (a0) current acceleration [m/ss] - * @param (am) minimum deceleration [m/ss] - * @param (ja) maximum jerk [m/sss] - * @param (jd) minimum jerk [m/sss] - * @param (t_min) duration of constant deceleration [s] - * @return moving distance until velocity is reached vt [m] - * @detail TODO(Satoshi Ota) - */ -boost::optional calcDecelDistPlanType1( - const double v0, const double vt, const double a0, const double am, const double ja, - const double jd, const double t_min) -{ - constexpr double epsilon = 1e-3; - - const double j1 = am < a0 ? jd : ja; - const double t1 = epsilon < (am - a0) / j1 ? (am - a0) / j1 : 0.0; - const double a1 = a0 + j1 * t1; - const double v1 = v0 + a0 * t1 + 0.5 * j1 * t1 * t1; - const double x1 = v0 * t1 + 0.5 * a0 * t1 * t1 + (1.0 / 6.0) * j1 * t1 * t1 * t1; - - const double t2 = epsilon < t_min ? t_min : 0.0; - const double a2 = a1; - const double v2 = v1 + a1 * t2; - const double x2 = x1 + v1 * t2 + 0.5 * a1 * t2 * t2; - - const double t3 = epsilon < (0.0 - am) / ja ? (0.0 - am) / ja : 0.0; - const double a3 = a2 + ja * t3; - const double v3 = v2 + a2 * t3 + 0.5 * ja * t3 * t3; - const double x3 = x2 + v2 * t3 + 0.5 * a2 * t3 * t3 + (1.0 / 6.0) * ja * t3 * t3 * t3; - - const double a_target = 0.0; - const double v_margin = 0.3; // [m/s] - const double a_margin = 0.1; // [m/s^2] - - if (!validCheckDecelPlan(v3, a3, vt, a_target, v_margin, a_margin)) { - return {}; - } - - return x3; -} -/** - * @brief calculate distance until velocity is reached target velocity (TYPE2) - * @param (v0) current velocity [m/s] - * @param (vt) target velocity [m/s] - * @param (a0) current acceleration [m/ss] - * @param (am) minimum deceleration [m/ss] - * @param (ja) maximum jerk [m/sss] - * @param (jd) minimum jerk [m/sss] - * @return moving distance until velocity is reached vt [m] - * @detail TODO(Satoshi Ota) - */ -boost::optional calcDecelDistPlanType2( - const double v0, const double vt, const double a0, const double ja, const double jd) -{ - constexpr double epsilon = 1e-3; - - const double a1_square = (vt - v0 - 0.5 * (0.0 - a0) / jd * a0) * (2.0 * ja * jd / (ja - jd)); - const double a1 = -std::sqrt(a1_square); - - const double t1 = epsilon < (a1 - a0) / jd ? (a1 - a0) / jd : 0.0; - const double v1 = v0 + a0 * t1 + 0.5 * jd * t1 * t1; - const double x1 = v0 * t1 + 0.5 * a0 * t1 * t1 + (1.0 / 6.0) * jd * t1 * t1 * t1; - - const double t2 = epsilon < (0.0 - a1) / ja ? (0.0 - a1) / ja : 0.0; - const double a2 = a1 + ja * t2; - const double v2 = v1 + a1 * t2 + 0.5 * ja * t2 * t2; - const double x2 = x1 + v1 * t2 + 0.5 * a1 * t2 * t2 + (1.0 / 6.0) * ja * t2 * t2 * t2; - - const double a_target = 0.0; - const double v_margin = 0.3; - const double a_margin = 0.1; - - if (!validCheckDecelPlan(v2, a2, vt, a_target, v_margin, a_margin)) { - return {}; - } - - return x2; -} -/** - * @brief calculate distance until velocity is reached target velocity (TYPE3) - * @param (v0) current velocity [m/s] - * @param (vt) target velocity [m/s] - * @param (a0) current acceleration [m/ss] - * @param (ja) maximum jerk [m/sss] - * @return moving distance until velocity is reached vt [m] - * @detail TODO(Satoshi Ota) - */ -boost::optional calcDecelDistPlanType3( - const double v0, const double vt, const double a0, const double ja) -{ - constexpr double epsilon = 1e-3; - - const double t_acc = (0.0 - a0) / ja; - - const double t1 = epsilon < t_acc ? t_acc : 0.0; - const double a1 = a0 + ja * t1; - const double v1 = v0 + a0 * t1 + 0.5 * ja * t1 * t1; - const double x1 = v0 * t1 + 0.5 * a0 * t1 * t1 + (1.0 / 6.0) * ja * t1 * t1 * t1; - - const double a_target = 0.0; - const double v_margin = 0.3; - const double a_margin = 0.1; - - if (!validCheckDecelPlan(v1, a1, vt, a_target, v_margin, a_margin)) { - return {}; - } - - return x1; -} -boost::optional calcDecelDistWithJerkAndAccConstraints( - const double current_vel, const double target_vel, const double current_acc, const double acc_min, - const double jerk_acc, const double jerk_dec) -{ - constexpr double epsilon = 1e-3; - const double t_dec = - acc_min < current_acc ? (acc_min - current_acc) / jerk_dec : (acc_min - current_acc) / jerk_acc; - const double t_acc = (0.0 - acc_min) / jerk_acc; - const double t_min = (target_vel - current_vel - current_acc * t_dec - - 0.5 * jerk_dec * t_dec * t_dec - 0.5 * acc_min * t_acc) / - acc_min; - - // check if it is possible to decelerate to the target velocity - // by simply bringing the current acceleration to zero. - const auto is_decel_needed = - 0.5 * (0.0 - current_acc) / jerk_acc * current_acc > target_vel - current_vel; - - if (t_min > epsilon) { - return calcDecelDistPlanType1( - current_vel, target_vel, current_acc, acc_min, jerk_acc, jerk_dec, t_min); - } else if (is_decel_needed || current_acc > epsilon) { - return calcDecelDistPlanType2(current_vel, target_vel, current_acc, jerk_acc, jerk_dec); - } else { - return calcDecelDistPlanType3(current_vel, target_vel, current_acc, jerk_acc); - } - - return {}; -} -boost::optional> calcFeasibleMarginAndVelocity( - const SlowDownParam & slow_down_param, const double dist_baselink_to_obstacle, - const double current_vel, const double current_acc) -{ - const auto & p = slow_down_param; - const auto & logger = rclcpp::get_logger("calcFeasibleMarginAndVelocity"); - constexpr double epsilon = 1e-4; - - if (current_vel < p.slow_down_velocity + epsilon) { - return std::make_pair(p.longitudinal_forward_margin, p.slow_down_velocity); - } - - for (double planning_jerk = p.jerk_start; planning_jerk > p.slow_down_min_jerk - epsilon; - planning_jerk += p.jerk_span) { - const double jerk_dec = planning_jerk; - const double jerk_acc = std::abs(planning_jerk); - - const auto planning_dec = - planning_jerk > p.normal_min_jerk ? p.limit_min_acc : p.normal_min_acc; - const auto stop_dist = calcDecelDistWithJerkAndAccConstraints( - current_vel, p.slow_down_velocity, current_acc, planning_dec, jerk_acc, jerk_dec); - - if (!stop_dist) { - continue; - } - - if (stop_dist.get() + p.longitudinal_forward_margin < dist_baselink_to_obstacle) { - RCLCPP_DEBUG( - logger, "[found plan] dist:%-6.2f jerk:%-6.2f margin:%-6.2f v0:%-6.2f vt:%-6.2f", - stop_dist.get(), planning_jerk, p.longitudinal_forward_margin, p.slow_down_velocity, - current_vel); - return std::make_pair(p.longitudinal_forward_margin, p.slow_down_velocity); - } - } - - { - const double jerk_dec = p.slow_down_min_jerk; - const double jerk_acc = std::abs(p.slow_down_min_jerk); - - const auto planning_dec = - p.slow_down_min_jerk > p.normal_min_jerk ? p.limit_min_acc : p.normal_min_acc; - const auto stop_dist = calcDecelDistWithJerkAndAccConstraints( - current_vel, p.slow_down_velocity, current_acc, planning_dec, jerk_acc, jerk_dec); - - if (!stop_dist) { - return {}; - } - - if (stop_dist.get() + p.min_longitudinal_forward_margin < dist_baselink_to_obstacle) { - const auto planning_margin = dist_baselink_to_obstacle - stop_dist.get(); - RCLCPP_DEBUG( - logger, "[relax margin] dist:%-6.2f jerk:%-6.2f margin:%-6.2f v0:%-6.2f vt%-6.2f", - stop_dist.get(), p.slow_down_min_jerk, planning_margin, p.slow_down_velocity, current_vel); - return std::make_pair(planning_margin, p.slow_down_velocity); - } - } - - RCLCPP_DEBUG(logger, "relax target slow down velocity"); - return {}; -} -TrajectoryPoint getBackwardPointFromBasePoint( - const TrajectoryPoint & p_from, const TrajectoryPoint & p_to, const TrajectoryPoint & p_base, - const double backward_length) -{ - TrajectoryPoint output; - const double dx = p_to.pose.position.x - p_from.pose.position.x; - const double dy = p_to.pose.position.y - p_from.pose.position.y; - const double norm = std::hypot(dx, dy); - - output = p_base; - output.pose.position.x += backward_length * dx / norm; - output.pose.position.y += backward_length * dy / norm; - - return output; -} -boost::optional> getForwardInsertPointFromBasePoint( - const size_t base_idx, const TrajectoryPoints & trajectory, const double margin) -{ - if (base_idx + 1 > trajectory.size()) { - return {}; - } - - if (margin < std::numeric_limits::epsilon()) { - return std::make_pair(base_idx, trajectory.at(base_idx)); - } - - double length_sum = 0.0; - double length_residual = 0.0; - - for (size_t i = base_idx; i < trajectory.size() - 1; ++i) { - const auto & p_front = trajectory.at(i); - const auto & p_back = trajectory.at(i + 1); - - length_sum += calcDistance2d(p_front, p_back); - length_residual = length_sum - margin; - - if (length_residual >= std::numeric_limits::epsilon()) { - const auto p_insert = getBackwardPointFromBasePoint(p_back, p_front, p_back, length_residual); - - // p_front(trajectory.points.at(i)) is insert base point - return std::make_pair(i, p_insert); - } - } - - if (length_residual < std::numeric_limits::epsilon()) { - return std::make_pair(trajectory.size() - 1, trajectory.back()); - } - - return {}; -} -boost::optional> getBackwardInsertPointFromBasePoint( - const size_t base_idx, const TrajectoryPoints & trajectory, const double margin) -{ - if (base_idx + 1 > trajectory.size()) { - return {}; - } - - if (margin < std::numeric_limits::epsilon()) { - return std::make_pair(base_idx, trajectory.at(base_idx)); - } - - double length_sum = 0.0; - double length_residual = 0.0; - - for (size_t i = base_idx; 0 < i; --i) { - const auto & p_front = trajectory.at(i - 1); - const auto & p_back = trajectory.at(i); - - length_sum += calcDistance2d(p_front, p_back); - length_residual = length_sum - margin; - - if (length_residual >= std::numeric_limits::epsilon()) { - const auto p_insert = - getBackwardPointFromBasePoint(p_front, p_back, p_front, length_residual); - - // p_front(trajectory.points.at(i-1)) is insert base point - return std::make_pair(i - 1, p_insert); - } - } - - if (length_residual < std::numeric_limits::epsilon()) { - return std::make_pair(size_t(0), trajectory.front()); - } - - return {}; -} -boost::optional> findNearestFrontIndex( - const size_t start_idx, const TrajectoryPoints & trajectory, - const geometry_msgs::msg::Point & point) -{ - for (size_t i = start_idx; i < trajectory.size(); ++i) { - const auto & p_traj = trajectory.at(i).pose; - const auto yaw = getRPY(p_traj).z; - const Point2d p_traj_direction(std::cos(yaw), std::sin(yaw)); - const Point2d p_traj_to_target(point.x - p_traj.position.x, point.y - p_traj.position.y); - - const auto is_in_front_of_target_point = p_traj_direction.dot(p_traj_to_target) < 0.0; - const auto is_trajectory_end = i + 1 == trajectory.size(); - - if (is_in_front_of_target_point || is_trajectory_end) { - const auto dist_p_traj_to_target = p_traj_direction.normalized().dot(p_traj_to_target); - return std::make_pair(i, dist_p_traj_to_target); - } - } - - return {}; -} -bool isInFrontOfTargetPoint( - const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Point & point) -{ - const auto yaw = getRPY(pose).z; - const Point2d pose_direction(std::cos(yaw), std::sin(yaw)); - const Point2d to_target(point.x - pose.position.x, point.y - pose.position.y); - - return pose_direction.dot(to_target) < 0.0; -} -bool checkValidIndex( - const geometry_msgs::msg::Pose & p_base, const geometry_msgs::msg::Pose & p_next, - const geometry_msgs::msg::Pose & p_target) -{ - const Point2d base2target( - p_target.position.x - p_base.position.x, p_target.position.y - p_base.position.y); - const Point2d target2next( - p_next.position.x - p_target.position.x, p_next.position.y - p_target.position.y); - return base2target.dot(target2next) > 0.0; -} -std::string jsonDumpsPose(const geometry_msgs::msg::Pose & pose) -{ - const std::string json_dumps_pose = - (boost::format( - R"({"position":{"x":%lf,"y":%lf,"z":%lf},"orientation":{"w":%lf,"x":%lf,"y":%lf,"z":%lf}})") % - pose.position.x % pose.position.y % pose.position.z % pose.orientation.w % pose.orientation.x % - pose.orientation.y % pose.orientation.z) - .str(); - return json_dumps_pose; -} -diagnostic_msgs::msg::DiagnosticStatus makeStopReasonDiag( - const std::string stop_reason, const geometry_msgs::msg::Pose & stop_pose) -{ - diagnostic_msgs::msg::DiagnosticStatus stop_reason_diag; - diagnostic_msgs::msg::KeyValue stop_reason_diag_kv; - stop_reason_diag.level = diagnostic_msgs::msg::DiagnosticStatus::OK; - stop_reason_diag.name = "stop_reason"; - stop_reason_diag.message = stop_reason; - stop_reason_diag_kv.key = "stop_pose"; - stop_reason_diag_kv.value = jsonDumpsPose(stop_pose); - stop_reason_diag.values.push_back(stop_reason_diag_kv); - return stop_reason_diag; -} -} // namespace - ObstacleStopPlannerNode::ObstacleStopPlannerNode(const rclcpp::NodeOptions & node_options) : Node("obstacle_stop_planner", node_options) { @@ -457,8 +66,6 @@ ObstacleStopPlannerNode::ObstacleStopPlannerNode(const rclcpp::NodeOptions & nod p.enable_slow_down = declare_parameter("enable_slow_down"); p.max_velocity = declare_parameter("max_velocity"); p.hunting_threshold = declare_parameter("hunting_threshold"); - p.lowpass_gain = declare_parameter("lowpass_gain"); - lpf_acc_ = std::make_shared(p.lowpass_gain); p.ego_nearest_dist_threshold = declare_parameter("ego_nearest_dist_threshold"); p.ego_nearest_yaw_threshold = declare_parameter("ego_nearest_yaw_threshold"); } @@ -539,58 +146,66 @@ ObstacleStopPlannerNode::ObstacleStopPlannerNode(const rclcpp::NodeOptions & nod } // Initializer - acc_controller_ = std::make_unique( + acc_controller_ = std::make_unique( this, i.vehicle_width_m, i.vehicle_length_m, i.max_longitudinal_offset_m); debug_ptr_ = std::make_shared(this, i.max_longitudinal_offset_m); last_detect_time_slowdown_point_ = this->now(); last_detect_time_collision_point_ = this->now(); // Publishers - path_pub_ = this->create_publisher("~/output/trajectory", 1); - stop_reason_diag_pub_ = - this->create_publisher("~/output/stop_reason", 1); + pub_trajectory_ = this->create_publisher("~/output/trajectory", 1); + + pub_stop_reason_ = this->create_publisher("~/output/stop_reason", 1); + pub_clear_velocity_limit_ = this->create_publisher( "~/output/velocity_limit_clear_command", rclcpp::QoS{1}.transient_local()); + pub_velocity_limit_ = this->create_publisher( "~/output/max_velocity", rclcpp::QoS{1}.transient_local()); // Subscribers - obstacle_pointcloud_sub_ = this->create_subscription( + sub_point_cloud_ = this->create_subscription( "~/input/pointcloud", rclcpp::SensorDataQoS(), - std::bind(&ObstacleStopPlannerNode::obstaclePointcloudCallback, this, std::placeholders::_1), + std::bind(&ObstacleStopPlannerNode::onPointCloud, this, std::placeholders::_1), createSubscriptionOptions(this)); - path_sub_ = this->create_subscription( + + sub_trajectory_ = this->create_subscription( "~/input/trajectory", 1, - std::bind(&ObstacleStopPlannerNode::pathCallback, this, std::placeholders::_1), + std::bind(&ObstacleStopPlannerNode::onTrigger, this, std::placeholders::_1), createSubscriptionOptions(this)); - current_velocity_sub_ = this->create_subscription( + + sub_odometry_ = this->create_subscription( "~/input/odometry", 1, - std::bind(&ObstacleStopPlannerNode::currentVelocityCallback, this, std::placeholders::_1), + std::bind(&ObstacleStopPlannerNode::onOdometry, this, std::placeholders::_1), + createSubscriptionOptions(this)); + + sub_acceleration_ = this->create_subscription( + "~/input/acceleration", 1, + std::bind(&ObstacleStopPlannerNode::onAcceleration, this, std::placeholders::_1), createSubscriptionOptions(this)); - dynamic_object_sub_ = this->create_subscription( + + sub_dynamic_objects_ = this->create_subscription( "~/input/objects", 1, - std::bind(&ObstacleStopPlannerNode::dynamicObjectCallback, this, std::placeholders::_1), + std::bind(&ObstacleStopPlannerNode::onDynamicObjects, this, std::placeholders::_1), createSubscriptionOptions(this)); - expand_stop_range_sub_ = this->create_subscription( + + sub_expand_stop_range_ = this->create_subscription( "~/input/expand_stop_range", 1, - std::bind( - &ObstacleStopPlannerNode::externalExpandStopRangeCallback, this, std::placeholders::_1), + std::bind(&ObstacleStopPlannerNode::onExpandStopRange, this, std::placeholders::_1), createSubscriptionOptions(this)); } -void ObstacleStopPlannerNode::obstaclePointcloudCallback( - const sensor_msgs::msg::PointCloud2::ConstSharedPtr input_msg) +void ObstacleStopPlannerNode::onPointCloud(const PointCloud2::ConstSharedPtr input_msg) { // mutex for obstacle_ros_pointcloud_ptr_ // NOTE: *obstacle_ros_pointcloud_ptr_ is used std::lock_guard lock(mutex_); - obstacle_ros_pointcloud_ptr_ = std::make_shared(); + obstacle_ros_pointcloud_ptr_ = std::make_shared(); pcl::VoxelGrid filter; - pcl::PointCloud::Ptr pointcloud_ptr(new pcl::PointCloud); - pcl::PointCloud::Ptr no_height_pointcloud_ptr(new pcl::PointCloud); - pcl::PointCloud::Ptr no_height_filtered_pointcloud_ptr( - new pcl::PointCloud); + PointCloud::Ptr pointcloud_ptr(new PointCloud); + PointCloud::Ptr no_height_pointcloud_ptr(new PointCloud); + PointCloud::Ptr no_height_filtered_pointcloud_ptr(new PointCloud); pcl::fromROSMsg(*input_msg, *pointcloud_ptr); @@ -604,38 +219,42 @@ void ObstacleStopPlannerNode::obstaclePointcloudCallback( obstacle_ros_pointcloud_ptr_->header = input_msg->header; } -void ObstacleStopPlannerNode::pathCallback(const Trajectory::ConstSharedPtr input_msg) +void ObstacleStopPlannerNode::onTrigger(const Trajectory::ConstSharedPtr input_msg) { mutex_.lock(); // NOTE: these variables must not be referenced for multithreading const auto vehicle_info = vehicle_info_; const auto stop_param = stop_param_; const auto obstacle_ros_pointcloud_ptr = obstacle_ros_pointcloud_ptr_; - const auto current_vel = current_velocity_ptr_->twist.twist.linear.x; - const auto current_acc = current_acc_; + const auto object_ptr = object_ptr_; + const auto current_velocity_ptr = current_velocity_ptr_; + const auto current_acceleration_ptr = current_acceleration_ptr_; mutex_.unlock(); { - std::lock_guard lock(mutex_); - - if (!object_ptr_) { + const auto waiting = [this](const auto & str) { RCLCPP_WARN_THROTTLE( - get_logger(), *get_clock(), std::chrono::milliseconds(1000).count(), - "waiting for dynamic objects..."); + get_logger(), *get_clock(), std::chrono::milliseconds(1000).count(), "waiting for %s ...", + str); + }; + + if (!object_ptr) { + waiting("perception object"); return; } if (!obstacle_ros_pointcloud_ptr) { - RCLCPP_WARN_THROTTLE( - get_logger(), *get_clock(), std::chrono::milliseconds(1000).count(), - "waiting for obstacle pointcloud..."); + waiting("obstacle pointcloud"); return; } - if (!current_velocity_ptr_ && node_param_.enable_slow_down) { - RCLCPP_WARN_THROTTLE( - get_logger(), *get_clock(), std::chrono::milliseconds(1000).count(), - "waiting for current velocity..."); + if (!current_velocity_ptr) { + waiting("current velocity"); + return; + } + + if (!current_acceleration_ptr) { + waiting("current acceleration"); return; } @@ -644,13 +263,16 @@ void ObstacleStopPlannerNode::pathCallback(const Trajectory::ConstSharedPtr inpu } } + const auto current_vel = current_velocity_ptr->twist.twist.linear.x; + const auto current_acc = current_acceleration_ptr->accel.accel.linear.x; + // TODO(someone): support backward path const auto is_driving_forward = motion_utils::isDrivingForwardWithTwist(input_msg->points); is_driving_forward_ = is_driving_forward ? is_driving_forward.get() : is_driving_forward_; if (!is_driving_forward_) { RCLCPP_WARN_THROTTLE( get_logger(), *get_clock(), 3000, "Backward path is NOT supported. publish input as it is."); - path_pub_->publish(*input_msg); + pub_trajectory_->publish(*input_msg); return; } @@ -693,19 +315,17 @@ void ObstacleStopPlannerNode::pathCallback(const Trajectory::ConstSharedPtr inpu publishDebugData(planner_data, current_acc, current_vel); trajectory.header = input_msg->header; - path_pub_->publish(trajectory); + pub_trajectory_->publish(trajectory); } void ObstacleStopPlannerNode::searchObstacle( const TrajectoryPoints & decimate_trajectory, TrajectoryPoints & output, - PlannerData & planner_data, const std_msgs::msg::Header & trajectory_header, - const VehicleInfo & vehicle_info, const StopParam & stop_param, - const sensor_msgs::msg::PointCloud2::SharedPtr obstacle_ros_pointcloud_ptr) + PlannerData & planner_data, const Header & trajectory_header, const VehicleInfo & vehicle_info, + const StopParam & stop_param, const PointCloud2::SharedPtr obstacle_ros_pointcloud_ptr) { // search candidate obstacle pointcloud - pcl::PointCloud::Ptr slow_down_pointcloud_ptr(new pcl::PointCloud); - pcl::PointCloud::Ptr obstacle_candidate_pointcloud_ptr( - new pcl::PointCloud); + PointCloud::Ptr slow_down_pointcloud_ptr(new PointCloud); + PointCloud::Ptr obstacle_candidate_pointcloud_ptr(new PointCloud); if (!searchPointcloudNearTrajectory( decimate_trajectory, obstacle_ros_pointcloud_ptr, obstacle_candidate_pointcloud_ptr, trajectory_header, vehicle_info, stop_param)) { @@ -769,8 +389,7 @@ void ObstacleStopPlannerNode::searchObstacle( one_step_move_vehicle_polygon, decimate_trajectory.at(i).pose.position.z, PolygonType::Vehicle); - pcl::PointCloud::Ptr collision_pointcloud_ptr( - new pcl::PointCloud); + PointCloud::Ptr collision_pointcloud_ptr(new PointCloud); collision_pointcloud_ptr->header = obstacle_candidate_pointcloud_ptr->header; planner_data.found_collision_points = withinPolygon( @@ -811,9 +430,9 @@ void ObstacleStopPlannerNode::searchObstacle( } void ObstacleStopPlannerNode::insertVelocity( - TrajectoryPoints & output, PlannerData & planner_data, - const std_msgs::msg::Header & trajectory_header, const VehicleInfo & vehicle_info, - const double current_acc, const double current_vel, const StopParam & stop_param) + TrajectoryPoints & output, PlannerData & planner_data, const Header & trajectory_header, + const VehicleInfo & vehicle_info, const double current_acc, const double current_vel, + const StopParam & stop_param) { const auto & base_link2front = vehicle_info.max_longitudinal_offset_m; const auto no_hunting_collision_point = @@ -988,35 +607,10 @@ void ObstacleStopPlannerNode::insertVelocity( } } - stop_reason_diag_pub_->publish(planner_data.stop_reason_diag); -} - -bool ObstacleStopPlannerNode::withinPolygon( - const std::vector & cv_polygon, const double radius, const Point2d & prev_point, - const Point2d & next_point, pcl::PointCloud::Ptr candidate_points_ptr, - pcl::PointCloud::Ptr within_points_ptr) -{ - Polygon2d boost_polygon; - bool find_within_points = false; - for (const auto & point : cv_polygon) { - boost_polygon.outer().push_back(bg::make(point.x, point.y)); - } - boost_polygon.outer().push_back(bg::make(cv_polygon.front().x, cv_polygon.front().y)); - - for (size_t j = 0; j < candidate_points_ptr->size(); ++j) { - Point2d point(candidate_points_ptr->at(j).x, candidate_points_ptr->at(j).y); - if (bg::distance(prev_point, point) < radius || bg::distance(next_point, point) < radius) { - if (bg::within(point, boost_polygon)) { - within_points_ptr->push_back(candidate_points_ptr->at(j)); - find_within_points = true; - } - } - } - return find_within_points; + pub_stop_reason_->publish(planner_data.stop_reason_diag); } -void ObstacleStopPlannerNode::externalExpandStopRangeCallback( - const ExpandStopRange::ConstSharedPtr input_msg) +void ObstacleStopPlannerNode::onExpandStopRange(const ExpandStopRange::ConstSharedPtr input_msg) { // mutex for vehicle_info_, stop_param_ std::lock_guard lock(mutex_); @@ -1028,41 +622,6 @@ void ObstacleStopPlannerNode::externalExpandStopRangeCallback( std::hypot(i.vehicle_width_m / 2.0 + stop_param_.lateral_margin, i.vehicle_length_m / 2.0); } -void ObstacleStopPlannerNode::insertStopPoint( - const StopPoint & stop_point, TrajectoryPoints & output, - diagnostic_msgs::msg::DiagnosticStatus & stop_reason_diag) -{ - const auto traj_end_idx = output.size() - 1; - const auto & stop_idx = stop_point.index; - - const auto & p_base = output.at(stop_idx); - const auto & p_next = output.at(std::min(stop_idx + 1, traj_end_idx)); - const auto & p_insert = stop_point.point; - - constexpr double min_dist = 1e-3; - - const auto is_p_base_and_p_insert_overlap = calcDistance2d(p_base, p_insert) < min_dist; - const auto is_p_next_and_p_insert_overlap = calcDistance2d(p_next, p_insert) < min_dist; - const auto is_valid_index = checkValidIndex(p_base.pose, p_next.pose, p_insert.pose); - - auto update_stop_idx = stop_idx; - - if (!is_p_base_and_p_insert_overlap && !is_p_next_and_p_insert_overlap && is_valid_index) { - // insert: start_idx and end_idx are shifted by one - output.insert(output.begin() + stop_idx + 1, p_insert); - update_stop_idx = std::min(update_stop_idx + 1, traj_end_idx); - } else if (is_p_next_and_p_insert_overlap) { - // not insert: p_insert is merged into p_next - update_stop_idx = std::min(update_stop_idx + 1, traj_end_idx); - } - - for (size_t i = update_stop_idx; i < output.size(); ++i) { - output.at(i).longitudinal_velocity_mps = 0.0; - } - - stop_reason_diag = makeStopReasonDiag("obstacle", p_insert.pose); -} - StopPoint ObstacleStopPlannerNode::searchInsertPoint( const int idx, const TrajectoryPoints & base_trajectory, const double dist_remain, const StopParam & stop_param) @@ -1256,122 +815,29 @@ void ObstacleStopPlannerNode::insertSlowDownSection( debug_ptr_->pushPose(p_base_end.pose, PoseType::SlowDownEnd); } -void ObstacleStopPlannerNode::dynamicObjectCallback( - const PredictedObjects::ConstSharedPtr input_msg) +void ObstacleStopPlannerNode::onDynamicObjects(const PredictedObjects::ConstSharedPtr input_msg) { std::lock_guard lock(mutex_); object_ptr_ = input_msg; } -void ObstacleStopPlannerNode::currentVelocityCallback( - const nav_msgs::msg::Odometry::ConstSharedPtr input_msg) +void ObstacleStopPlannerNode::onOdometry(const Odometry::ConstSharedPtr input_msg) { // mutex for current_acc_, lpf_acc_ std::lock_guard lock(mutex_); - current_velocity_ptr_ = input_msg; - - if (!prev_velocity_ptr_) { - prev_velocity_ptr_ = current_velocity_ptr_; - return; - } - - const double dv = - current_velocity_ptr_->twist.twist.linear.x - prev_velocity_ptr_->twist.twist.linear.x; - const double dt = std::max( - (rclcpp::Time(current_velocity_ptr_->header.stamp) - - rclcpp::Time(prev_velocity_ptr_->header.stamp)) - .seconds(), - 1e-03); - - const double accel = dv / dt; - - current_acc_ = lpf_acc_->filter(accel); - prev_velocity_ptr_ = current_velocity_ptr_; } -TrajectoryPoint ObstacleStopPlannerNode::getExtendTrajectoryPoint( - const double extend_distance, const TrajectoryPoint & goal_point) +void ObstacleStopPlannerNode::onAcceleration( + const AccelWithCovarianceStamped::ConstSharedPtr input_msg) { - tf2::Transform map2goal; - tf2::fromMsg(goal_point.pose, map2goal); - tf2::Transform local_extend_point; - local_extend_point.setOrigin(tf2::Vector3(extend_distance, 0.0, 0.0)); - tf2::Quaternion q; - q.setRPY(0, 0, 0); - local_extend_point.setRotation(q); - const auto map2extend_point = map2goal * local_extend_point; - geometry_msgs::msg::Pose extend_pose; - tf2::toMsg(map2extend_point, extend_pose); - TrajectoryPoint extend_trajectory_point; - extend_trajectory_point.pose = extend_pose; - extend_trajectory_point.longitudinal_velocity_mps = goal_point.longitudinal_velocity_mps; - extend_trajectory_point.lateral_velocity_mps = goal_point.lateral_velocity_mps; - extend_trajectory_point.acceleration_mps2 = goal_point.acceleration_mps2; - return extend_trajectory_point; -} - -TrajectoryPoints ObstacleStopPlannerNode::extendTrajectory( - const TrajectoryPoints & input, const double extend_distance) -{ - TrajectoryPoints output = input; - - if (extend_distance < std::numeric_limits::epsilon()) { - return output; - } - - const auto goal_point = input.back(); - double interpolation_distance = 0.1; - - double extend_sum = 0.0; - while (extend_sum <= (extend_distance - interpolation_distance)) { - const auto extend_trajectory_point = getExtendTrajectoryPoint(extend_sum, goal_point); - output.push_back(extend_trajectory_point); - extend_sum += interpolation_distance; - } - const auto extend_trajectory_point = getExtendTrajectoryPoint(extend_distance, goal_point); - output.push_back(extend_trajectory_point); - - return output; -} - -TrajectoryPoints ObstacleStopPlannerNode::decimateTrajectory( - const TrajectoryPoints & input, const double step_length, - std::map & index_map) -{ - TrajectoryPoints output{}; - - double trajectory_length_sum = 0.0; - double next_length = 0.0; - - for (int i = 0; i < static_cast(input.size()) - 1; ++i) { - const auto & p_front = input.at(i); - const auto & p_back = input.at(i + 1); - constexpr double epsilon = 1e-3; - - if (next_length <= trajectory_length_sum + epsilon) { - const auto p_interpolate = - getBackwardPointFromBasePoint(p_front, p_back, p_back, next_length - trajectory_length_sum); - output.push_back(p_interpolate); - - index_map.insert(std::make_pair(output.size() - 1, size_t(i))); - next_length += step_length; - continue; - } - - trajectory_length_sum += calcDistance2d(p_front, p_back); - } - if (!input.empty()) { - output.push_back(input.back()); - index_map.insert(std::make_pair(output.size() - 1, input.size() - 1)); - } - - return output; + std::lock_guard lock(mutex_); + current_acceleration_ptr_ = input_msg; } TrajectoryPoints ObstacleStopPlannerNode::trimTrajectoryWithIndexFromSelfPose( - const TrajectoryPoints & input, const geometry_msgs::msg::Pose & self_pose, size_t & index) + const TrajectoryPoints & input, const Pose & self_pose, size_t & index) { TrajectoryPoints output{}; @@ -1388,14 +854,12 @@ TrajectoryPoints ObstacleStopPlannerNode::trimTrajectoryWithIndexFromSelfPose( } bool ObstacleStopPlannerNode::searchPointcloudNearTrajectory( - const TrajectoryPoints & trajectory, - const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_points_ptr, - pcl::PointCloud::Ptr output_points_ptr, - const std_msgs::msg::Header & trajectory_header, const VehicleInfo & vehicle_info, - const StopParam & stop_param) + const TrajectoryPoints & trajectory, const PointCloud2::ConstSharedPtr & input_points_ptr, + PointCloud::Ptr output_points_ptr, const Header & trajectory_header, + const VehicleInfo & vehicle_info, const StopParam & stop_param) { // transform pointcloud - geometry_msgs::msg::TransformStamped transform_stamped{}; + TransformStamped transform_stamped{}; try { transform_stamped = tf_buffer_.lookupTransform( trajectory_header.frame_id, input_points_ptr->header.frame_id, input_points_ptr->header.stamp, @@ -1407,11 +871,11 @@ bool ObstacleStopPlannerNode::searchPointcloudNearTrajectory( return false; } - sensor_msgs::msg::PointCloud2 transformed_points{}; + PointCloud2 transformed_points{}; const Eigen::Matrix4f affine_matrix = tf2::transformToEigen(transform_stamped.transform).matrix().cast(); pcl_ros::transformPointCloud(affine_matrix, *input_points_ptr, transformed_points); - pcl::PointCloud::Ptr transformed_points_ptr(new pcl::PointCloud); + PointCloud::Ptr transformed_points_ptr(new PointCloud); pcl::fromROSMsg(transformed_points, *transformed_points_ptr); output_points_ptr->header = transformed_points_ptr->header; @@ -1439,160 +903,6 @@ bool ObstacleStopPlannerNode::searchPointcloudNearTrajectory( return true; } -void ObstacleStopPlannerNode::createOneStepPolygon( - const geometry_msgs::msg::Pose & base_step_pose, const geometry_msgs::msg::Pose & next_step_pose, - std::vector & polygon, const VehicleInfo & vehicle_info, const double expand_width) -{ - std::vector one_step_move_vehicle_corner_points; - - const auto & i = vehicle_info; - const auto & front_m = i.max_longitudinal_offset_m; - const auto & width_m = i.vehicle_width_m / 2.0 + expand_width; - const auto & back_m = i.rear_overhang_m; - // start step - { - const auto yaw = getRPY(base_step_pose).z; - one_step_move_vehicle_corner_points.push_back(cv::Point2d( - base_step_pose.position.x + std::cos(yaw) * front_m - std::sin(yaw) * width_m, - base_step_pose.position.y + std::sin(yaw) * front_m + std::cos(yaw) * width_m)); - one_step_move_vehicle_corner_points.push_back(cv::Point2d( - base_step_pose.position.x + std::cos(yaw) * front_m - std::sin(yaw) * -width_m, - base_step_pose.position.y + std::sin(yaw) * front_m + std::cos(yaw) * -width_m)); - one_step_move_vehicle_corner_points.push_back(cv::Point2d( - base_step_pose.position.x + std::cos(yaw) * -back_m - std::sin(yaw) * -width_m, - base_step_pose.position.y + std::sin(yaw) * -back_m + std::cos(yaw) * -width_m)); - one_step_move_vehicle_corner_points.push_back(cv::Point2d( - base_step_pose.position.x + std::cos(yaw) * -back_m - std::sin(yaw) * width_m, - base_step_pose.position.y + std::sin(yaw) * -back_m + std::cos(yaw) * width_m)); - } - // next step - { - const auto yaw = getRPY(next_step_pose).z; - one_step_move_vehicle_corner_points.push_back(cv::Point2d( - next_step_pose.position.x + std::cos(yaw) * front_m - std::sin(yaw) * width_m, - next_step_pose.position.y + std::sin(yaw) * front_m + std::cos(yaw) * width_m)); - one_step_move_vehicle_corner_points.push_back(cv::Point2d( - next_step_pose.position.x + std::cos(yaw) * front_m - std::sin(yaw) * -width_m, - next_step_pose.position.y + std::sin(yaw) * front_m + std::cos(yaw) * -width_m)); - one_step_move_vehicle_corner_points.push_back(cv::Point2d( - next_step_pose.position.x + std::cos(yaw) * -back_m - std::sin(yaw) * -width_m, - next_step_pose.position.y + std::sin(yaw) * -back_m + std::cos(yaw) * -width_m)); - one_step_move_vehicle_corner_points.push_back(cv::Point2d( - next_step_pose.position.x + std::cos(yaw) * -back_m - std::sin(yaw) * width_m, - next_step_pose.position.y + std::sin(yaw) * -back_m + std::cos(yaw) * width_m)); - } - convexHull(one_step_move_vehicle_corner_points, polygon); -} - -bool ObstacleStopPlannerNode::convexHull( - const std::vector & pointcloud, std::vector & polygon_points) -{ - cv::Point2d centroid; - centroid.x = 0; - centroid.y = 0; - for (const auto & point : pointcloud) { - centroid.x += point.x; - centroid.y += point.y; - } - centroid.x = centroid.x / static_cast(pointcloud.size()); - centroid.y = centroid.y / static_cast(pointcloud.size()); - - std::vector normalized_pointcloud; - std::vector normalized_polygon_points; - for (size_t i = 0; i < pointcloud.size(); ++i) { - normalized_pointcloud.push_back(cv::Point( - (pointcloud.at(i).x - centroid.x) * 1000.0, (pointcloud.at(i).y - centroid.y) * 1000.0)); - } - cv::convexHull(normalized_pointcloud, normalized_polygon_points); - - for (size_t i = 0; i < normalized_polygon_points.size(); ++i) { - cv::Point2d polygon_point; - polygon_point.x = (normalized_polygon_points.at(i).x / 1000.0 + centroid.x); - polygon_point.y = (normalized_polygon_points.at(i).y / 1000.0 + centroid.y); - polygon_points.push_back(polygon_point); - } - return true; -} - -bool ObstacleStopPlannerNode::getSelfPose( - const std_msgs::msg::Header & header, const tf2_ros::Buffer & tf_buffer, - geometry_msgs::msg::Pose & self_pose) -{ - try { - geometry_msgs::msg::TransformStamped transform; - transform = tf_buffer.lookupTransform( - header.frame_id, "base_link", header.stamp, rclcpp::Duration::from_seconds(0.1)); - self_pose.position.x = transform.transform.translation.x; - self_pose.position.y = transform.transform.translation.y; - self_pose.position.z = transform.transform.translation.z; - self_pose.orientation.x = transform.transform.rotation.x; - self_pose.orientation.y = transform.transform.rotation.y; - self_pose.orientation.z = transform.transform.rotation.z; - self_pose.orientation.w = transform.transform.rotation.w; - return true; - } catch (tf2::TransformException & ex) { - return false; - } -} - -void ObstacleStopPlannerNode::getNearestPoint( - const pcl::PointCloud & pointcloud, const geometry_msgs::msg::Pose & base_pose, - pcl::PointXYZ * nearest_collision_point, rclcpp::Time * nearest_collision_point_time) -{ - double min_norm = 0.0; - bool is_init = false; - const auto yaw = getRPY(base_pose).z; - const Eigen::Vector2d base_pose_vec(std::cos(yaw), std::sin(yaw)); - - for (size_t i = 0; i < pointcloud.size(); ++i) { - const Eigen::Vector2d pointcloud_vec( - pointcloud.at(i).x - base_pose.position.x, pointcloud.at(i).y - base_pose.position.y); - double norm = base_pose_vec.dot(pointcloud_vec); - if (norm < min_norm || !is_init) { - min_norm = norm; - *nearest_collision_point = pointcloud.at(i); - *nearest_collision_point_time = pcl_conversions::fromPCL(pointcloud.header).stamp; - is_init = true; - } - } -} - -void ObstacleStopPlannerNode::getLateralNearestPoint( - const pcl::PointCloud & pointcloud, const geometry_msgs::msg::Pose & base_pose, - pcl::PointXYZ * lateral_nearest_point, double * deviation) -{ - double min_norm = std::numeric_limits::max(); - const auto yaw = getRPY(base_pose).z; - const Eigen::Vector2d base_pose_vec(std::cos(yaw), std::sin(yaw)); - for (size_t i = 0; i < pointcloud.size(); ++i) { - const Eigen::Vector2d pointcloud_vec( - pointcloud.at(i).x - base_pose.position.x, pointcloud.at(i).y - base_pose.position.y); - double norm = - std::abs(base_pose_vec.x() * pointcloud_vec.y() - base_pose_vec.y() * pointcloud_vec.x()); - if (norm < min_norm) { - min_norm = norm; - *lateral_nearest_point = pointcloud.at(i); - } - } - *deviation = min_norm; -} - -geometry_msgs::msg::Pose ObstacleStopPlannerNode::getVehicleCenterFromBase( - const geometry_msgs::msg::Pose & base_pose, const VehicleInfo & vehicle_info) -{ - const auto & i = vehicle_info; - const auto yaw = getRPY(base_pose).z; - - geometry_msgs::msg::Pose center_pose; - center_pose.position.x = - base_pose.position.x + (i.vehicle_length_m / 2.0 - i.rear_overhang_m) * std::cos(yaw); - center_pose.position.y = - base_pose.position.y + (i.vehicle_length_m / 2.0 - i.rear_overhang_m) * std::sin(yaw); - center_pose.position.z = base_pose.position.z; - center_pose.orientation = base_pose.orientation; - return center_pose; -} - void ObstacleStopPlannerNode::setExternalVelocityLimit() { const auto & p = slow_down_param_; diff --git a/planning/obstacle_stop_planner/src/planner_utils.cpp b/planning/obstacle_stop_planner/src/planner_utils.cpp new file mode 100644 index 0000000000000..f990d28ef02f0 --- /dev/null +++ b/planning/obstacle_stop_planner/src/planner_utils.cpp @@ -0,0 +1,713 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "obstacle_stop_planner/planner_utils.hpp" + +#include +#include + +#include + +#include + +#include + +namespace motion_planning +{ + +using motion_utils::findFirstNearestIndexWithSoftConstraints; +using motion_utils::findFirstNearestSegmentIndexWithSoftConstraints; +using tier4_autoware_utils::calcDistance2d; +using tier4_autoware_utils::getRPY; + +bool validCheckDecelPlan( + const double v_end, const double a_end, const double v_target, const double a_target, + const double v_margin, const double a_margin) +{ + const double v_min = v_target - std::abs(v_margin); + const double v_max = v_target + std::abs(v_margin); + const double a_min = a_target - std::abs(a_margin); + const double a_max = a_target + std::abs(a_margin); + + if (v_end < v_min || v_max < v_end) { + RCLCPP_DEBUG_STREAM( + rclcpp::get_logger("validCheckDecelPlan"), + "[validCheckDecelPlan] valid check error! v_target = " << v_target << ", v_end = " << v_end); + return false; + } + if (a_end < a_min || a_max < a_end) { + RCLCPP_DEBUG_STREAM( + rclcpp::get_logger("validCheckDecelPlan"), + "[validCheckDecelPlan] valid check error! a_target = " << a_target << ", a_end = " << a_end); + return false; + } + + return true; +} + +/** + * @brief calculate distance until velocity is reached target velocity (TYPE1) + * @param (v0) current velocity [m/s] + * @param (vt) target velocity [m/s] + * @param (a0) current acceleration [m/ss] + * @param (am) minimum deceleration [m/ss] + * @param (ja) maximum jerk [m/sss] + * @param (jd) minimum jerk [m/sss] + * @param (t_min) duration of constant deceleration [s] + * @return moving distance until velocity is reached vt [m] + * @detail TODO(Satoshi Ota) + */ +boost::optional calcDecelDistPlanType1( + const double v0, const double vt, const double a0, const double am, const double ja, + const double jd, const double t_min) +{ + constexpr double epsilon = 1e-3; + + const double j1 = am < a0 ? jd : ja; + const double t1 = epsilon < (am - a0) / j1 ? (am - a0) / j1 : 0.0; + const double a1 = a0 + j1 * t1; + const double v1 = v0 + a0 * t1 + 0.5 * j1 * t1 * t1; + const double x1 = v0 * t1 + 0.5 * a0 * t1 * t1 + (1.0 / 6.0) * j1 * t1 * t1 * t1; + + const double t2 = epsilon < t_min ? t_min : 0.0; + const double a2 = a1; + const double v2 = v1 + a1 * t2; + const double x2 = x1 + v1 * t2 + 0.5 * a1 * t2 * t2; + + const double t3 = epsilon < (0.0 - am) / ja ? (0.0 - am) / ja : 0.0; + const double a3 = a2 + ja * t3; + const double v3 = v2 + a2 * t3 + 0.5 * ja * t3 * t3; + const double x3 = x2 + v2 * t3 + 0.5 * a2 * t3 * t3 + (1.0 / 6.0) * ja * t3 * t3 * t3; + + const double a_target = 0.0; + const double v_margin = 0.3; // [m/s] + const double a_margin = 0.1; // [m/s^2] + + if (!validCheckDecelPlan(v3, a3, vt, a_target, v_margin, a_margin)) { + return {}; + } + + return x3; +} + +/** + * @brief calculate distance until velocity is reached target velocity (TYPE2) + * @param (v0) current velocity [m/s] + * @param (vt) target velocity [m/s] + * @param (a0) current acceleration [m/ss] + * @param (am) minimum deceleration [m/ss] + * @param (ja) maximum jerk [m/sss] + * @param (jd) minimum jerk [m/sss] + * @return moving distance until velocity is reached vt [m] + * @detail TODO(Satoshi Ota) + */ +boost::optional calcDecelDistPlanType2( + const double v0, const double vt, const double a0, const double ja, const double jd) +{ + constexpr double epsilon = 1e-3; + + const double a1_square = (vt - v0 - 0.5 * (0.0 - a0) / jd * a0) * (2.0 * ja * jd / (ja - jd)); + const double a1 = -std::sqrt(a1_square); + + const double t1 = epsilon < (a1 - a0) / jd ? (a1 - a0) / jd : 0.0; + const double v1 = v0 + a0 * t1 + 0.5 * jd * t1 * t1; + const double x1 = v0 * t1 + 0.5 * a0 * t1 * t1 + (1.0 / 6.0) * jd * t1 * t1 * t1; + + const double t2 = epsilon < (0.0 - a1) / ja ? (0.0 - a1) / ja : 0.0; + const double a2 = a1 + ja * t2; + const double v2 = v1 + a1 * t2 + 0.5 * ja * t2 * t2; + const double x2 = x1 + v1 * t2 + 0.5 * a1 * t2 * t2 + (1.0 / 6.0) * ja * t2 * t2 * t2; + + const double a_target = 0.0; + const double v_margin = 0.3; + const double a_margin = 0.1; + + if (!validCheckDecelPlan(v2, a2, vt, a_target, v_margin, a_margin)) { + return {}; + } + + return x2; +} + +/** + * @brief calculate distance until velocity is reached target velocity (TYPE3) + * @param (v0) current velocity [m/s] + * @param (vt) target velocity [m/s] + * @param (a0) current acceleration [m/ss] + * @param (ja) maximum jerk [m/sss] + * @return moving distance until velocity is reached vt [m] + * @detail TODO(Satoshi Ota) + */ +boost::optional calcDecelDistPlanType3( + const double v0, const double vt, const double a0, const double ja) +{ + constexpr double epsilon = 1e-3; + + const double t_acc = (0.0 - a0) / ja; + + const double t1 = epsilon < t_acc ? t_acc : 0.0; + const double a1 = a0 + ja * t1; + const double v1 = v0 + a0 * t1 + 0.5 * ja * t1 * t1; + const double x1 = v0 * t1 + 0.5 * a0 * t1 * t1 + (1.0 / 6.0) * ja * t1 * t1 * t1; + + const double a_target = 0.0; + const double v_margin = 0.3; + const double a_margin = 0.1; + + if (!validCheckDecelPlan(v1, a1, vt, a_target, v_margin, a_margin)) { + return {}; + } + + return x1; +} + +boost::optional calcDecelDistWithJerkAndAccConstraints( + const double current_vel, const double target_vel, const double current_acc, const double acc_min, + const double jerk_acc, const double jerk_dec) +{ + constexpr double epsilon = 1e-3; + const double t_dec = + acc_min < current_acc ? (acc_min - current_acc) / jerk_dec : (acc_min - current_acc) / jerk_acc; + const double t_acc = (0.0 - acc_min) / jerk_acc; + const double t_min = (target_vel - current_vel - current_acc * t_dec - + 0.5 * jerk_dec * t_dec * t_dec - 0.5 * acc_min * t_acc) / + acc_min; + + // check if it is possible to decelerate to the target velocity + // by simply bringing the current acceleration to zero. + const auto is_decel_needed = + 0.5 * (0.0 - current_acc) / jerk_acc * current_acc > target_vel - current_vel; + + if (t_min > epsilon) { + return calcDecelDistPlanType1( + current_vel, target_vel, current_acc, acc_min, jerk_acc, jerk_dec, t_min); + } else if (is_decel_needed || current_acc > epsilon) { + return calcDecelDistPlanType2(current_vel, target_vel, current_acc, jerk_acc, jerk_dec); + } else { + return calcDecelDistPlanType3(current_vel, target_vel, current_acc, jerk_acc); + } + + return {}; +} + +boost::optional> calcFeasibleMarginAndVelocity( + const SlowDownParam & slow_down_param, const double dist_baselink_to_obstacle, + const double current_vel, const double current_acc) +{ + const auto & p = slow_down_param; + const auto & logger = rclcpp::get_logger("calcFeasibleMarginAndVelocity"); + constexpr double epsilon = 1e-4; + + if (current_vel < p.slow_down_velocity + epsilon) { + return std::make_pair(p.longitudinal_forward_margin, p.slow_down_velocity); + } + + for (double planning_jerk = p.jerk_start; planning_jerk > p.slow_down_min_jerk - epsilon; + planning_jerk += p.jerk_span) { + const double jerk_dec = planning_jerk; + const double jerk_acc = std::abs(planning_jerk); + + const auto planning_dec = + planning_jerk > p.normal_min_jerk ? p.limit_min_acc : p.normal_min_acc; + const auto stop_dist = calcDecelDistWithJerkAndAccConstraints( + current_vel, p.slow_down_velocity, current_acc, planning_dec, jerk_acc, jerk_dec); + + if (!stop_dist) { + continue; + } + + if (stop_dist.get() + p.longitudinal_forward_margin < dist_baselink_to_obstacle) { + RCLCPP_DEBUG( + logger, "[found plan] dist:%-6.2f jerk:%-6.2f margin:%-6.2f v0:%-6.2f vt:%-6.2f", + stop_dist.get(), planning_jerk, p.longitudinal_forward_margin, p.slow_down_velocity, + current_vel); + return std::make_pair(p.longitudinal_forward_margin, p.slow_down_velocity); + } + } + + { + const double jerk_dec = p.slow_down_min_jerk; + const double jerk_acc = std::abs(p.slow_down_min_jerk); + + const auto planning_dec = + p.slow_down_min_jerk > p.normal_min_jerk ? p.limit_min_acc : p.normal_min_acc; + const auto stop_dist = calcDecelDistWithJerkAndAccConstraints( + current_vel, p.slow_down_velocity, current_acc, planning_dec, jerk_acc, jerk_dec); + + if (!stop_dist) { + return {}; + } + + if (stop_dist.get() + p.min_longitudinal_forward_margin < dist_baselink_to_obstacle) { + const auto planning_margin = dist_baselink_to_obstacle - stop_dist.get(); + RCLCPP_DEBUG( + logger, "[relax margin] dist:%-6.2f jerk:%-6.2f margin:%-6.2f v0:%-6.2f vt%-6.2f", + stop_dist.get(), p.slow_down_min_jerk, planning_margin, p.slow_down_velocity, current_vel); + return std::make_pair(planning_margin, p.slow_down_velocity); + } + } + + RCLCPP_DEBUG(logger, "relax target slow down velocity"); + return {}; +} + +boost::optional> getForwardInsertPointFromBasePoint( + const size_t base_idx, const TrajectoryPoints & trajectory, const double margin) +{ + if (base_idx + 1 > trajectory.size()) { + return {}; + } + + if (margin < std::numeric_limits::epsilon()) { + return std::make_pair(base_idx, trajectory.at(base_idx)); + } + + double length_sum = 0.0; + double length_residual = 0.0; + + for (size_t i = base_idx; i < trajectory.size() - 1; ++i) { + const auto & p_front = trajectory.at(i); + const auto & p_back = trajectory.at(i + 1); + + length_sum += calcDistance2d(p_front, p_back); + length_residual = length_sum - margin; + + if (length_residual >= std::numeric_limits::epsilon()) { + const auto p_insert = getBackwardPointFromBasePoint(p_back, p_front, p_back, length_residual); + + // p_front(trajectory.points.at(i)) is insert base point + return std::make_pair(i, p_insert); + } + } + + if (length_residual < std::numeric_limits::epsilon()) { + return std::make_pair(trajectory.size() - 1, trajectory.back()); + } + + return {}; +} + +boost::optional> getBackwardInsertPointFromBasePoint( + const size_t base_idx, const TrajectoryPoints & trajectory, const double margin) +{ + if (base_idx + 1 > trajectory.size()) { + return {}; + } + + if (margin < std::numeric_limits::epsilon()) { + return std::make_pair(base_idx, trajectory.at(base_idx)); + } + + double length_sum = 0.0; + double length_residual = 0.0; + + for (size_t i = base_idx; 0 < i; --i) { + const auto & p_front = trajectory.at(i - 1); + const auto & p_back = trajectory.at(i); + + length_sum += calcDistance2d(p_front, p_back); + length_residual = length_sum - margin; + + if (length_residual >= std::numeric_limits::epsilon()) { + const auto p_insert = + getBackwardPointFromBasePoint(p_front, p_back, p_front, length_residual); + + // p_front(trajectory.points.at(i-1)) is insert base point + return std::make_pair(i - 1, p_insert); + } + } + + if (length_residual < std::numeric_limits::epsilon()) { + return std::make_pair(size_t(0), trajectory.front()); + } + + return {}; +} + +boost::optional> findNearestFrontIndex( + const size_t start_idx, const TrajectoryPoints & trajectory, const Point & point) +{ + for (size_t i = start_idx; i < trajectory.size(); ++i) { + const auto & p_traj = trajectory.at(i).pose; + const auto yaw = getRPY(p_traj).z; + const Point2d p_traj_direction(std::cos(yaw), std::sin(yaw)); + const Point2d p_traj_to_target(point.x - p_traj.position.x, point.y - p_traj.position.y); + + const auto is_in_front_of_target_point = p_traj_direction.dot(p_traj_to_target) < 0.0; + const auto is_trajectory_end = i + 1 == trajectory.size(); + + if (is_in_front_of_target_point || is_trajectory_end) { + const auto dist_p_traj_to_target = p_traj_direction.normalized().dot(p_traj_to_target); + return std::make_pair(i, dist_p_traj_to_target); + } + } + + return {}; +} + +bool isInFrontOfTargetPoint(const Pose & pose, const Point & point) +{ + const auto yaw = getRPY(pose).z; + const Point2d pose_direction(std::cos(yaw), std::sin(yaw)); + const Point2d to_target(point.x - pose.position.x, point.y - pose.position.y); + + return pose_direction.dot(to_target) < 0.0; +} + +bool checkValidIndex(const Pose & p_base, const Pose & p_next, const Pose & p_target) +{ + const Point2d base2target( + p_target.position.x - p_base.position.x, p_target.position.y - p_base.position.y); + const Point2d target2next( + p_next.position.x - p_target.position.x, p_next.position.y - p_target.position.y); + return base2target.dot(target2next) > 0.0; +} + +std::string jsonDumpsPose(const Pose & pose) +{ + const std::string json_dumps_pose = + (boost::format( + R"({"position":{"x":%lf,"y":%lf,"z":%lf},"orientation":{"w":%lf,"x":%lf,"y":%lf,"z":%lf}})") % + pose.position.x % pose.position.y % pose.position.z % pose.orientation.w % pose.orientation.x % + pose.orientation.y % pose.orientation.z) + .str(); + return json_dumps_pose; +} + +DiagnosticStatus makeStopReasonDiag(const std::string stop_reason, const Pose & stop_pose) +{ + DiagnosticStatus stop_reason_diag; + KeyValue stop_reason_diag_kv; + stop_reason_diag.level = DiagnosticStatus::OK; + stop_reason_diag.name = "stop_reason"; + stop_reason_diag.message = stop_reason; + stop_reason_diag_kv.key = "stop_pose"; + stop_reason_diag_kv.value = jsonDumpsPose(stop_pose); + stop_reason_diag.values.push_back(stop_reason_diag_kv); + return stop_reason_diag; +} + +TrajectoryPoint getBackwardPointFromBasePoint( + const TrajectoryPoint & p_from, const TrajectoryPoint & p_to, const TrajectoryPoint & p_base, + const double backward_length) +{ + TrajectoryPoint output; + const double dx = p_to.pose.position.x - p_from.pose.position.x; + const double dy = p_to.pose.position.y - p_from.pose.position.y; + const double norm = std::hypot(dx, dy); + + output = p_base; + output.pose.position.x += backward_length * dx / norm; + output.pose.position.y += backward_length * dy / norm; + + return output; +} + +rclcpp::SubscriptionOptions createSubscriptionOptions(rclcpp::Node * node_ptr) +{ + rclcpp::CallbackGroup::SharedPtr callback_group = + node_ptr->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + + auto sub_opt = rclcpp::SubscriptionOptions(); + sub_opt.callback_group = callback_group; + + return sub_opt; +} + +bool withinPolygon( + const std::vector & cv_polygon, const double radius, const Point2d & prev_point, + const Point2d & next_point, PointCloud::Ptr candidate_points_ptr, + PointCloud::Ptr within_points_ptr) +{ + Polygon2d boost_polygon; + bool find_within_points = false; + for (const auto & point : cv_polygon) { + boost_polygon.outer().push_back(bg::make(point.x, point.y)); + } + boost_polygon.outer().push_back(bg::make(cv_polygon.front().x, cv_polygon.front().y)); + + for (size_t j = 0; j < candidate_points_ptr->size(); ++j) { + Point2d point(candidate_points_ptr->at(j).x, candidate_points_ptr->at(j).y); + if (bg::distance(prev_point, point) < radius || bg::distance(next_point, point) < radius) { + if (bg::within(point, boost_polygon)) { + within_points_ptr->push_back(candidate_points_ptr->at(j)); + find_within_points = true; + } + } + } + return find_within_points; +} + +bool convexHull( + const std::vector & pointcloud, std::vector & polygon_points) +{ + cv::Point2d centroid; + centroid.x = 0; + centroid.y = 0; + for (const auto & point : pointcloud) { + centroid.x += point.x; + centroid.y += point.y; + } + centroid.x = centroid.x / static_cast(pointcloud.size()); + centroid.y = centroid.y / static_cast(pointcloud.size()); + + std::vector normalized_pointcloud; + std::vector normalized_polygon_points; + for (const auto & p : pointcloud) { + normalized_pointcloud.emplace_back( + cv::Point((p.x - centroid.x) * 1000.0, (p.y - centroid.y) * 1000.0)); + } + cv::convexHull(normalized_pointcloud, normalized_polygon_points); + + for (const auto & p : normalized_polygon_points) { + cv::Point2d polygon_point; + polygon_point.x = (p.x / 1000.0 + centroid.x); + polygon_point.y = (p.y / 1000.0 + centroid.y); + polygon_points.push_back(polygon_point); + } + return true; +} + +void createOneStepPolygon( + const Pose & base_step_pose, const Pose & next_step_pose, std::vector & polygon, + const VehicleInfo & vehicle_info, const double expand_width) +{ + std::vector one_step_move_vehicle_corner_points; + + const auto & i = vehicle_info; + const auto & front_m = i.max_longitudinal_offset_m; + const auto & width_m = i.vehicle_width_m / 2.0 + expand_width; + const auto & back_m = i.rear_overhang_m; + // start step + { + const auto yaw = getRPY(base_step_pose).z; + one_step_move_vehicle_corner_points.emplace_back(cv::Point2d( + base_step_pose.position.x + std::cos(yaw) * front_m - std::sin(yaw) * width_m, + base_step_pose.position.y + std::sin(yaw) * front_m + std::cos(yaw) * width_m)); + one_step_move_vehicle_corner_points.emplace_back(cv::Point2d( + base_step_pose.position.x + std::cos(yaw) * front_m - std::sin(yaw) * -width_m, + base_step_pose.position.y + std::sin(yaw) * front_m + std::cos(yaw) * -width_m)); + one_step_move_vehicle_corner_points.emplace_back(cv::Point2d( + base_step_pose.position.x + std::cos(yaw) * -back_m - std::sin(yaw) * -width_m, + base_step_pose.position.y + std::sin(yaw) * -back_m + std::cos(yaw) * -width_m)); + one_step_move_vehicle_corner_points.emplace_back(cv::Point2d( + base_step_pose.position.x + std::cos(yaw) * -back_m - std::sin(yaw) * width_m, + base_step_pose.position.y + std::sin(yaw) * -back_m + std::cos(yaw) * width_m)); + } + // next step + { + const auto yaw = getRPY(next_step_pose).z; + one_step_move_vehicle_corner_points.emplace_back(cv::Point2d( + next_step_pose.position.x + std::cos(yaw) * front_m - std::sin(yaw) * width_m, + next_step_pose.position.y + std::sin(yaw) * front_m + std::cos(yaw) * width_m)); + one_step_move_vehicle_corner_points.emplace_back(cv::Point2d( + next_step_pose.position.x + std::cos(yaw) * front_m - std::sin(yaw) * -width_m, + next_step_pose.position.y + std::sin(yaw) * front_m + std::cos(yaw) * -width_m)); + one_step_move_vehicle_corner_points.emplace_back(cv::Point2d( + next_step_pose.position.x + std::cos(yaw) * -back_m - std::sin(yaw) * -width_m, + next_step_pose.position.y + std::sin(yaw) * -back_m + std::cos(yaw) * -width_m)); + one_step_move_vehicle_corner_points.emplace_back(cv::Point2d( + next_step_pose.position.x + std::cos(yaw) * -back_m - std::sin(yaw) * width_m, + next_step_pose.position.y + std::sin(yaw) * -back_m + std::cos(yaw) * width_m)); + } + convexHull(one_step_move_vehicle_corner_points, polygon); +} + +void insertStopPoint( + const StopPoint & stop_point, TrajectoryPoints & output, DiagnosticStatus & stop_reason_diag) +{ + const auto traj_end_idx = output.size() - 1; + const auto & stop_idx = stop_point.index; + + const auto & p_base = output.at(stop_idx); + const auto & p_next = output.at(std::min(stop_idx + 1, traj_end_idx)); + const auto & p_insert = stop_point.point; + + constexpr double min_dist = 1e-3; + + const auto is_p_base_and_p_insert_overlap = calcDistance2d(p_base, p_insert) < min_dist; + const auto is_p_next_and_p_insert_overlap = calcDistance2d(p_next, p_insert) < min_dist; + const auto is_valid_index = checkValidIndex(p_base.pose, p_next.pose, p_insert.pose); + + auto update_stop_idx = stop_idx; + + if (!is_p_base_and_p_insert_overlap && !is_p_next_and_p_insert_overlap && is_valid_index) { + // insert: start_idx and end_idx are shifted by one + output.insert(output.begin() + stop_idx + 1, p_insert); + update_stop_idx = std::min(update_stop_idx + 1, traj_end_idx); + } else if (is_p_next_and_p_insert_overlap) { + // not insert: p_insert is merged into p_next + update_stop_idx = std::min(update_stop_idx + 1, traj_end_idx); + } + + for (size_t i = update_stop_idx; i < output.size(); ++i) { + output.at(i).longitudinal_velocity_mps = 0.0; + } + + stop_reason_diag = makeStopReasonDiag("obstacle", p_insert.pose); +} + +TrajectoryPoint getExtendTrajectoryPoint( + const double extend_distance, const TrajectoryPoint & goal_point) +{ + tf2::Transform map2goal; + tf2::fromMsg(goal_point.pose, map2goal); + tf2::Transform local_extend_point; + local_extend_point.setOrigin(tf2::Vector3(extend_distance, 0.0, 0.0)); + tf2::Quaternion q; + q.setRPY(0, 0, 0); + local_extend_point.setRotation(q); + const auto map2extend_point = map2goal * local_extend_point; + Pose extend_pose; + tf2::toMsg(map2extend_point, extend_pose); + TrajectoryPoint extend_trajectory_point; + extend_trajectory_point.pose = extend_pose; + extend_trajectory_point.longitudinal_velocity_mps = goal_point.longitudinal_velocity_mps; + extend_trajectory_point.lateral_velocity_mps = goal_point.lateral_velocity_mps; + extend_trajectory_point.acceleration_mps2 = goal_point.acceleration_mps2; + return extend_trajectory_point; +} + +TrajectoryPoints decimateTrajectory( + const TrajectoryPoints & input, const double step_length, + std::map & index_map) +{ + TrajectoryPoints output{}; + + double trajectory_length_sum = 0.0; + double next_length = 0.0; + + for (int i = 0; i < static_cast(input.size()) - 1; ++i) { + const auto & p_front = input.at(i); + const auto & p_back = input.at(i + 1); + constexpr double epsilon = 1e-3; + + if (next_length <= trajectory_length_sum + epsilon) { + const auto p_interpolate = + getBackwardPointFromBasePoint(p_front, p_back, p_back, next_length - trajectory_length_sum); + output.push_back(p_interpolate); + + index_map.insert(std::make_pair(output.size() - 1, size_t(i))); + next_length += step_length; + continue; + } + + trajectory_length_sum += calcDistance2d(p_front, p_back); + } + if (!input.empty()) { + output.push_back(input.back()); + index_map.insert(std::make_pair(output.size() - 1, input.size() - 1)); + } + + return output; +} + +TrajectoryPoints extendTrajectory(const TrajectoryPoints & input, const double extend_distance) +{ + TrajectoryPoints output = input; + + if (extend_distance < std::numeric_limits::epsilon()) { + return output; + } + + const auto goal_point = input.back(); + double interpolation_distance = 0.1; + + double extend_sum = 0.0; + while (extend_sum <= (extend_distance - interpolation_distance)) { + const auto extend_trajectory_point = getExtendTrajectoryPoint(extend_sum, goal_point); + output.push_back(extend_trajectory_point); + extend_sum += interpolation_distance; + } + const auto extend_trajectory_point = getExtendTrajectoryPoint(extend_distance, goal_point); + output.push_back(extend_trajectory_point); + + return output; +} + +bool getSelfPose(const Header & header, const tf2_ros::Buffer & tf_buffer, Pose & self_pose) +{ + try { + TransformStamped transform; + transform = tf_buffer.lookupTransform( + header.frame_id, "base_link", header.stamp, rclcpp::Duration::from_seconds(0.1)); + self_pose.position.x = transform.transform.translation.x; + self_pose.position.y = transform.transform.translation.y; + self_pose.position.z = transform.transform.translation.z; + self_pose.orientation.x = transform.transform.rotation.x; + self_pose.orientation.y = transform.transform.rotation.y; + self_pose.orientation.z = transform.transform.rotation.z; + self_pose.orientation.w = transform.transform.rotation.w; + return true; + } catch (tf2::TransformException & ex) { + return false; + } +} + +void getNearestPoint( + const PointCloud & pointcloud, const Pose & base_pose, pcl::PointXYZ * nearest_collision_point, + rclcpp::Time * nearest_collision_point_time) +{ + double min_norm = 0.0; + bool is_init = false; + const auto yaw = getRPY(base_pose).z; + const Eigen::Vector2d base_pose_vec(std::cos(yaw), std::sin(yaw)); + + for (const auto & p : pointcloud) { + const Eigen::Vector2d pointcloud_vec(p.x - base_pose.position.x, p.y - base_pose.position.y); + double norm = base_pose_vec.dot(pointcloud_vec); + if (norm < min_norm || !is_init) { + min_norm = norm; + *nearest_collision_point = p; + *nearest_collision_point_time = pcl_conversions::fromPCL(pointcloud.header).stamp; + is_init = true; + } + } +} + +void getLateralNearestPoint( + const PointCloud & pointcloud, const Pose & base_pose, pcl::PointXYZ * lateral_nearest_point, + double * deviation) +{ + double min_norm = std::numeric_limits::max(); + const auto yaw = getRPY(base_pose).z; + const Eigen::Vector2d base_pose_vec(std::cos(yaw), std::sin(yaw)); + for (size_t i = 0; i < pointcloud.size(); ++i) { + const Eigen::Vector2d pointcloud_vec( + pointcloud.at(i).x - base_pose.position.x, pointcloud.at(i).y - base_pose.position.y); + double norm = + std::abs(base_pose_vec.x() * pointcloud_vec.y() - base_pose_vec.y() * pointcloud_vec.x()); + if (norm < min_norm) { + min_norm = norm; + *lateral_nearest_point = pointcloud.at(i); + } + } + *deviation = min_norm; +} + +Pose getVehicleCenterFromBase(const Pose & base_pose, const VehicleInfo & vehicle_info) +{ + const auto & i = vehicle_info; + const auto yaw = getRPY(base_pose).z; + + Pose center_pose; + center_pose.position.x = + base_pose.position.x + (i.vehicle_length_m / 2.0 - i.rear_overhang_m) * std::cos(yaw); + center_pose.position.y = + base_pose.position.y + (i.vehicle_length_m / 2.0 - i.rear_overhang_m) * std::sin(yaw); + center_pose.position.z = base_pose.position.z; + center_pose.orientation = base_pose.orientation; + return center_pose; +} +} // namespace motion_planning diff --git a/planning/route_handler/src/route_handler.cpp b/planning/route_handler/src/route_handler.cpp index 621365eda0e7c..6106fa86c1f59 100644 --- a/planning/route_handler/src/route_handler.cpp +++ b/planning/route_handler/src/route_handler.cpp @@ -865,7 +865,19 @@ boost::optional RouteHandler::getLeftLanelet( lanelet::Lanelets RouteHandler::getRightOppositeLanelets( const lanelet::ConstLanelet & lanelet) const { - return lanelet_map_ptr_->laneletLayer.findUsages(lanelet.rightBound().invert()); + const auto opposite_candidate_lanelets = + lanelet_map_ptr_->laneletLayer.findUsages(lanelet.rightBound().invert()); + + lanelet::Lanelets opposite_lanelets; + for (const auto & candidate_lanelet : opposite_candidate_lanelets) { + if (candidate_lanelet.leftBound().id() == lanelet.rightBound().id()) { + continue; + } + + opposite_lanelets.push_back(candidate_lanelet); + } + + return opposite_lanelets; } lanelet::ConstLanelets RouteHandler::getAllLeftSharedLinestringLanelets( @@ -943,7 +955,19 @@ lanelet::ConstLanelets RouteHandler::getAllSharedLineStringLanelets( lanelet::Lanelets RouteHandler::getLeftOppositeLanelets(const lanelet::ConstLanelet & lanelet) const { - return lanelet_map_ptr_->laneletLayer.findUsages(lanelet.leftBound().invert()); + const auto opposite_candidate_lanelets = + lanelet_map_ptr_->laneletLayer.findUsages(lanelet.leftBound().invert()); + + lanelet::Lanelets opposite_lanelets; + for (const auto & candidate_lanelet : opposite_candidate_lanelets) { + if (candidate_lanelet.rightBound().id() == lanelet.leftBound().id()) { + continue; + } + + opposite_lanelets.push_back(candidate_lanelet); + } + + return opposite_lanelets; } lanelet::ConstLineString3d RouteHandler::getRightMostSameDirectionLinestring( diff --git a/sensing/gnss_poser/launch/gnss_poser.launch.xml b/sensing/gnss_poser/launch/gnss_poser.launch.xml index 5be5d6ae44b52..5f4ac9def3e04 100755 --- a/sensing/gnss_poser/launch/gnss_poser.launch.xml +++ b/sensing/gnss_poser/launch/gnss_poser.launch.xml @@ -33,6 +33,7 @@ + diff --git a/sensing/gnss_poser/src/gnss_poser_core.cpp b/sensing/gnss_poser/src/gnss_poser_core.cpp index d78b1cc8bd02f..6fce8756e13ac 100644 --- a/sensing/gnss_poser/src/gnss_poser_core.cpp +++ b/sensing/gnss_poser/src/gnss_poser_core.cpp @@ -31,7 +31,7 @@ GNSSPoser::GNSSPoser(const rclcpp::NodeOptions & node_options) gnss_frame_(declare_parameter("gnss_frame", "gnss")), gnss_base_frame_(declare_parameter("gnss_base_frame", "gnss_base_link")), map_frame_(declare_parameter("map_frame", "map")), - use_gnss_ins_orientation_(declare_parameter("use_gnss_heading", true)), + use_gnss_ins_orientation_(declare_parameter("use_gnss_ins_orientation", true)), plane_zone_(declare_parameter("plane_zone", 9)), msg_gnss_ins_orientation_stamped_( std::make_shared()) diff --git a/system/default_ad_api/package.xml b/system/default_ad_api/package.xml index 9826f5af66f1c..1cee944589391 100644 --- a/system/default_ad_api/package.xml +++ b/system/default_ad_api/package.xml @@ -11,8 +11,9 @@ autoware_cmake - autoware_ad_api_msgs autoware_ad_api_specs + autoware_adapi_v1_msgs + autoware_adapi_version_msgs component_interface_specs component_interface_utils rclcpp diff --git a/system/default_ad_api/script/web_server.py b/system/default_ad_api/script/web_server.py index ac2e4762cd80b..80a801c8fafa6 100755 --- a/system/default_ad_api/script/web_server.py +++ b/system/default_ad_api/script/web_server.py @@ -17,7 +17,7 @@ import sys from threading import Thread -from autoware_ad_api_msgs.srv import InterfaceVersion +from autoware_adapi_version_msgs.srv import InterfaceVersion import flask import rclpy from rclpy.node import Node diff --git a/system/default_ad_api/src/routing.cpp b/system/default_ad_api/src/routing.cpp index c09e483be1a28..a77258edb61a1 100644 --- a/system/default_ad_api/src/routing.cpp +++ b/system/default_ad_api/src/routing.cpp @@ -22,7 +22,7 @@ RoutingNode::RoutingNode(const rclcpp::NodeOptions & options) : Node("routing", const auto adaptor = component_interface_utils::NodeAdaptor(this); group_srv_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); adaptor.relay_message(pub_route_state_, sub_route_state_); - adaptor.relay_message(pub_route_, sub_route_); + // adaptor.relay_message(pub_route_, sub_route_); // TODO(Takagi, Isamu): temporary disabled adaptor.relay_service(cli_set_route_points_, srv_set_route_points_, group_srv_); adaptor.relay_service(cli_set_route_, srv_set_route_, group_srv_); adaptor.relay_service(cli_clear_route_, srv_clear_route_, group_srv_); diff --git a/system/default_ad_api/test/node/interface_version.py b/system/default_ad_api/test/node/interface_version.py index d5abd2924e24e..12c3918ef902a 100644 --- a/system/default_ad_api/test/node/interface_version.py +++ b/system/default_ad_api/test/node/interface_version.py @@ -12,7 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. -from autoware_ad_api_msgs.srv import InterfaceVersion +from autoware_adapi_version_msgs.srv import InterfaceVersion import rclpy import rclpy.node diff --git a/system/default_ad_api_helpers/ad_api_adaptors/package.xml b/system/default_ad_api_helpers/ad_api_adaptors/package.xml index 442a076a9c544..84b02be0799d1 100644 --- a/system/default_ad_api_helpers/ad_api_adaptors/package.xml +++ b/system/default_ad_api_helpers/ad_api_adaptors/package.xml @@ -11,8 +11,8 @@ autoware_cmake - autoware_ad_api_msgs autoware_ad_api_specs + autoware_adapi_v1_msgs component_interface_utils rclcpp tier4_localization_msgs diff --git a/system/default_ad_api_helpers/automatic_pose_initializer/package.xml b/system/default_ad_api_helpers/automatic_pose_initializer/package.xml index e6a351b392680..cff20de603f7f 100644 --- a/system/default_ad_api_helpers/automatic_pose_initializer/package.xml +++ b/system/default_ad_api_helpers/automatic_pose_initializer/package.xml @@ -11,8 +11,8 @@ autoware_cmake - autoware_ad_api_msgs autoware_ad_api_specs + autoware_adapi_v1_msgs component_interface_utils rclcpp diff --git a/system/system_error_monitor/config/diagnostic_aggregator/system.param.yaml b/system/system_error_monitor/config/diagnostic_aggregator/system.param.yaml index 68ef4b47552c8..bd39a73361b7c 100644 --- a/system/system_error_monitor/config/diagnostic_aggregator/system.param.yaml +++ b/system/system_error_monitor/config/diagnostic_aggregator/system.param.yaml @@ -134,6 +134,12 @@ contains: [": Network CRC Error"] timeout: 3.0 + ip_packet_reassembles_failed: + type: diagnostic_aggregator/GenericAnalyzer + path: ip_packet_reassembles_failed + contains: [": IP Packet Reassembles Failed"] + timeout: 3.0 + storage: type: diagnostic_aggregator/AnalyzerGroup path: storage @@ -144,6 +150,36 @@ contains: [": HDD Temperature"] timeout: 3.0 + recovered_error: + type: diagnostic_aggregator/GenericAnalyzer + path: recovered_error + contains: [": HDD RecoveredError"] + timeout: 3.0 + + read_data_rate: + type: diagnostic_aggregator/GenericAnalyzer + path: read_data_rate + contains: [": HDD ReadDataRate"] + timeout: 3.0 + + write_data_rate: + type: diagnostic_aggregator/GenericAnalyzer + path: write_data_rate + contains: [": HDD WriteDataRate"] + timeout: 3.0 + + read_iops: + type: diagnostic_aggregator/GenericAnalyzer + path: read_iops + contains: [": HDD ReadIOPS"] + timeout: 3.0 + + write_iops: + type: diagnostic_aggregator/GenericAnalyzer + path: write_iops + contains: [": HDD WriteIOPS"] + timeout: 3.0 + usage: type: diagnostic_aggregator/GenericAnalyzer path: usage @@ -162,6 +198,12 @@ contains: [": HDD TotalDataWritten"] timeout: 3.0 + connection: + type: diagnostic_aggregator/GenericAnalyzer + path: connection + contains: [": HDD Connection"] + timeout: 3.0 + process: type: diagnostic_aggregator/AnalyzerGroup path: process diff --git a/system/system_error_monitor/package.xml b/system/system_error_monitor/package.xml index d98b62051d216..be25f799af457 100644 --- a/system/system_error_monitor/package.xml +++ b/system/system_error_monitor/package.xml @@ -5,6 +5,7 @@ 0.1.1 The system_error_monitor package in ROS2 Kenji Miyake + Fumihito Ito Apache License 2.0 ament_cmake_auto diff --git a/system/system_monitor/README.md b/system/system_monitor/README.md index 1eeffa58154dd..c30709d5cffea 100644 --- a/system/system_monitor/README.md +++ b/system/system_monitor/README.md @@ -53,29 +53,36 @@ Every topic is published in 1 minute interval. [Usage] ✓：Supported, -：Not supported -| Node | Message | Intel | arm64(tegra) | arm64(raspi) | Notes | -| --------------- | ---------------------- | :---: | :----------: | :----------: | ----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | -| CPU Monitor | CPU Temperature | ✓ | ✓ | ✓ | | -| | CPU Usage | ✓ | ✓ | ✓ | | -| | CPU Load Average | ✓ | ✓ | ✓ | | -| | CPU Thermal Throttling | ✓ | - | ✓ | | -| | CPU Frequency | ✓ | ✓ | ✓ | Notification of frequency only, normally error not generated. | -| HDD Monitor | HDD Temperature | ✓ | ✓ | ✓ | | -| | HDD PowerOnHours | ✓ | ✓ | ✓ | | -| | HDD TotalDataWritten | ✓ | ✓ | ✓ | | -| | HDD Usage | ✓ | ✓ | ✓ | | -| Memory Monitor | Memory Usage | ✓ | ✓ | ✓ | | -| Net Monitor | Network Usage | ✓ | ✓ | ✓ | | -| | Network CRC Error | ✓ | ✓ | ✓ | Warning occurs when the number of CRC errors in the period reaches the threshold value. The number of CRC errors that occur is the same as the value that can be confirmed with the ip command. | -| NTP Monitor | NTP Offset | ✓ | ✓ | ✓ | | -| Process Monitor | Tasks Summary | ✓ | ✓ | ✓ | | -| | High-load Proc[0-9] | ✓ | ✓ | ✓ | | -| | High-mem Proc[0-9] | ✓ | ✓ | ✓ | | -| GPU Monitor | GPU Temperature | ✓ | ✓ | - | | -| | GPU Usage | ✓ | ✓ | - | | -| | GPU Memory Usage | ✓ | - | - | | -| | GPU Thermal Throttling | ✓ | - | - | | -| | GPU Frequency | ✓ | ✓ | - | For Intel platform, monitor whether current GPU clock is supported by the GPU. | +| Node | Message | Intel | arm64(tegra) | arm64(raspi) | Notes | +| --------------- | ---------------------------- | :---: | :----------: | :----------: | ----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| CPU Monitor | CPU Temperature | ✓ | ✓ | ✓ | | +| | CPU Usage | ✓ | ✓ | ✓ | | +| | CPU Load Average | ✓ | ✓ | ✓ | | +| | CPU Thermal Throttling | ✓ | - | ✓ | | +| | CPU Frequency | ✓ | ✓ | ✓ | Notification of frequency only, normally error not generated. | +| HDD Monitor | HDD Temperature | ✓ | ✓ | ✓ | | +| | HDD PowerOnHours | ✓ | ✓ | ✓ | | +| | HDD TotalDataWritten | ✓ | ✓ | ✓ | | +| | HDD RecoveredError | ✓ | ✓ | ✓ | | +| | HDD Usage | ✓ | ✓ | ✓ | | +| | HDD ReadDataRate | ✓ | ✓ | ✓ | | +| | HDD WriteDataRate | ✓ | ✓ | ✓ | | +| | HDD ReadIOPS | ✓ | ✓ | ✓ | | +| | HDD WriteIOPS | ✓ | ✓ | ✓ | | +| | HDD Connection | ✓ | ✓ | ✓ | | +| Memory Monitor | Memory Usage | ✓ | ✓ | ✓ | | +| Net Monitor | Network Usage | ✓ | ✓ | ✓ | | +| | Network CRC Error | ✓ | ✓ | ✓ | Warning occurs when the number of CRC errors in the period reaches the threshold value. The number of CRC errors that occur is the same as the value that can be confirmed with the ip command. | +| | IP Packet Reassembles Failed | ✓ | ✓ | ✓ | | +| NTP Monitor | NTP Offset | ✓ | ✓ | ✓ | | +| Process Monitor | Tasks Summary | ✓ | ✓ | ✓ | | +| | High-load Proc[0-9] | ✓ | ✓ | ✓ | | +| | High-mem Proc[0-9] | ✓ | ✓ | ✓ | | +| GPU Monitor | GPU Temperature | ✓ | ✓ | - | | +| | GPU Usage | ✓ | ✓ | - | | +| | GPU Memory Usage | ✓ | - | - | | +| | GPU Thermal Throttling | ✓ | - | - | | +| | GPU Frequency | ✓ | ✓ | - | For Intel platform, monitor whether current GPU clock is supported by the GPU. | ## ROS parameters diff --git a/system/system_monitor/config/hdd_monitor.param.yaml b/system/system_monitor/config/hdd_monitor.param.yaml index 70f8dc5ffa13f..d818d848be801 100644 --- a/system/system_monitor/config/hdd_monitor.param.yaml +++ b/system/system_monitor/config/hdd_monitor.param.yaml @@ -5,10 +5,19 @@ disks: # Until multi type lists are allowed, name N the disks as disk0...disk{N-1} disk0: name: / + temp_attribute_id: 0xC2 temp_warn: 55.0 temp_error: 70.0 + power_on_hours_attribute_id: 0x09 power_on_hours_warn: 3000000 + total_data_written_attribute_id: 0xF1 total_data_written_warn: 4915200 # =150TB (1unit=32MB) total_data_written_safety_factor: 0.05 + recovered_error_attribute_id: 0xC3 + recovered_error_warn: 1 free_warn: 5120 # MB(8hour) free_error: 100 # MB(last 1 minute) + read_data_rate_warn: 360.0 # MB/s + write_data_rate_warn: 103.5 # MB/s + read_iops_warn: 63360.0 # IOPS + write_iops_warn: 24120.0 # IOPS diff --git a/system/system_monitor/config/net_monitor.param.yaml b/system/system_monitor/config/net_monitor.param.yaml index 686ee349b0765..cb7e1b48380a3 100644 --- a/system/system_monitor/config/net_monitor.param.yaml +++ b/system/system_monitor/config/net_monitor.param.yaml @@ -5,3 +5,5 @@ monitor_program: "greengrass" crc_error_check_duration: 1 crc_error_count_threshold: 1 + reassembles_failed_check_duration: 1 + reassembles_failed_check_count: 1 diff --git a/system/system_monitor/docs/ros_parameters.md b/system/system_monitor/docs/ros_parameters.md index 044c1eb10a5d0..a5bb41cb7c769 100644 --- a/system/system_monitor/docs/ros_parameters.md +++ b/system/system_monitor/docs/ros_parameters.md @@ -25,12 +25,20 @@ hdd_monitor: | Name | Type | Unit | Default | Notes | | :------------------------------- | :----: | :---------------: | :-----: | :--------------------------------------------------------------------------------- | | name | string | n/a | none | The disk name to monitor temperature. (e.g. /dev/sda) | +| temp_attribute_id | int | n/a | 0xC2 | S.M.A.R.T attribute ID of temperature. | | temp_warn | float | DegC | 55.0 | Generates warning when HDD temperature reaches a specified value or higher. | | temp_error | float | DegC | 70.0 | Generates error when HDD temperature reaches a specified value or higher. | +| power_on_hours_attribute_id | int | n/a | 0x09 | S.M.A.R.T attribute ID of power-on hours. | | power_on_hours_warn | int | Hour | 3000000 | Generates warning when HDD power-on hours reaches a specified value or higher. | | total_data_written_attribute_id | int | n/a | 0xF1 | S.M.A.R.T attribute ID of total data written. | | total_data_written_warn | int | depends on device | 4915200 | Generates warning when HDD total data written reaches a specified value or higher. | | total_data_written_safety_factor | int | %(1e-2) | 0.05 | Safety factor of HDD total data written. | +| recovered_error_attribute_id | int | n/a | 0xC3 | S.M.A.R.T attribute ID of recovered error. | +| recovered_error_warn | int | n/a | 1 | Generates warning when HDD recovered error reaches a specified value or higher. | +| read_data_rate_warn | float | MB/s | 360.0 | Generates warning when HDD read data rate reaches a specified value or higher. | +| write_data_rate_warn | float | MB/s | 103.5 | Generates warning when HDD write data rate reaches a specified value or higher. | +| read_iops_warn | float | IOPS | 63360.0 | Generates warning when HDD read IOPS reaches a specified value or higher. | +| write_iops_warn | float | IOPS | 24120.0 | Generates warning when HDD write IOPS reaches a specified value or higher. | hdd_monitor: @@ -53,12 +61,14 @@ mem_monitor: net_monitor: -| Name | Type | Unit | Default | Notes | -| :------------------------ | :----------: | :-----: | :-----: | :-------------------------------------------------------------------------------------------------------------- | -| devices | list[string] | n/a | none | The name of network interface to monitor. (e.g. eth0, \* for all network interfaces) | -| usage_warn | float | %(1e-2) | 0.95 | Generates warning when network usage reaches a specified value or higher. | -| crc_error_check_duration | int | sec | 1 | CRC error check duration. | -| crc_error_count_threshold | int | n/a | 1 | Generates warning when count of CRC errors during CRC error check duration reaches a specified value or higher. | +| Name | Type | Unit | Default | Notes | +| :-------------------------------- | :----------: | :-----: | :-----: | :--------------------------------------------------------------------------------------------------------------------------------------------------- | +| devices | list[string] | n/a | none | The name of network interface to monitor. (e.g. eth0, \* for all network interfaces) | +| usage_warn | float | %(1e-2) | 0.95 | Generates warning when network usage reaches a specified value or higher. | +| crc_error_check_duration | int | sec | 1 | CRC error check duration. | +| crc_error_count_threshold | int | n/a | 1 | Generates warning when count of CRC errors during CRC error check duration reaches a specified value or higher. | +| reassembles_failed_check_duration | int | sec | 1 | IP packet reassembles failed check duration. | +| reassembles_failed_check_count | int | n/a | 1 | Generates warning when count of IP packet reassembles failed during IP packet reassembles failed check duration reaches a specified value or higher. | ## NTP Monitor diff --git a/system/system_monitor/docs/topics_hdd_monitor.md b/system/system_monitor/docs/topics_hdd_monitor.md index 4ef37b6ae5207..31301177f38f1 100644 --- a/system/system_monitor/docs/topics_hdd_monitor.md +++ b/system/system_monitor/docs/topics_hdd_monitor.md @@ -14,13 +14,13 @@ [values] -| key | value (example) | -| ---------------------- | -------------------------- | -| HDD [0-9]: status | OK / hot / critical hot | -| HDD [0-9]: name | /dev/nvme0 | -| HDD [0-9]: model | SAMSUNG MZVLB1T0HBLR-000L7 | -| HDD [0-9]: serial | S4EMNF0M820682 | -| HDD [0-9]: temperature | 37.0 DegC | +| key | value (example) | +| ---------------------- | ---------------------------- | +| HDD [0-9]: status | OK / hot / critical hot | +| HDD [0-9]: name | /dev/nvme0 | +| HDD [0-9]: model | SAMSUNG MZVLB1T0HBLR-000L7 | +| HDD [0-9]: serial | S4EMNF0M820682 | +| HDD [0-9]: temperature | 37.0 DegC
not available | ## HDD PowerOnHours @@ -35,13 +35,13 @@ [values] -| key | value (example) | -| ------------------------- | ----------------------- | -| HDD [0-9]: status | OK / lifetime limit | -| HDD [0-9]: name | /dev/nvme0 | -| HDD [0-9]: model | PHISON PS5012-E12S-512G | -| HDD [0-9]: serial | FB590709182505050767 | -| HDD [0-9]: power on hours | 4834 Hours | +| key | value (example) | +| ------------------------- | ----------------------------- | +| HDD [0-9]: status | OK / lifetime limit | +| HDD [0-9]: name | /dev/nvme0 | +| HDD [0-9]: model | PHISON PS5012-E12S-512G | +| HDD [0-9]: serial | FB590709182505050767 | +| HDD [0-9]: power on hours | 4834 Hours
not available | ## HDD TotalDataWritten @@ -64,6 +64,27 @@ | HDD [0-9]: serial | FB590709182505050767 | | HDD [0-9]: total data written | 146295330
not available | +## HDD RecoveredError + +/diagnostics/hdd_monitor: HDD RecoveredError + +[summary] + +| level | message | +| ----- | -------------------- | +| OK | OK | +| WARN | high soft error rate | + +[values] + +| key | value (example) | +| -------------------------- | ------------------------- | +| HDD [0-9]: status | OK / high soft error rate | +| HDD [0-9]: name | /dev/nvme0 | +| HDD [0-9]: model | PHISON PS5012-E12S-512G | +| HDD [0-9]: serial | FB590709182505050767 | +| HDD [0-9]: recovered error | 0
not available | + ## HDD Usage /diagnostics/hdd_monitor: HDD Usage @@ -87,3 +108,98 @@ | HDD [0-9]: avail | 749G | | HDD [0-9]: use | 69% | | HDD [0-9]: mounted on | / | + +## HDD ReadDataRate + +/diagnostics/hdd_monitor: HDD ReadDataRate + +[summary] + +| level | message | +| ----- | ---------------------- | +| OK | OK | +| WARN | high data rate of read | + +[values] + +| key | value (example) | +| ---------------------------- | --------------------------- | +| HDD [0-9]: status | OK / high data rate of read | +| HDD [0-9]: name | /dev/nvme0 | +| HDD [0-9]: data rate of read | 0.00 MB/s | + +## HDD WriteDataRate + +/diagnostics/hdd_monitor: HDD WriteDataRate + +[summary] + +| level | message | +| ----- | ----------------------- | +| OK | OK | +| WARN | high data rate of write | + +[values] + +| key | value (example) | +| ----------------------------- | ---------------------------- | +| HDD [0-9]: status | OK / high data rate of write | +| HDD [0-9]: name | /dev/nvme0 | +| HDD [0-9]: data rate of write | 0.00 MB/s | + +## HDD ReadIOPS + +/diagnostics/hdd_monitor: HDD ReadIOPS + +[summary] + +| level | message | +| ----- | ----------------- | +| OK | OK | +| WARN | high IOPS of read | + +[values] + +| key | value (example) | +| ----------------------- | ---------------------- | +| HDD [0-9]: status | OK / high IOPS of read | +| HDD [0-9]: name | /dev/nvme0 | +| HDD [0-9]: IOPS of read | 0.00 IOPS | + +## HDD WriteIOPS + +/diagnostics/hdd_monitor: HDD WriteIOPS + +[summary] + +| level | message | +| ----- | ------------------ | +| OK | OK | +| WARN | high IOPS of write | + +[values] + +| key | value (example) | +| ------------------------ | ----------------------- | +| HDD [0-9]: status | OK / high IOPS of write | +| HDD [0-9]: name | /dev/nvme0 | +| HDD [0-9]: IOPS of write | 0.00 IOPS | + +## HDD Connection + +/diagnostics/hdd_monitor: HDD Connection + +[summary] + +| level | message | +| ----- | ------------- | +| OK | OK | +| WARN | not connected | + +[values] + +| key | value (example) | +| ---------------------- | ------------------ | +| HDD [0-9]: status | OK / not connected | +| HDD [0-9]: name | /dev/nvme0 | +| HDD [0-9]: mount point | / | diff --git a/system/system_monitor/docs/topics_net_monitor.md b/system/system_monitor/docs/topics_net_monitor.md index 261cede53de21..d223b359cdb08 100644 --- a/system/system_monitor/docs/topics_net_monitor.md +++ b/system/system_monitor/docs/topics_net_monitor.md @@ -81,3 +81,21 @@ | Network [0-9]: interface name | wlp82s0 | | Network [0-9]: total rx_crc_errors | 0 | | Network [0-9]: rx_crc_errors per unit time | 0 | + +## IP Packet Reassembles Failed + +/diagnostics/net_monitor: IP Packet Reassembles Failed + +[summary] + +| level | message | +| ----- | ------------------ | +| OK | OK | +| WARN | reassembles failed | + +[values] + +| key | value (example) | +| --------------------------------------- | --------------- | +| total packet reassembles failed | 0 | +| packet reassembles failed per unit time | 0 | diff --git a/system/system_monitor/include/hdd_reader/hdd_reader.hpp b/system/system_monitor/include/hdd_reader/hdd_reader.hpp index 44c52a491b9fe..2762dd8995b00 100644 --- a/system/system_monitor/include/hdd_reader/hdd_reader.hpp +++ b/system/system_monitor/include/hdd_reader/hdd_reader.hpp @@ -29,18 +29,24 @@ #include /** - * @brief ATA attribute IDs + * @brief Enumeration of Request ID to hdd_reader */ -enum class ATAAttributeIDs : uint8_t { TEMPERATURE = 0, POWER_ON_HOURS = 1, SIZE }; +enum HDDReaderRequestID { + GetHDDInfo, + UnmountDevice, +}; /** * @brief HDD device */ struct HDDDevice { - std::string name_; //!< @brief Device name + std::string name_; //!< @brief Device name + uint8_t temp_attribute_id_; //!< @brief S.M.A.R.T attribute ID of temperature + uint8_t power_on_hours_attribute_id_; //!< @brief S.M.A.R.T attribute ID of power on hours uint8_t - total_data_written_attribute_id_; //!< @brief S.M.A.R.T attribute ID of total data written + total_data_written_attribute_id_; //!< @brief S.M.A.R.T attribute ID of total data written + uint8_t recovered_error_attribute_id_; //!< @brief S.M.A.R.T attribute ID of recovered error /** * @brief Load or save data members. @@ -53,7 +59,10 @@ struct HDDDevice void serialize(archive & ar, const unsigned /*version*/) // NOLINT(runtime/references) { ar & name_; + ar & temp_attribute_id_; + ar & power_on_hours_attribute_id_; ar & total_data_written_attribute_id_; + ar & recovered_error_attribute_id_; } }; @@ -70,7 +79,11 @@ struct HDDInfo // in S.M.A.R.T. information. uint64_t power_on_hours_; //!< @brief power on hours count uint64_t total_data_written_; //!< @brief total data written + uint32_t recovered_error_; //!< @brief recovered error count + bool is_valid_temp_; //!< @brief whether temp_ is valid value + bool is_valid_power_on_hours_; //!< @brief whether power_on_hours_ is valid value bool is_valid_total_data_written_; //!< @brief whether total_data_written_ is valid value + bool is_valid_recovered_error_; //!< @brief whether recovered_error_ is valid value /** * @brief Load or save data members. @@ -88,7 +101,32 @@ struct HDDInfo ar & temp_; ar & power_on_hours_; ar & total_data_written_; + ar & recovered_error_; + ar & is_valid_temp_; + ar & is_valid_power_on_hours_; ar & is_valid_total_data_written_; + ar & is_valid_recovered_error_; + } +}; + +/** + * @brief unmount device information + */ +struct UnmountDeviceInfo +{ + std::string part_device_; //!< @brief partition device + + /** + * @brief Load or save data members. + * @param [inout] ar archive reference to load or save the serialized data members + * @param [in] version version for the archive + * @note NOLINT syntax is needed since this is an interface to serialization and + * used inside boost serialization. + */ + template + void serialize(archive & ar, const unsigned /*version*/) // NOLINT(runtime/references) + { + ar & part_device_; } }; diff --git a/system/system_monitor/include/system_monitor/hdd_monitor/hdd_monitor.hpp b/system/system_monitor/include/system_monitor/hdd_monitor/hdd_monitor.hpp index c739f466e95e0..6e7d010645fe6 100644 --- a/system/system_monitor/include/system_monitor/hdd_monitor/hdd_monitor.hpp +++ b/system/system_monitor/include/system_monitor/hdd_monitor/hdd_monitor.hpp @@ -33,14 +33,25 @@ */ struct HDDParam { - std::string device_; //!< @brief device + std::string part_device_; //!< @brief partition device + std::string disk_device_; //!< @brief disk device float temp_warn_; //!< @brief HDD temperature(DegC) to generate warning float temp_error_; //!< @brief HDD temperature(DegC) to generate error int power_on_hours_warn_; //!< @brief HDD power on hours to generate warning uint64_t total_data_written_warn_; //!< @brief HDD total data written to generate warning float total_data_written_safety_factor_; //!< @brief safety factor of HDD total data written - int free_warn_; //!< @brief HDD free space(MB) to generate warning - int free_error_; //!< @brief HDD free space(MB) to generate error + int recovered_error_warn_; //!< @brief HDD recovered error count to generate warning + int free_warn_; //!< @brief HDD free space(MB) to generate warning + int free_error_; //!< @brief HDD free space(MB) to generate error + float read_data_rate_warn_; //!< @brief HDD data rate(MB/s) of read to generate warning + float write_data_rate_warn_; //!< @brief HDD data rate(MB/s) of write to generate warning + float read_iops_warn_; //!< @brief HDD IOPS of read to generate warning + float write_iops_warn_; //!< @brief HDD IOPS of write to generate warning + uint8_t temp_attribute_id_; //!< @brief S.M.A.R.T attribute ID of temperature + uint8_t power_on_hours_attribute_id_; //!< @brief S.M.A.R.T attribute ID of power on hours + uint8_t + total_data_written_attribute_id_; //!< @brief S.M.A.R.T attribute ID of total data written + uint8_t recovered_error_attribute_id_; //!< @brief S.M.A.R.T attribute ID of recovered error HDDParam() : temp_warn_(55.0), @@ -48,8 +59,48 @@ struct HDDParam power_on_hours_warn_(3000000), total_data_written_warn_(4915200), total_data_written_safety_factor_(0.05), + recovered_error_warn_(1), free_warn_(5120), - free_error_(100) + free_error_(100), + read_data_rate_warn_(360.0), + write_data_rate_warn_(103.5), + read_iops_warn_(63360.0), + write_iops_warn_(24120.0), + temp_attribute_id_(0xC2), + power_on_hours_attribute_id_(0x09), + total_data_written_attribute_id_(0xF1), + recovered_error_attribute_id_(0xC3) + { + } +}; + +/** + * @brief statistics of sysfs device + */ +struct SysfsDevStat +{ + uint64_t rd_ios_; //!< @brief number of read operations issued to the device + uint64_t rd_sectors_; //!< @brief number of sectors read + uint64_t wr_ios_; //!< @brief number of write operations issued to the device + uint64_t wr_sectors_; //!< @brief number of sectors written + + SysfsDevStat() : rd_ios_(0), rd_sectors_(0), wr_ios_(0), wr_sectors_(0) {} +}; + +/** + * @brief statistics of HDD + */ +struct HDDStat +{ + std::string device_; //!< @brief device + std::string error_str_; //!< @brief error string + float read_data_rate_MBs_; //!< @brief data rate of read (MB/s) + float write_data_rate_MBs_; //!< @brief data rate of write (MB/s) + float read_iops_; //!< @brief IOPS of read + float write_iops_; //!< @brief IOPS of write + SysfsDevStat last_sfdevstat_; //!< @brief last statistics of sysfs device + + HDDStat() : read_data_rate_MBs_(0.0), write_data_rate_MBs_(0.0), read_iops_(0.0), write_iops_(0.0) { } }; @@ -61,6 +112,18 @@ enum class HDDSMARTInfoItem : uint32_t { TEMPERATURE = 0, POWER_ON_HOURS = 1, TOTAL_DATA_WRITTEN = 2, + RECOVERED_ERROR = 3, + SIZE +}; + +/** + * @brief HDD statistics items to check + */ +enum class HDDStatItem : uint32_t { + READ_DATA_RATE = 0, + WRITE_DATA_RATE = 1, + READ_IOPS = 2, + WRITE_IOPS = 3, SIZE }; @@ -103,9 +166,19 @@ class HDDMonitor : public rclcpp::Node void checkSMARTTotalDataWritten( diagnostic_updater::DiagnosticStatusWrapper & stat); // NOLINT(runtime/references) + /** + * @brief check HDD recovered error count + * @param [out] stat diagnostic message passed directly to diagnostic publish calls + * @note NOLINT syntax is needed since diagnostic_updater asks for a non-const reference + * to pass diagnostic message updated in this function to diagnostic publish calls. + */ + void checkSMARTRecoveredError( + diagnostic_updater::DiagnosticStatusWrapper & stat); // NOLINT(runtime/references) + /** * @brief check S.M.A.R.T. information * @param [out] stat diagnostic message passed directly to diagnostic publish calls + * @param [in] item S.M.A.R.T information item to be checked * @note NOLINT syntax is needed since diagnostic_updater asks for a non-const reference * to pass diagnostic message updated in this function to diagnostic publish calls. */ @@ -122,6 +195,62 @@ class HDDMonitor : public rclcpp::Node void checkUsage( diagnostic_updater::DiagnosticStatusWrapper & stat); // NOLINT(runtime/references) + /** + * @brief check HDD data rate of read + * @param [out] stat diagnostic message passed directly to diagnostic publish calls + * @note NOLINT syntax is needed since diagnostic_updater asks for a non-const reference + * to pass diagnostic message updated in this function to diagnostic publish calls. + */ + void checkReadDataRate( + diagnostic_updater::DiagnosticStatusWrapper & stat); // NOLINT(runtime/references) + + /** + * @brief check HDD data rate of write + * @param [out] stat diagnostic message passed directly to diagnostic publish calls + * @note NOLINT syntax is needed since diagnostic_updater asks for a non-const reference + * to pass diagnostic message updated in this function to diagnostic publish calls. + */ + void checkWriteDataRate( + diagnostic_updater::DiagnosticStatusWrapper & stat); // NOLINT(runtime/references) + + /** + * @brief check HDD IOPS of read + * @param [out] stat diagnostic message passed directly to diagnostic publish calls + * @note NOLINT syntax is needed since diagnostic_updater asks for a non-const reference + * to pass diagnostic message updated in this function to diagnostic publish calls. + */ + void checkReadIOPS( + diagnostic_updater::DiagnosticStatusWrapper & stat); // NOLINT(runtime/references) + + /** + * @brief check HDD IOPS of write + * @param [out] stat diagnostic message passed directly to diagnostic publish calls + * @note NOLINT syntax is needed since diagnostic_updater asks for a non-const reference + * to pass diagnostic message updated in this function to diagnostic publish calls. + */ + void checkWriteIOPS( + diagnostic_updater::DiagnosticStatusWrapper & stat); // NOLINT(runtime/references) + + /** + * @brief check HDD statistics + * @param [out] stat diagnostic message passed directly to diagnostic publish calls + * @param [in] item statistic item to be checked + * @note NOLINT syntax is needed since diagnostic_updater asks for a non-const reference + * to pass diagnostic message updated in this function to diagnostic publish calls. + */ + void checkStatistics( + diagnostic_updater::DiagnosticStatusWrapper & stat, + HDDStatItem item); // NOLINT(runtime/references) + + /** + * @brief check HDD connection + * @param [out] stat diagnostic message passed directly to diagnostic publish calls + * @note NOLINT syntax is needed since diagnostic_updater asks for a non-const reference + * to pass diagnostic message updated in this function to diagnostic publish calls. + */ + void checkConnection( + diagnostic_updater::DiagnosticStatusWrapper & stat); // NOLINT(runtime/references) + /** * @brief human readable size string to MB * @param [in] human readable size string @@ -151,6 +280,46 @@ class HDDMonitor : public rclcpp::Node */ void updateHDDInfoList(); + /** + * @brief start HDD transfer measurement + */ + void startHDDTransferMeasurement(); + + /** + * @brief update HDD statistics + */ + void updateHDDStatistics(); + + /** + * @brief get increment value of sysfs device stats per second + * @param [in] cur_val current value + * @param [in] last_val last value + * @param [in] duration_sec duration in seconds + * @return increment value + */ + double getIncreaseSysfsDeviceStatValuePerSec( + uint64_t cur_val, uint64_t last_val, double duration_sec); + + /** + * @brief read stats for current whole device using /sys/block/ directory + * @param [in] device device name + * @param [out] sfdevstat statistics of sysfs device + * @return result of success or failure + */ + int readSysfsDeviceStat(const std::string & device, SysfsDevStat & sfdevstat); + + /** + * @brief update HDD connections + */ + void updateHDDConnections(); + + /** + * @brief unmount device + * @param [in] device device name + * @return result of success or failure + */ + int unmountDevice(std::string & device); + diagnostic_updater::Updater updater_; //!< @brief Updater class which advertises to /diagnostics rclcpp::TimerBase::SharedPtr timer_; //!< @brief timer to get HDD information from HDDReader @@ -158,10 +327,15 @@ class HDDMonitor : public rclcpp::Node int hdd_reader_port_; //!< @brief port number to connect to hdd_reader std::map hdd_params_; //!< @brief list of error and warning levels - std::vector hdd_devices_; //!< @brief list of devices + std::map + hdd_connected_flags_; //!< @brief list of flag whether HDD is connected + std::map + initial_recovered_errors_; //!< @brief list of initial recovered error count + std::map hdd_stats_; //!< @brief list of HDD statistics //!< @brief diagnostic of connection diagnostic_updater::DiagnosticStatusWrapper connect_diag_; - HDDInfoList hdd_info_list_; //!< @brief list of HDD information + HDDInfoList hdd_info_list_; //!< @brief list of HDD information + rclcpp::Time last_hdd_stat_update_time_; //!< @brief last HDD statistics update time /** * @brief HDD SMART status messages @@ -173,6 +347,10 @@ class HDDMonitor : public rclcpp::Node {{DiagStatus::OK, "OK"}, {DiagStatus::WARN, "lifetime limit"}, {DiagStatus::ERROR, "unused"}}, // total data written {{DiagStatus::OK, "OK"}, {DiagStatus::WARN, "warranty period"}, {DiagStatus::ERROR, "unused"}}, + // recovered error count + {{DiagStatus::OK, "OK"}, + {DiagStatus::WARN, "high soft error rate"}, + {DiagStatus::ERROR, "unused"}}, }; /** @@ -182,6 +360,34 @@ class HDDMonitor : public rclcpp::Node {DiagStatus::OK, "OK"}, {DiagStatus::WARN, "low disk space"}, {DiagStatus::ERROR, "very low disk space"}}; + + /** + * @brief HDD statistics status messages + */ + const std::map stat_dicts_[static_cast(HDDStatItem::SIZE)] = { + // data rate of read + {{DiagStatus::OK, "OK"}, + {DiagStatus::WARN, "high data rate of read"}, + {DiagStatus::ERROR, "unused"}}, + // data rate of write + {{DiagStatus::OK, "OK"}, + {DiagStatus::WARN, "high data rate of write"}, + {DiagStatus::ERROR, "unused"}}, + // IOPS of read + {{DiagStatus::OK, "OK"}, + {DiagStatus::WARN, "high IOPS of read"}, + {DiagStatus::ERROR, "unused"}}, + // IOPS of write + {{DiagStatus::OK, "OK"}, + {DiagStatus::WARN, "high IOPS of write"}, + {DiagStatus::ERROR, "unused"}}, + }; + + /** + * @brief HDD connection status messages + */ + const std::map connection_dict_ = { + {DiagStatus::OK, "OK"}, {DiagStatus::WARN, "not connected"}, {DiagStatus::ERROR, "unused"}}; }; #endif // SYSTEM_MONITOR__HDD_MONITOR__HDD_MONITOR_HPP_ diff --git a/system/system_monitor/include/system_monitor/net_monitor/net_monitor.hpp b/system/system_monitor/include/system_monitor/net_monitor/net_monitor.hpp index 53f3e1250c9f0..3ddb078fa00e1 100644 --- a/system/system_monitor/include/system_monitor/net_monitor/net_monitor.hpp +++ b/system/system_monitor/include/system_monitor/net_monitor/net_monitor.hpp @@ -91,6 +91,15 @@ class NetMonitor : public rclcpp::Node void checkCrcError( diagnostic_updater::DiagnosticStatusWrapper & stat); // NOLINT(runtime/references) + /** + * @brief check IP packet reassembles failed + * @param [out] stat diagnostic message passed directly to diagnostic publish calls + * @note NOLINT syntax is needed since diagnostic_updater asks for a non-const reference + * to pass diagnostic message updated in this function to diagnostic publish calls. + */ + void checkReassemblesFailed( + diagnostic_updater::DiagnosticStatusWrapper & stat); // NOLINT(runtime/references) + /** * @brief get wireless speed * @param [in] ifa_name interface name @@ -168,6 +177,18 @@ class NetMonitor : public rclcpp::Node const NetworkInfo & net_info, int index, diagnostic_updater::DiagnosticStatusWrapper & stat, std::string & error_str); + /** + * @brief search column index of IP packet reassembles failed in /proc/net/snmp + */ + void searchReassemblesFailedColumnIndex(); + + /** + * @brief get IP packet reassembles failed + * @param [out] reassembles_failed IP packet reassembles failed + * @return execution result + */ + bool getReassemblesFailed(uint64_t & reassembles_failed); + diagnostic_updater::Updater updater_; //!< @brief Updater class which advertises to /diagnostics rclcpp::TimerBase::SharedPtr timer_; //!< @brief timer to get Network information @@ -191,11 +212,22 @@ class NetMonitor : public rclcpp::Node } crc_errors; std::map crc_errors_; //!< @brief list of CRC errors + std::deque + reassembles_failed_queue_; //!< @brief queue that holds count of IP packet reassembles failed + uint64_t last_reassembles_failed_; //!< @brief IP packet reassembles failed at the time of the + //!< last monitoring + std::string monitor_program_; //!< @brief nethogs monitor program name bool nethogs_all_; //!< @brief nethogs result all mode int traffic_reader_port_; //!< @brief port number to connect to traffic_reader unsigned int crc_error_check_duration_; //!< @brief CRC error check duration unsigned int crc_error_count_threshold_; //!< @brief CRC error count threshold + unsigned int + reassembles_failed_check_duration_; //!< @brief IP packet reassembles failed check duration + unsigned int + reassembles_failed_check_count_; //!< @brief IP packet reassembles failed check count threshold + unsigned int reassembles_failed_column_index_; //!< @brief column index of IP Reassembles failed + //!< in /proc/net/snmp /** * @brief Network usage status messages diff --git a/system/system_monitor/reader/hdd_reader/hdd_reader.cpp b/system/system_monitor/reader/hdd_reader/hdd_reader.cpp index f31efde2f5e01..dc5581b3430f1 100644 --- a/system/system_monitor/reader/hdd_reader/hdd_reader.cpp +++ b/system/system_monitor/reader/hdd_reader/hdd_reader.cpp @@ -24,9 +24,11 @@ #include #include #include +#include #include #include +#include #include #include #include @@ -279,16 +281,21 @@ int get_ata_SMARTData(int fd, HDDInfo * info, const HDDDevice & device) return errno; } - std::bitset(ATAAttributeIDs::SIZE)> found_flag; + info->is_valid_temp_ = false; + info->is_valid_power_on_hours_ = false; info->is_valid_total_data_written_ = false; + info->is_valid_recovered_error_ = false; // Retrieve S.M.A.R.T. Informations for (int i = 0; i < 30; ++i) { - if (data.attribute_entry_[i].attribute_id_ == 0xC2) { // Temperature - Device Internal + if (data.attribute_entry_[i].attribute_id_ == device.temp_attribute_id_) { // Temperature - + // Device Internal info->temp_ = static_cast(data.attribute_entry_[i].data_); - found_flag.set(static_cast(ATAAttributeIDs::TEMPERATURE)); - } else if (data.attribute_entry_[i].attribute_id_ == 0x09) { // Power-on Hours Count + info->is_valid_temp_ = true; + } else if ( + data.attribute_entry_[i].attribute_id_ == + device.power_on_hours_attribute_id_) { // Power-on Hours Count info->power_on_hours_ = data.attribute_entry_[i].data_; - found_flag.set(static_cast(ATAAttributeIDs::POWER_ON_HOURS)); + info->is_valid_power_on_hours_ = true; } else if ( data.attribute_entry_[i].attribute_id_ == device.total_data_written_attribute_id_) { // Total LBAs Written @@ -296,14 +303,15 @@ int get_ata_SMARTData(int fd, HDDInfo * info, const HDDDevice & device) (data.attribute_entry_[i].data_ | (static_cast(data.attribute_entry_[i].attribute_specific_) << 32)); info->is_valid_total_data_written_ = true; + } else if ( + data.attribute_entry_[i].attribute_id_ == + device.recovered_error_attribute_id_) { // Hardware ECC Recovered + info->recovered_error_ = data.attribute_entry_[i].data_; + info->is_valid_recovered_error_ = true; } } - if (found_flag.all()) { - return EXIT_SUCCESS; - } - - return ENOENT; + return EXIT_SUCCESS; } /** @@ -376,6 +384,7 @@ int get_nvme_SMARTData(int fd, HDDInfo * info) // Bytes 2:1 Composite Temperature // Convert kelvin to celsius unsigned int temperature = ((data[2] << 8) | data[1]) - 273; + info->is_valid_temp_ = true; info->temp_ = static_cast(temperature); // Bytes 63:48 Data Units Written @@ -389,13 +398,136 @@ int get_nvme_SMARTData(int fd, HDDInfo * info) info->total_data_written_ = *(reinterpret_cast(&data[48])); // Bytes 143:128 Power On Hours + info->is_valid_power_on_hours_ = true; info->power_on_hours_ = *(reinterpret_cast(&data[128])); + // NVMe S.M.A.R.T has no information of recovered error count + info->is_valid_recovered_error_ = false; + return EXIT_SUCCESS; } /** - * @brief check HDD temperature + * @brief get HDD information + * @param [in] boost::archive::text_iarchive object + * @param [out] boost::archive::text_oarchive object + * @return 0 on success, otherwise error + */ +int get_hdd_info(boost::archive::text_iarchive & ia, boost::archive::text_oarchive & oa) +{ + std::vector hdd_devices; + HDDInfoList list; + + try { + ia & hdd_devices; + } catch (const std::exception & e) { + syslog(LOG_ERR, "exception. %s\n", e.what()); + return -1; + } + + for (auto & hdd_device : hdd_devices) { + HDDInfo info{}; + + // Open a file + int fd = open(hdd_device.name_.c_str(), O_RDONLY); + if (fd < 0) { + info.error_code_ = errno; + syslog(LOG_ERR, "Failed to open a file. %s\n", strerror(info.error_code_)); + continue; + } + + // AHCI device + if (boost::starts_with(hdd_device.name_.c_str(), "/dev/sd")) { + // Get IDENTIFY DEVICE for ATA drive + info.error_code_ = get_ata_identify(fd, &info); + if (info.error_code_ != 0) { + syslog( + LOG_ERR, "Failed to get IDENTIFY DEVICE for ATA drive. %s\n", strerror(info.error_code_)); + close(fd); + continue; + } + // Get SMART DATA for ATA drive + info.error_code_ = get_ata_SMARTData(fd, &info, hdd_device); + if (info.error_code_ != 0) { + syslog(LOG_ERR, "Failed to get SMART LOG for ATA drive. %s\n", strerror(info.error_code_)); + close(fd); + continue; + } + } else if (boost::starts_with(hdd_device.name_.c_str(), "/dev/nvme")) { // NVMe device + // Get Identify for NVMe drive + info.error_code_ = get_nvme_identify(fd, &info); + if (info.error_code_ != 0) { + syslog(LOG_ERR, "Failed to get Identify for NVMe drive. %s\n", strerror(info.error_code_)); + close(fd); + continue; + } + // Get SMART / Health Information for NVMe drive + info.error_code_ = get_nvme_SMARTData(fd, &info); + if (info.error_code_ != 0) { + syslog( + LOG_ERR, "Failed to get SMART / Health Information for NVMe drive. %s\n", + strerror(info.error_code_)); + close(fd); + continue; + } + } + + // Close the file descriptor FD + info.error_code_ = close(fd); + if (info.error_code_ < 0) { + info.error_code_ = errno; + syslog(LOG_ERR, "Failed to close the file descriptor FD. %s\n", strerror(info.error_code_)); + } + + list[hdd_device.name_] = info; + } + + oa << list; + return 0; +} + +/** + * @brief unmount device with lazy option + * @param [in] boost::archive::text_iarchive object + * @param [out] boost::archive::text_oarchive object + * @return 0 on success, otherwise error + */ +int unmount_device_with_lazy(boost::archive::text_iarchive & ia, boost::archive::text_oarchive & oa) +{ + std::vector unmount_devices; + std::vector responses; + + try { + ia & unmount_devices; + } catch (const std::exception & e) { + syslog(LOG_ERR, "exception. %s\n", e.what()); + return -1; + } + + for (auto & unmount_device : unmount_devices) { + int ret = 0; + boost::process::ipstream is_out; + boost::process::ipstream is_err; + + boost::process::child c( + "/bin/sh", "-c", fmt::format("umount -l {}", unmount_device.part_device_.c_str()), + boost::process::std_out > is_out, boost::process::std_err > is_err); + c.wait(); + + if (c.exit_code() != 0) { + syslog( + LOG_ERR, "Failed to execute umount command. %s\n", unmount_device.part_device_.c_str()); + ret = -1; + } + responses.push_back(ret); + } + + oa << responses; + return 0; +} + +/** + * @brief hdd_reader main procedure * @param [in] port port to listen */ void run(int port) @@ -468,14 +600,14 @@ void run(int port) return; } - // Restore list of devices - std::vector hdd_devices; + uint8_t request_id; + + buf[sizeof(buf) - 1] = '\0'; + std::istringstream iss(buf); + boost::archive::text_iarchive ia(iss); try { - buf[sizeof(buf) - 1] = '\0'; - std::istringstream iss(buf); - boost::archive::text_iarchive oa(iss); - oa & hdd_devices; + ia & request_id; } catch (const std::exception & e) { syslog(LOG_ERR, "exception. %s\n", e.what()); close(new_sock); @@ -483,71 +615,26 @@ void run(int port) return; } - HDDInfoList list; std::ostringstream oss; boost::archive::text_oarchive oa(oss); - for (auto & hdd_device : hdd_devices) { - HDDInfo info{}; - - // Open a file - int fd = open(hdd_device.name_.c_str(), O_RDONLY); - if (fd < 0) { - info.error_code_ = errno; - syslog(LOG_ERR, "Failed to open a file. %s\n", strerror(info.error_code_)); + switch (request_id) { + case HDDReaderRequestID::GetHDDInfo: + ret = get_hdd_info(ia, oa); + break; + case HDDReaderRequestID::UnmountDevice: + ret = unmount_device_with_lazy(ia, oa); + break; + default: + syslog(LOG_ERR, "Request ID is invalid. %d\n", request_id); continue; - } - - // AHCI device - if (boost::starts_with(hdd_device.name_.c_str(), "/dev/sd")) { - // Get IDENTIFY DEVICE for ATA drive - info.error_code_ = get_ata_identify(fd, &info); - if (info.error_code_ != 0) { - syslog( - LOG_ERR, "Failed to get IDENTIFY DEVICE for ATA drive. %s\n", - strerror(info.error_code_)); - close(fd); - continue; - } - // Get SMART DATA for ATA drive - info.error_code_ = get_ata_SMARTData(fd, &info, hdd_device); - if (info.error_code_ != 0) { - syslog( - LOG_ERR, "Failed to get SMART LOG for ATA drive. %s\n", strerror(info.error_code_)); - close(fd); - continue; - } - } else if (boost::starts_with(hdd_device.name_.c_str(), "/dev/nvme")) { // NVMe device - // Get Identify for NVMe drive - info.error_code_ = get_nvme_identify(fd, &info); - if (info.error_code_ != 0) { - syslog( - LOG_ERR, "Failed to get Identify for NVMe drive. %s\n", strerror(info.error_code_)); - close(fd); - continue; - } - // Get SMART / Health Information for NVMe drive - info.error_code_ = get_nvme_SMARTData(fd, &info); - if (info.error_code_ != 0) { - syslog( - LOG_ERR, "Failed to get SMART / Health Information for NVMe drive. %s\n", - strerror(info.error_code_)); - close(fd); - continue; - } - } - - // Close the file descriptor FD - info.error_code_ = close(fd); - if (info.error_code_ < 0) { - info.error_code_ = errno; - syslog(LOG_ERR, "Failed to close the file descriptor FD. %s\n", strerror(info.error_code_)); - } - - list[hdd_device.name_] = info; + } + if (ret != 0) { + close(new_sock); + close(sock); + return; } - oa << list; // Write N bytes of BUF to FD ret = write(new_sock, oss.str().c_str(), oss.str().length()); if (ret < 0) { diff --git a/system/system_monitor/src/hdd_monitor/hdd_monitor.cpp b/system/system_monitor/src/hdd_monitor/hdd_monitor.cpp index 3e05e76054f62..85a3ba7742cab 100644 --- a/system/system_monitor/src/hdd_monitor/hdd_monitor.cpp +++ b/system/system_monitor/src/hdd_monitor/hdd_monitor.cpp @@ -30,8 +30,10 @@ #include #include +#include #include +#include #include #include @@ -40,7 +42,8 @@ namespace bp = boost::process; HDDMonitor::HDDMonitor(const rclcpp::NodeOptions & options) : Node("hdd_monitor", options), updater_(this), - hdd_reader_port_(declare_parameter("hdd_reader_port", 7635)) + hdd_reader_port_(declare_parameter("hdd_reader_port", 7635)), + last_hdd_stat_update_time_{0, 0, this->get_clock()->get_clock_type()} { using namespace std::literals::chrono_literals; @@ -52,11 +55,23 @@ HDDMonitor::HDDMonitor(const rclcpp::NodeOptions & options) updater_.add("HDD Temperature", this, &HDDMonitor::checkSMARTTemperature); updater_.add("HDD PowerOnHours", this, &HDDMonitor::checkSMARTPowerOnHours); updater_.add("HDD TotalDataWritten", this, &HDDMonitor::checkSMARTTotalDataWritten); + updater_.add("HDD RecoveredError", this, &HDDMonitor::checkSMARTRecoveredError); updater_.add("HDD Usage", this, &HDDMonitor::checkUsage); + updater_.add("HDD ReadDataRate", this, &HDDMonitor::checkReadDataRate); + updater_.add("HDD WriteDataRate", this, &HDDMonitor::checkWriteDataRate); + updater_.add("HDD ReadIOPS", this, &HDDMonitor::checkReadIOPS); + updater_.add("HDD WriteIOPS", this, &HDDMonitor::checkWriteIOPS); + updater_.add("HDD Connection", this, &HDDMonitor::checkConnection); + + // get HDD connection status + updateHDDConnections(); // get HDD information from HDD reader for the first time updateHDDInfoList(); + // start HDD transfer measurement + startHDDTransferMeasurement(); + timer_ = rclcpp::create_timer(this, get_clock(), 1s, std::bind(&HDDMonitor::onTimer, this)); } @@ -75,6 +90,11 @@ void HDDMonitor::checkSMARTTotalDataWritten(diagnostic_updater::DiagnosticStatus checkSMART(stat, HDDSMARTInfoItem::TOTAL_DATA_WRITTEN); } +void HDDMonitor::checkSMARTRecoveredError(diagnostic_updater::DiagnosticStatusWrapper & stat) +{ + checkSMART(stat, HDDSMARTInfoItem::RECOVERED_ERROR); +} + void HDDMonitor::checkSMART( diagnostic_updater::DiagnosticStatusWrapper & stat, HDDSMARTInfoItem item) { @@ -103,11 +123,15 @@ void HDDMonitor::checkSMART( std::string val_str = ""; for (auto itr = hdd_params_.begin(); itr != hdd_params_.end(); ++itr, ++index) { + if (!hdd_connected_flags_[itr->first]) { + continue; + } + // Retrieve HDD information - auto hdd_itr = hdd_info_list_.find(itr->second.device_); + auto hdd_itr = hdd_info_list_.find(itr->second.disk_device_); if (hdd_itr == hdd_info_list_.end()) { stat.add(fmt::format("HDD {}: status", index), "hdd_reader error"); - stat.add(fmt::format("HDD {}: name", index), itr->first.c_str()); + stat.add(fmt::format("HDD {}: name", index), itr->second.part_device_.c_str()); stat.add(fmt::format("HDD {}: hdd_reader", index), strerror(ENOENT)); error_str = "hdd_reader error"; continue; @@ -115,7 +139,7 @@ void HDDMonitor::checkSMART( if (hdd_itr->second.error_code_ != 0) { stat.add(fmt::format("HDD {}: status", index), "hdd_reader error"); - stat.add(fmt::format("HDD {}: name", index), itr->first.c_str()); + stat.add(fmt::format("HDD {}: name", index), itr->second.part_device_.c_str()); stat.add(fmt::format("HDD {}: hdd_reader", index), strerror(hdd_itr->second.error_code_)); error_str = "hdd_reader error"; continue; @@ -132,7 +156,11 @@ void HDDMonitor::checkSMART( level = DiagStatus::WARN; } key_str = fmt::format("HDD {}: temperature", index); - val_str = fmt::format("{:.1f} DegC", temp); + if (hdd_itr->second.is_valid_temp_) { + val_str = fmt::format("{:.1f} DegC", temp); + } else { + val_str = "not available"; + } } break; case HDDSMARTInfoItem::POWER_ON_HOURS: { int64_t power_on_hours = static_cast(hdd_itr->second.power_on_hours_); @@ -142,7 +170,11 @@ void HDDMonitor::checkSMART( level = DiagStatus::WARN; } key_str = fmt::format("HDD {}: power on hours", index); - val_str = fmt::format("{} Hours", hdd_itr->second.power_on_hours_); + if (hdd_itr->second.is_valid_power_on_hours_) { + val_str = fmt::format("{} Hours", hdd_itr->second.power_on_hours_); + } else { + val_str = "not available"; + } } break; case HDDSMARTInfoItem::TOTAL_DATA_WRITTEN: { uint64_t total_data_written = static_cast(hdd_itr->second.total_data_written_); @@ -158,13 +190,31 @@ void HDDMonitor::checkSMART( val_str = "not available"; } } break; + case HDDSMARTInfoItem::RECOVERED_ERROR: { + int32_t recovered_error = static_cast(hdd_itr->second.recovered_error_); + if (initial_recovered_errors_.find(itr->first) == initial_recovered_errors_.end()) { + initial_recovered_errors_[itr->first] = recovered_error; + } + recovered_error -= initial_recovered_errors_[itr->first]; + + level = DiagStatus::OK; + if (recovered_error >= itr->second.recovered_error_warn_) { + level = DiagStatus::WARN; + } + key_str = fmt::format("HDD {}: recovered error", index); + if (hdd_itr->second.is_valid_recovered_error_) { + val_str = fmt::format("{}", hdd_itr->second.recovered_error_); + } else { + val_str = "not available"; + } + } break; default: break; } stat.add( fmt::format("HDD {}: status", index), smart_dicts_[static_cast(item)].at(level)); - stat.add(fmt::format("HDD {}: name", index), itr->second.device_.c_str()); + stat.add(fmt::format("HDD {}: name", index), itr->second.disk_device_.c_str()); stat.add(fmt::format("HDD {}: model", index), hdd_itr->second.model_.c_str()); stat.add(fmt::format("HDD {}: serial", index), hdd_itr->second.serial_.c_str()); stat.addf(key_str, val_str.c_str()); @@ -197,6 +247,10 @@ void HDDMonitor::checkUsage(diagnostic_updater::DiagnosticStatusWrapper & stat) std::string error_str = ""; for (auto itr = hdd_params_.begin(); itr != hdd_params_.end(); ++itr, ++hdd_index) { + if (!hdd_connected_flags_[itr->first]) { + continue; + } + // Get summary of disk space usage of ext4 // boost::process create file descriptor without O_CLOEXEC required for multithreading. @@ -225,8 +279,8 @@ void HDDMonitor::checkUsage(diagnostic_updater::DiagnosticStatusWrapper & stat) // Invoke shell to use shell wildcard expansion bp::child c( - "/bin/sh", "-c", fmt::format("df -Pm {}*", itr->first.c_str()), bp::std_out > is_out, - bp::std_err > is_err); + "/bin/sh", "-c", fmt::format("df -Pm {}*", itr->second.part_device_.c_str()), + bp::std_out > is_out, bp::std_err > is_err); c.wait(); if (c.exit_code() != 0) { @@ -234,7 +288,7 @@ void HDDMonitor::checkUsage(diagnostic_updater::DiagnosticStatusWrapper & stat) is_err >> os.rdbuf(); error_str = "df error"; stat.add(fmt::format("HDD {}: status", hdd_index), "df error"); - stat.add(fmt::format("HDD {}: name", hdd_index), itr->first.c_str()); + stat.add(fmt::format("HDD {}: name", hdd_index), itr->second.part_device_.c_str()); stat.add(fmt::format("HDD {}: df", hdd_index), os.str().c_str()); continue; } @@ -300,18 +354,155 @@ void HDDMonitor::checkUsage(diagnostic_updater::DiagnosticStatusWrapper & stat) SystemMonitorUtility::stopMeasurement(t_start, stat); } +void HDDMonitor::checkReadDataRate(diagnostic_updater::DiagnosticStatusWrapper & stat) +{ + checkStatistics(stat, HDDStatItem::READ_DATA_RATE); +} + +void HDDMonitor::checkWriteDataRate(diagnostic_updater::DiagnosticStatusWrapper & stat) +{ + checkStatistics(stat, HDDStatItem::WRITE_DATA_RATE); +} + +void HDDMonitor::checkReadIOPS(diagnostic_updater::DiagnosticStatusWrapper & stat) +{ + checkStatistics(stat, HDDStatItem::READ_IOPS); +} + +void HDDMonitor::checkWriteIOPS(diagnostic_updater::DiagnosticStatusWrapper & stat) +{ + checkStatistics(stat, HDDStatItem::WRITE_IOPS); +} + +void HDDMonitor::checkStatistics( + diagnostic_updater::DiagnosticStatusWrapper & stat, HDDStatItem item) +{ + // Remember start time to measure elapsed time + const auto t_start = SystemMonitorUtility::startMeasurement(); + + if (hdd_params_.empty()) { + stat.summary(DiagStatus::ERROR, "invalid disk parameter"); + return; + } + + int hdd_index = 0; + int whole_level = DiagStatus::OK; + std::string error_str = ""; + std::string key_str = ""; + std::string val_str = ""; + + for (auto itr = hdd_params_.begin(); itr != hdd_params_.end(); ++itr, ++hdd_index) { + if (!hdd_connected_flags_[itr->first]) { + continue; + } + + int level = DiagStatus::OK; + + switch (item) { + case HDDStatItem::READ_DATA_RATE: { + float read_data_rate = hdd_stats_[itr->first].read_data_rate_MBs_; + + if (read_data_rate >= itr->second.read_data_rate_warn_) { + level = DiagStatus::WARN; + } + key_str = fmt::format("HDD {}: data rate of read", hdd_index); + val_str = fmt::format("{:.2f} MB/s", read_data_rate); + } break; + case HDDStatItem::WRITE_DATA_RATE: { + float write_data_rate = hdd_stats_[itr->first].write_data_rate_MBs_; + + if (write_data_rate >= itr->second.write_data_rate_warn_) { + level = DiagStatus::WARN; + } + key_str = fmt::format("HDD {}: data rate of write", hdd_index); + val_str = fmt::format("{:.2f} MB/s", write_data_rate); + } break; + case HDDStatItem::READ_IOPS: { + float read_iops = hdd_stats_[itr->first].read_iops_; + + if (read_iops >= itr->second.read_iops_warn_) { + level = DiagStatus::WARN; + } + key_str = fmt::format("HDD {}: IOPS of read", hdd_index); + val_str = fmt::format("{:.2f} IOPS", read_iops); + } break; + case HDDStatItem::WRITE_IOPS: { + float write_iops = hdd_stats_[itr->first].write_iops_; + + if (write_iops >= itr->second.write_iops_warn_) { + level = DiagStatus::WARN; + } + key_str = fmt::format("HDD {}: IOPS of write", hdd_index); + val_str = fmt::format("{:.2f} IOPS", write_iops); + } break; + default: + break; + } + + if (!hdd_stats_[itr->first].error_str_.empty()) { + error_str = hdd_stats_[itr->first].error_str_; + stat.add(fmt::format("HDD {}: status", hdd_index), error_str); + stat.add(fmt::format("HDD {}: name", hdd_index), itr->second.disk_device_.c_str()); + } else { + stat.add( + fmt::format("HDD {}: status", hdd_index), + stat_dicts_[static_cast(item)].at(level)); + stat.add(fmt::format("HDD {}: name", hdd_index), itr->second.disk_device_.c_str()); + stat.add(key_str, val_str.c_str()); + } + + whole_level = std::max(whole_level, level); + } + + if (!error_str.empty()) { + stat.summary(DiagStatus::ERROR, error_str); + } else { + stat.summary(whole_level, stat_dicts_[static_cast(item)].at(whole_level)); + } + + // Measure elapsed time since start time and report + SystemMonitorUtility::stopMeasurement(t_start, stat); +} + +void HDDMonitor::checkConnection(diagnostic_updater::DiagnosticStatusWrapper & stat) +{ + // Remember start time to measure elapsed time + const auto t_start = SystemMonitorUtility::startMeasurement(); + + if (hdd_params_.empty()) { + stat.summary(DiagStatus::ERROR, "invalid disk parameter"); + return; + } + + int hdd_index = 0; + int whole_level = DiagStatus::OK; + + for (auto itr = hdd_params_.begin(); itr != hdd_params_.end(); ++itr, ++hdd_index) { + int level = DiagStatus::OK; + + if (!hdd_connected_flags_[itr->first]) { + level = DiagStatus::WARN; + } + + stat.add(fmt::format("HDD {}: status", hdd_index), connection_dict_.at(level)); + stat.add(fmt::format("HDD {}: name", hdd_index), itr->second.disk_device_); + stat.add(fmt::format("HDD {}: mount point", hdd_index), itr->first.c_str()); + + whole_level = std::max(whole_level, level); + } + + stat.summary(whole_level, connection_dict_.at(whole_level)); + + // Measure elapsed time since start time and report + SystemMonitorUtility::stopMeasurement(t_start, stat); +} + void HDDMonitor::getHDDParams() { const auto num_disks = this->declare_parameter("num_disks", 0); for (auto i = 0; i < num_disks; ++i) { const auto prefix = "disks.disk" + std::to_string(i); - const auto name = declare_parameter(prefix + ".name", "/"); - - // Get device name from mount point - const auto device_name = getDeviceFromMountPoint(name); - if (device_name.empty()) { - continue; - } + const auto mount_point = declare_parameter(prefix + ".name", "/"); HDDParam param; param.temp_warn_ = declare_parameter(prefix + ".temp_warn", 55.0f); @@ -323,24 +514,26 @@ void HDDMonitor::getHDDParams() declare_parameter(prefix + ".total_data_written_warn", 4915200); param.total_data_written_warn_ = static_cast( total_data_written_warn_org * (1.0f - param.total_data_written_safety_factor_)); + param.recovered_error_warn_ = declare_parameter(prefix + ".recovered_error_warn", 1); param.free_warn_ = declare_parameter(prefix + ".free_warn", 5120); param.free_error_ = declare_parameter(prefix + ".free_error", 100); + param.read_data_rate_warn_ = declare_parameter(prefix + ".read_data_rate_warn", 360.0); + param.write_data_rate_warn_ = declare_parameter(prefix + ".write_data_rate_warn", 103.5); + param.read_iops_warn_ = declare_parameter(prefix + ".read_iops_warn", 63360.0); + param.write_iops_warn_ = declare_parameter(prefix + ".write_iops_warn", 24120.0); + param.temp_attribute_id_ = + static_cast(declare_parameter(prefix + ".temp_attribute_id", 0xC2)); + param.power_on_hours_attribute_id_ = + static_cast(declare_parameter(prefix + ".power_on_hours_attribute_id", 0x09)); + param.total_data_written_attribute_id_ = static_cast( + declare_parameter(prefix + ".total_data_written_attribute_id", 0xF1)); + param.recovered_error_attribute_id_ = + static_cast(declare_parameter(prefix + ".recovered_error_attribute_id", 0xC3)); - // Remove index number of partition for passing device name to hdd-reader - if (boost::starts_with(device_name, "/dev/sd")) { - const std::regex pattern("\\d+$"); - param.device_ = std::regex_replace(device_name, pattern, ""); - } else if (boost::starts_with(device_name, "/dev/nvme")) { - const std::regex pattern("p\\d+$"); - param.device_ = std::regex_replace(device_name, pattern, ""); - } - hdd_params_[device_name] = param; + hdd_params_[mount_point] = param; - HDDDevice device; - device.name_ = param.device_; - device.total_data_written_attribute_id_ = static_cast( - declare_parameter(prefix + ".total_data_written_attribute_id", 0xF1)); - hdd_devices_.push_back(device); + HDDStat stat; + hdd_stats_[mount_point] = stat; } } @@ -384,7 +577,12 @@ std::string HDDMonitor::getDeviceFromMountPoint(const std::string & mount_point) return ret; } -void HDDMonitor::onTimer() { updateHDDInfoList(); } +void HDDMonitor::onTimer() +{ + updateHDDConnections(); + updateHDDInfoList(); + updateHDDStatistics(); +} void HDDMonitor::updateHDDInfoList() { @@ -426,9 +624,27 @@ void HDDMonitor::updateHDDInfoList() return; } + uint8_t request_id = HDDReaderRequestID::GetHDDInfo; + std::vector hdd_devices; + for (auto itr = hdd_params_.begin(); itr != hdd_params_.end(); ++itr) { + if (!hdd_connected_flags_[itr->first]) { + continue; + } + + HDDDevice device; + device.name_ = itr->second.disk_device_; + device.temp_attribute_id_ = itr->second.temp_attribute_id_; + device.power_on_hours_attribute_id_ = itr->second.power_on_hours_attribute_id_; + device.total_data_written_attribute_id_ = itr->second.total_data_written_attribute_id_; + device.recovered_error_attribute_id_ = itr->second.recovered_error_attribute_id_; + + hdd_devices.push_back(device); + } + std::ostringstream oss; boost::archive::text_oarchive oa(oss); - oa & hdd_devices_; + oa & request_id; + oa & hdd_devices; // Write list of devices to FD ret = write(sock, oss.str().c_str(), oss.str().length()); @@ -477,5 +693,234 @@ void HDDMonitor::updateHDDInfoList() } } +void HDDMonitor::startHDDTransferMeasurement() +{ + for (auto & hdd_stat : hdd_stats_) { + hdd_stat.second.error_str_ = ""; + + if (!hdd_connected_flags_[hdd_stat.first]) { + continue; + } + + SysfsDevStat sfdevstat; + if (readSysfsDeviceStat(hdd_stat.second.device_, sfdevstat)) { + hdd_stat.second.error_str_ = "stat file read error"; + continue; + } + hdd_stat.second.last_sfdevstat_ = sfdevstat; + } + + last_hdd_stat_update_time_ = this->now(); +} + +void HDDMonitor::updateHDDStatistics() +{ + double duration_sec = (this->now() - last_hdd_stat_update_time_).seconds(); + + for (auto & hdd_stat : hdd_stats_) { + hdd_stat.second.error_str_ = ""; + + if (!hdd_connected_flags_[hdd_stat.first]) { + continue; + } + + SysfsDevStat sfdevstat; + if (readSysfsDeviceStat(hdd_stat.second.device_, sfdevstat)) { + hdd_stat.second.error_str_ = "stat file read error"; + continue; + } + + SysfsDevStat & last_sfdevstat = hdd_stat.second.last_sfdevstat_; + + hdd_stat.second.read_data_rate_MBs_ = getIncreaseSysfsDeviceStatValuePerSec( + sfdevstat.rd_sectors_, last_sfdevstat.rd_sectors_, duration_sec); + hdd_stat.second.read_data_rate_MBs_ /= 2048; + hdd_stat.second.write_data_rate_MBs_ = getIncreaseSysfsDeviceStatValuePerSec( + sfdevstat.wr_sectors_, last_sfdevstat.wr_sectors_, duration_sec); + hdd_stat.second.write_data_rate_MBs_ /= 2048; + hdd_stat.second.read_iops_ = getIncreaseSysfsDeviceStatValuePerSec( + sfdevstat.rd_ios_, last_sfdevstat.rd_ios_, duration_sec); + hdd_stat.second.write_iops_ = getIncreaseSysfsDeviceStatValuePerSec( + sfdevstat.wr_ios_, last_sfdevstat.wr_ios_, duration_sec); + + hdd_stat.second.last_sfdevstat_ = sfdevstat; + } + + last_hdd_stat_update_time_ = this->now(); +} + +double HDDMonitor::getIncreaseSysfsDeviceStatValuePerSec( + uint64_t cur_val, uint64_t last_val, double duration_sec) +{ + if (cur_val > last_val && duration_sec > 0.0) { + return static_cast(cur_val - last_val) / duration_sec; + } + return 0.0; +} + +int HDDMonitor::readSysfsDeviceStat(const std::string & device, SysfsDevStat & sfdevstat) +{ + int ret = -1; + unsigned int ios_pgr, tot_ticks, rq_ticks, wr_ticks; + uint64_t rd_ios, rd_merges_or_rd_sec, wr_ios, wr_merges; + uint64_t rd_sec_or_wr_ios, wr_sec, rd_ticks_or_wr_sec; + uint64_t dc_ios, dc_merges, dc_sec, dc_ticks; + + std::string filename("/sys/block/"); + filename += device + "/stat"; + FILE * fp = fopen(filename.c_str(), "r"); + if (fp == NULL) { + return ret; + } + + int i = fscanf( + fp, "%lu %lu %lu %lu %lu %lu %lu %u %u %u %u %lu %lu %lu %lu", &rd_ios, &rd_merges_or_rd_sec, + &rd_sec_or_wr_ios, &rd_ticks_or_wr_sec, &wr_ios, &wr_merges, &wr_sec, &wr_ticks, &ios_pgr, + &tot_ticks, &rq_ticks, &dc_ios, &dc_merges, &dc_sec, &dc_ticks); + + if (i >= 7) { + sfdevstat.rd_ios_ = rd_ios; + sfdevstat.rd_sectors_ = rd_sec_or_wr_ios; + sfdevstat.wr_ios_ = wr_ios; + sfdevstat.wr_sectors_ = wr_sec; + ret = 0; + } + + fclose(fp); + return ret; +} + +void HDDMonitor::updateHDDConnections() +{ + for (auto & hdd_param : hdd_params_) { + hdd_connected_flags_[hdd_param.first] = false; + + // Get device name from mount point + hdd_param.second.part_device_ = getDeviceFromMountPoint(hdd_param.first); + if (!hdd_param.second.part_device_.empty()) { + // Check the existence of device file + std::error_code ec; + if (std::filesystem::exists(hdd_param.second.part_device_, ec)) { + hdd_connected_flags_[hdd_param.first] = true; + + // Remove index number of partition for passing device name to hdd-reader + if (boost::starts_with(hdd_param.second.part_device_, "/dev/sd")) { + const std::regex pattern("\\d+$"); + hdd_param.second.disk_device_ = + std::regex_replace(hdd_param.second.part_device_, pattern, ""); + } else if (boost::starts_with(hdd_param.second.part_device_, "/dev/nvme")) { + const std::regex pattern("p\\d+$"); + hdd_param.second.disk_device_ = + std::regex_replace(hdd_param.second.part_device_, pattern, ""); + } + + const std::regex raw_pattern(".*/"); + hdd_stats_[hdd_param.first].device_ = + std::regex_replace(hdd_param.second.disk_device_, raw_pattern, ""); + } else { + // Deal with the issue that file system remains mounted when a drive is actually + // disconnected. + if (unmountDevice(hdd_param.second.part_device_)) { + RCLCPP_ERROR( + get_logger(), "Failed to unmount device : %s", hdd_param.second.part_device_.c_str()); + } + } + } + } +} + +int HDDMonitor::unmountDevice(std::string & device) +{ + // Create a new socket + int sock = socket(AF_INET, SOCK_STREAM, 0); + if (sock < 0) { + RCLCPP_ERROR(get_logger(), "socket create error. %s", strerror(errno)); + return -1; + } + + // Specify the receiving timeouts until reporting an error + struct timeval tv; + tv.tv_sec = 10; + tv.tv_usec = 0; + int ret = setsockopt(sock, SOL_SOCKET, SO_RCVTIMEO, &tv, sizeof(tv)); + if (ret < 0) { + RCLCPP_ERROR(get_logger(), "setsockopt error. %s", strerror(errno)); + close(sock); + return -1; + } + + // Connect the socket referred to by the file descriptor + sockaddr_in addr; + memset(&addr, 0, sizeof(sockaddr_in)); + addr.sin_family = AF_INET; + addr.sin_port = htons(hdd_reader_port_); + addr.sin_addr.s_addr = htonl(INADDR_ANY); + ret = connect(sock, (struct sockaddr *)&addr, sizeof(addr)); + if (ret < 0) { + RCLCPP_ERROR(get_logger(), "socket connect error. %s", strerror(errno)); + close(sock); + return -1; + } + + uint8_t request_id = HDDReaderRequestID::UnmountDevice; + std::vector umount_dev_infos; + UnmountDeviceInfo dev_info; + + dev_info.part_device_ = device; + umount_dev_infos.push_back(dev_info); + + std::ostringstream oss; + boost::archive::text_oarchive oa(oss); + oa & request_id; + oa & umount_dev_infos; + + // Write list of devices to FD + ret = write(sock, oss.str().c_str(), oss.str().length()); + if (ret < 0) { + RCLCPP_ERROR(get_logger(), "socket write error. %s", strerror(errno)); + close(sock); + return -1; + } + + // Receive messages from a socket + char buf[1024] = ""; + ret = recv(sock, buf, sizeof(buf) - 1, 0); + if (ret < 0) { + RCLCPP_ERROR(get_logger(), "socket recv error. %s", strerror(errno)); + close(sock); + return -1; + } + // No data received + if (ret == 0) { + RCLCPP_ERROR(get_logger(), "no data received from hdd_reader."); + close(sock); + return -1; + } + + // Close the file descriptor FD + ret = close(sock); + if (ret < 0) { + RCLCPP_ERROR(get_logger(), "socket close error. %s", strerror(errno)); + return -1; + } + + std::vector responses; + + // Restore responses + try { + std::istringstream iss(buf); + boost::archive::text_iarchive ia(iss); + ia >> responses; + } catch (const std::exception & e) { + RCLCPP_ERROR(get_logger(), "restore responses exception. %s", e.what()); + return -1; + } + if (responses.empty()) { + RCLCPP_ERROR(get_logger(), "responses from hdd_reader is empty."); + return -1; + } + return responses[0]; +} + #include RCLCPP_COMPONENTS_REGISTER_NODE(HDDMonitor) diff --git a/system/system_monitor/src/net_monitor/net_monitor.cpp b/system/system_monitor/src/net_monitor/net_monitor.cpp index 983af5668a082..c213e864db93b 100644 --- a/system/system_monitor/src/net_monitor/net_monitor.cpp +++ b/system/system_monitor/src/net_monitor/net_monitor.cpp @@ -37,6 +37,8 @@ #include #include +#include +#include #include #include @@ -46,10 +48,15 @@ NetMonitor::NetMonitor(const rclcpp::NodeOptions & options) last_update_time_{0, 0, this->get_clock()->get_clock_type()}, device_params_( declare_parameter>("devices", std::vector())), + last_reassembles_failed_(0), monitor_program_(declare_parameter("monitor_program", "greengrass")), traffic_reader_port_(declare_parameter("traffic_reader_port", TRAFFIC_READER_PORT)), crc_error_check_duration_(declare_parameter("crc_error_check_duration", 1)), - crc_error_count_threshold_(declare_parameter("crc_error_count_threshold", 1)) + crc_error_count_threshold_(declare_parameter("crc_error_count_threshold", 1)), + reassembles_failed_check_duration_( + declare_parameter("reassembles_failed_check_duration", 1)), + reassembles_failed_check_count_(declare_parameter("reassembles_failed_check_count", 1)), + reassembles_failed_column_index_(0) { using namespace std::literals::chrono_literals; @@ -65,9 +72,12 @@ NetMonitor::NetMonitor(const rclcpp::NodeOptions & options) updater_.add("Network Usage", this, &NetMonitor::checkUsage); updater_.add("Network Traffic", this, &NetMonitor::monitorTraffic); updater_.add("Network CRC Error", this, &NetMonitor::checkCrcError); + updater_.add("IP Packet Reassembles Failed", this, &NetMonitor::checkReassemblesFailed); nl80211_.init(); + searchReassemblesFailedColumnIndex(); + // get Network information for the first time updateNetworkInfoList(); @@ -492,5 +502,136 @@ void NetMonitor::monitorTraffic(diagnostic_updater::DiagnosticStatusWrapper & st SystemMonitorUtility::stopMeasurement(t_start, stat); } +void NetMonitor::checkReassemblesFailed(diagnostic_updater::DiagnosticStatusWrapper & stat) +{ + // Remember start time to measure elapsed time + const auto t_start = SystemMonitorUtility::startMeasurement(); + + int whole_level = DiagStatus::OK; + std::string error_str; + uint64_t total_reassembles_failed = 0; + uint64_t unit_reassembles_failed = 0; + + if (getReassemblesFailed(total_reassembles_failed)) { + reassembles_failed_queue_.push_back(total_reassembles_failed - last_reassembles_failed_); + while (reassembles_failed_queue_.size() > reassembles_failed_check_duration_) { + reassembles_failed_queue_.pop_front(); + } + + for (auto reassembles_failed : reassembles_failed_queue_) { + unit_reassembles_failed += reassembles_failed; + } + + stat.add(fmt::format("total packet reassembles failed"), total_reassembles_failed); + stat.add(fmt::format("packet reassembles failed per unit time"), unit_reassembles_failed); + + if (unit_reassembles_failed >= reassembles_failed_check_count_) { + whole_level = std::max(whole_level, static_cast(DiagStatus::WARN)); + error_str = "reassembles failed"; + } + + last_reassembles_failed_ = total_reassembles_failed; + } else { + reassembles_failed_queue_.push_back(0); + whole_level = std::max(whole_level, static_cast(DiagStatus::ERROR)); + error_str = "failed to read /proc/net/snmp"; + } + + if (!error_str.empty()) { + stat.summary(whole_level, error_str); + } else { + stat.summary(whole_level, "OK"); + } + + // Measure elapsed time since start time and report + SystemMonitorUtility::stopMeasurement(t_start, stat); +} + +void NetMonitor::searchReassemblesFailedColumnIndex() +{ + std::ifstream ifs("/proc/net/snmp"); + if (!ifs) { + RCLCPP_WARN(get_logger(), "Failed to open /proc/net/snmp."); + return; + } + + // /proc/net/snmp + // Ip: Forwarding DefaultTTL InReceives ... ReasmTimeout ReasmReqds ReasmOKs ReasmFails ... + // Ip: 2 64 5636471397 ... 135 2303339 216166 270 ... + std::string line; + + // Find column index of 'ReasmFails' + if (!std::getline(ifs, line)) { + RCLCPP_WARN(get_logger(), "Failed to get /proc/net/snmp first line."); + return; + } + + std::vector title_list; + boost::split(title_list, line, boost::is_space()); + + if (title_list.empty()) { + RCLCPP_WARN(get_logger(), "/proc/net/snmp first line is empty."); + return; + } + if (title_list[0] != "Ip:") { + RCLCPP_WARN( + get_logger(), "/proc/net/snmp line title column is invalid. : %s", title_list[0].c_str()); + return; + } + + int index = 0; + for (auto itr = title_list.begin(); itr != title_list.end(); ++itr, ++index) { + if (*itr == "ReasmFails") { + reassembles_failed_column_index_ = index; + break; + } + } +} + +bool NetMonitor::getReassemblesFailed(uint64_t & reassembles_failed) +{ + if (reassembles_failed_column_index_ == 0) { + RCLCPP_WARN( + get_logger(), "reassembles failed column index is invalid. : %d", + reassembles_failed_column_index_); + return false; + } + + std::ifstream ifs("/proc/net/snmp"); + if (!ifs) { + RCLCPP_WARN(get_logger(), "Failed to open /proc/net/snmp."); + return false; + } + + std::string line; + + // Skip title row + if (!std::getline(ifs, line)) { + RCLCPP_WARN(get_logger(), "Failed to get /proc/net/snmp first line."); + return false; + } + + // Find a value of 'ReasmFails' + if (!std::getline(ifs, line)) { + RCLCPP_WARN(get_logger(), "Failed to get /proc/net/snmp second line."); + return false; + } + + std::vector value_list; + boost::split(value_list, line, boost::is_space()); + + if (reassembles_failed_column_index_ >= value_list.size()) { + RCLCPP_WARN( + get_logger(), + "There are not enough columns for reassembles failed column index. : columns=%d index=%d", + static_cast(value_list.size()), reassembles_failed_column_index_); + return false; + } + + reassembles_failed = std::stoull(value_list[reassembles_failed_column_index_]); + + return true; +} + #include RCLCPP_COMPONENTS_REGISTER_NODE(NetMonitor) diff --git a/system/topic_state_monitor/Readme.md b/system/topic_state_monitor/Readme.md index 434e63c1ca783..28333305ef5a9 100644 --- a/system/topic_state_monitor/Readme.md +++ b/system/topic_state_monitor/Readme.md @@ -35,23 +35,25 @@ The types of topic status and corresponding diagnostic status are following. ### Node Parameters -| Name | Type | Default Value | Description | -| ------------- | ------ | ------------- | ----------------------------------------------------- | -| `update_rate` | double | 10.0 | Timer callback period [Hz] | -| `window_size` | int | 10 | Window size of target topic for calculating frequency | +| Name | Type | Default Value | Description | +| ----------------- | ------ | ------------- | ------------------------------------------------------------- | +| `topic` | string | - | Name of target topic | +| `topic_type` | string | - | Type of target topic (used if the topic is not transform) | +| `frame_id` | string | - | Frame ID of transform parent (used if the topic is transform) | +| `child_frame_id` | string | - | Frame ID of transform child (used if the topic is transform) | +| `transient_local` | bool | false | QoS policy of topic subscription (Transient Local/Volatile) | +| `best_effort` | bool | false | QoS policy of topic subscription (Best Effort/Reliable) | +| `diag_name` | string | - | Name used for the diagnostics to publish | +| `update_rate` | double | 10.0 | Timer callback period [Hz] | ### Core Parameters -| Name | Type | Default Value | Description | -| ----------------- | ------ | ------------- | ---------------------------------------------------------------------------------------------------- | -| `topic` | string | - | Name of target topic | -| `topic_type` | string | - | Type of target topic | -| `transient_local` | bool | false | QoS policy of topic subscription (Transient Local/Volatile) | -| `best_effort` | bool | false | QoS policy of topic subscription (Best Effort/Reliable) | -| `diag_name` | string | - | Name used for the diagnostics to publish | -| `warn_rate` | double | 0.5 | If the topic rate is lower than this value, the topic status becomes `WarnRate` | -| `error_rate` | double | 0.1 | If the topic rate is lower than this value, the topic status becomes `ErrorRate` | -| `timeout` | double | 1.0 | If the topic subscription is stopped for more than this time [s], the topic status becomes `Timeout` | +| Name | Type | Default Value | Description | +| ------------- | ------ | ------------- | ---------------------------------------------------------------------------------------------------- | +| `warn_rate` | double | 0.5 | If the topic rate is lower than this value, the topic status becomes `WarnRate` | +| `error_rate` | double | 0.1 | If the topic rate is lower than this value, the topic status becomes `ErrorRate` | +| `timeout` | double | 1.0 | If the topic subscription is stopped for more than this time [s], the topic status becomes `Timeout` | +| `window_size` | int | 10 | Window size of target topic for calculating frequency | ## Assumptions / Known limits diff --git a/system/topic_state_monitor/include/topic_state_monitor/topic_state_monitor.hpp b/system/topic_state_monitor/include/topic_state_monitor/topic_state_monitor.hpp index 3ef80851b59a4..2057ca4c5c86c 100644 --- a/system/topic_state_monitor/include/topic_state_monitor/topic_state_monitor.hpp +++ b/system/topic_state_monitor/include/topic_state_monitor/topic_state_monitor.hpp @@ -24,11 +24,6 @@ namespace topic_state_monitor { struct Param { - std::string topic; - std::string topic_type; - bool transient_local; - bool best_effort; - std::string diag_name; double warn_rate; double error_rate; double timeout; diff --git a/system/topic_state_monitor/include/topic_state_monitor/topic_state_monitor_core.hpp b/system/topic_state_monitor/include/topic_state_monitor/topic_state_monitor_core.hpp index f05ecc51206a5..4696823ffbd4b 100644 --- a/system/topic_state_monitor/include/topic_state_monitor/topic_state_monitor_core.hpp +++ b/system/topic_state_monitor/include/topic_state_monitor/topic_state_monitor_core.hpp @@ -20,6 +20,8 @@ #include #include +#include + #include #include #include @@ -31,6 +33,14 @@ namespace topic_state_monitor struct NodeParam { double update_rate; + std::string diag_name; + std::string topic; + std::string topic_type; + std::string frame_id; + std::string child_frame_id; + bool transient_local; + bool best_effort; + bool is_transform; }; class TopicStateMonitorNode : public rclcpp::Node @@ -53,6 +63,7 @@ class TopicStateMonitorNode : public rclcpp::Node // Subscriber rclcpp::GenericSubscription::SharedPtr sub_topic_; + rclcpp::Subscription::SharedPtr sub_transform_; // Timer void onTimer(); diff --git a/system/topic_state_monitor/launch/topic_state_monitor_tf.launch.xml b/system/topic_state_monitor/launch/topic_state_monitor_tf.launch.xml new file mode 100644 index 0000000000000..1a255a8a5859a --- /dev/null +++ b/system/topic_state_monitor/launch/topic_state_monitor_tf.launch.xml @@ -0,0 +1,26 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/system/topic_state_monitor/package.xml b/system/topic_state_monitor/package.xml index 13c182cc715ee..2707a74ff5413 100644 --- a/system/topic_state_monitor/package.xml +++ b/system/topic_state_monitor/package.xml @@ -15,6 +15,7 @@ diagnostic_updater rclcpp rclcpp_components + tf2_msgs ament_lint_auto autoware_lint_common diff --git a/system/topic_state_monitor/src/topic_state_monitor_core.cpp b/system/topic_state_monitor/src/topic_state_monitor_core.cpp index 739826c3f4b04..326193a58dc4e 100644 --- a/system/topic_state_monitor/src/topic_state_monitor_core.cpp +++ b/system/topic_state_monitor/src/topic_state_monitor_core.cpp @@ -40,13 +40,22 @@ TopicStateMonitorNode::TopicStateMonitorNode(const rclcpp::NodeOptions & node_op : Node("topic_state_monitor", node_options), updater_(this) { using std::placeholders::_1; + // Parameter node_param_.update_rate = declare_parameter("update_rate", 10.0); - param_.topic = declare_parameter("topic"); - param_.topic_type = declare_parameter("topic_type"); - param_.transient_local = declare_parameter("transient_local", false); - param_.best_effort = declare_parameter("best_effort", false); - param_.diag_name = declare_parameter("diag_name"); + node_param_.topic = declare_parameter("topic"); + node_param_.transient_local = declare_parameter("transient_local", false); + node_param_.best_effort = declare_parameter("best_effort", false); + node_param_.diag_name = declare_parameter("diag_name"); + node_param_.is_transform = (node_param_.topic == "/tf" || node_param_.topic == "/tf_static"); + + if (node_param_.is_transform) { + node_param_.frame_id = declare_parameter("frame_id"); + node_param_.child_frame_id = declare_parameter("child_frame_id"); + } else { + node_param_.topic_type = declare_parameter("topic_type"); + } + param_.warn_rate = declare_parameter("warn_rate", 0.5); param_.error_rate = declare_parameter("error_rate", 0.1); param_.timeout = declare_parameter("timeout", 1.0); @@ -62,21 +71,35 @@ TopicStateMonitorNode::TopicStateMonitorNode(const rclcpp::NodeOptions & node_op // Subscriber rclcpp::QoS qos = rclcpp::QoS{1}; - if (param_.transient_local) { + if (node_param_.transient_local) { qos.transient_local(); } - if (param_.best_effort) { + if (node_param_.best_effort) { qos.best_effort(); } - sub_topic_ = this->create_generic_subscription( - param_.topic, param_.topic_type, qos, - [this]([[maybe_unused]] std::shared_ptr msg) { - topic_state_monitor_->update(); - }); + + if (node_param_.is_transform) { + sub_transform_ = this->create_subscription( + node_param_.topic, qos, [this](tf2_msgs::msg::TFMessage::ConstSharedPtr msg) { + for (const auto & transform : msg->transforms) { + if ( + transform.header.frame_id == node_param_.frame_id && + transform.child_frame_id == node_param_.child_frame_id) { + topic_state_monitor_->update(); + } + } + }); + } else { + sub_topic_ = this->create_generic_subscription( + node_param_.topic, node_param_.topic_type, qos, + [this]([[maybe_unused]] std::shared_ptr msg) { + topic_state_monitor_->update(); + }); + } // Diagnostic Updater updater_.setHardwareID("topic_state_monitor"); - updater_.add(param_.diag_name, this, &TopicStateMonitorNode::checkTopicStatus); + updater_.add(node_param_.diag_name, this, &TopicStateMonitorNode::checkTopicStatus); // Timer const auto period_ns = rclcpp::Rate(node_param_.update_rate).period(); @@ -96,6 +119,7 @@ rcl_interfaces::msg::SetParametersResult TopicStateMonitorNode::onParameter( update_param(parameters, "error_rate", param_.error_rate); update_param(parameters, "timeout", param_.timeout); update_param(parameters, "window_size", param_.window_size); + topic_state_monitor_->setParam(param_); } catch (const rclcpp::exceptions::InvalidParameterTypeException & e) { result.successful = false; result.reason = e.what(); @@ -120,7 +144,12 @@ void TopicStateMonitorNode::checkTopicStatus(diagnostic_updater::DiagnosticStatu const auto topic_rate = topic_state_monitor_->getTopicRate(); // Add topic name - stat.addf("topic", "%s", param_.topic.c_str()); + if (node_param_.is_transform) { + const auto frame = "(" + node_param_.frame_id + " to " + node_param_.child_frame_id + ")"; + stat.addf("topic", "%s %s", node_param_.topic.c_str(), frame.c_str()); + } else { + stat.addf("topic", "%s", node_param_.topic.c_str()); + } // Judge level int8_t level = DiagnosticStatus::OK; diff --git a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/README.md b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/README.md index abdfd2ffdadc1..0532b6f474818 100644 --- a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/README.md +++ b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/README.md @@ -31,12 +31,14 @@ The `rviz:=true` option displays the RViz with a calibration plugin as below.

-The current status (velocity and pedal) is shown in the plugin. The color on the current cell varies green/red depending on the current data is valid/invalid. The data that doesn't satisfy the following conditions are considered invalid and will not be used for estimation. +The current status (velocity and pedal) is shown in the plugin. The color on the current cell varies green/red depending on the current data is valid/invalid. The data that doesn't satisfy the following conditions are considered invalid and will not be used for estimation since aggressive data (e.g. when the pedal is moving fast) causes bad calibration accuracy. - The velocity and pedal conditions are within certain ranges from the index values. - The steer value, pedal speed, pitch value, etc. are less than corresponding thresholds. - The velocity is higher than a threshold. +The detailed parameters are described in the parameter section. + Note: You don't need to worry about whether the current state is red or green during calibration. Just keep getting data until all the cells turn red. The value of each cell in the map is gray at first, and it changes from blue to red as the number of valid data in the cell accumulates. It is preferable to continue the calibration until each cell of the map becomes close to red. In particular, the performance near the stop depends strongly on the velocity of 0 ~ 6m/s range and the pedal value of +0.2 ~ -0.4, range so it is desirable to focus on those areas. @@ -130,6 +132,8 @@ You can also save accel and brake map in the default directory where Autoware re ## Parameters +## System Parameters + | Name | Type | Description | Default value | | :----------------------- | :----- | :-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------------------------------------------------- | | update_method | string | you can select map calibration method. "update_offset_each_cell" calculates offsets for each grid cells on the map. "update_offset_total" calculates the total offset of the map. | "update_offset_each_cell" | @@ -138,6 +142,25 @@ You can also save accel and brake map in the default directory where Autoware re | progress_file_output | bool | if true, it will output a log and csv file of the update process. | false | | default_map_dir | str | directory of default map | [directory of *raw_vehicle_cmd_converter*]/data/default/ | | calibrated_map_dir | str | directory of calibrated map | [directory of *accel_brake_map_calibrator*]/config/ | +| update_hz | double | hz for update | 10.0 | + +## Algorithm Parameters + +| Name | Type | Description | Default value | +| :---------------------- | :----- | :-------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | +| initial_covariance | double | Covariance of initial acceleration map (larger covariance makes the update speed faster) | 0.05 | +| velocity_min_threshold | double | Speeds smaller than this are not used for updating. | 0.1 | +| velocity_diff_threshold | double | When the velocity data is more than this threshold away from the grid reference speed (center value), the associated data is not used for updating. | 0.556 | +| max_steer_threshold | double | If the steer angle is greater than this value, the associated data is not used for updating. | 0.2 | +| max_pitch_threshold | double | If the pitch angle is greater than this value, the associated data is not used for updating. | 0.02 | +| max_jerk_threshold | double | If the ego jerk calculated from ego acceleration is greater than this value, the associated data is not used for updating. | 0.7 | +| pedal_velocity_thresh | double | If the pedal moving speed is greater than this value, the associated data is not used for updating. | 0.15 | +| pedal_diff_threshold | double | If the current pedal value is more then this threshold away from the previous value, the associated data is not used for updating. | 0.03 | +| max_accel | double | Maximum value of acceleration calculated from velocity source. | 5.0 | +| min_accel | double | Minimum value of acceleration calculated from velocity source. | -5.0 | +| pedal_to_accel_delay | double | The delay time between actuation_cmd to acceleration, considered in the update logic. | 0.3 | +| update_suggest_thresh | double | threshold of RMSE ratio that update suggest flag becomes true. ( RMSE ratio: [RMSE of new map] / [RMSE of original map] ) | 0.7 | +| max_data_count | int | For visualization. When the data num of each grid gets this value, the grid color gets red. | 100 | ## Test utility scripts diff --git a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/config/accel_brake_map_calibrator.param.yaml b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/config/accel_brake_map_calibrator.param.yaml index ffadfa4edd209..1869ca2f59d1c 100644 --- a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/config/accel_brake_map_calibrator.param.yaml +++ b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/config/accel_brake_map_calibrator.param.yaml @@ -9,7 +9,6 @@ max_pitch_threshold: 0.02 max_jerk_threshold: 0.7 pedal_velocity_thresh: 0.15 - map_update_gain: 0.02 pedal_to_accel_delay: 0.3 max_accel: 5.0 min_accel: -5.0 diff --git a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/include/accel_brake_map_calibrator/accel_brake_map_calibrator_node.hpp b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/include/accel_brake_map_calibrator/accel_brake_map_calibrator_node.hpp index 8c4ba7ddc8bba..b06a446bcac44 100644 --- a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/include/accel_brake_map_calibrator/accel_brake_map_calibrator_node.hpp +++ b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/include/accel_brake_map_calibrator/accel_brake_map_calibrator_node.hpp @@ -136,7 +136,6 @@ class AccelBrakeMapCalibrator : public rclcpp::Node double max_pitch_threshold_; double max_jerk_threshold_; double pedal_velocity_thresh_; - double map_update_gain_; double max_accel_; double min_accel_; double pedal_to_accel_delay_; diff --git a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/src/accel_brake_map_calibrator_node.cpp b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/src/accel_brake_map_calibrator_node.cpp index 6d907f4430281..e36748c2d8187 100644 --- a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/src/accel_brake_map_calibrator_node.cpp +++ b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/src/accel_brake_map_calibrator_node.cpp @@ -40,7 +40,6 @@ AccelBrakeMapCalibrator::AccelBrakeMapCalibrator(const rclcpp::NodeOptions & nod max_pitch_threshold_ = this->declare_parameter("max_pitch_threshold", 0.02); max_jerk_threshold_ = this->declare_parameter("max_jerk_threshold", 0.7); pedal_velocity_thresh_ = this->declare_parameter("pedal_velocity_thresh", 0.15); - map_update_gain_ = this->declare_parameter("map_update_gain", 0.02); max_accel_ = this->declare_parameter("max_accel", 5.0); min_accel_ = this->declare_parameter("min_accel", -5.0); pedal_to_accel_delay_ = this->declare_parameter("pedal_to_accel_delay", 0.3); diff --git a/vehicle/raw_vehicle_cmd_converter/src/node.cpp b/vehicle/raw_vehicle_cmd_converter/src/node.cpp index b72bf64cec33c..20799155b86ad 100644 --- a/vehicle/raw_vehicle_cmd_converter/src/node.cpp +++ b/vehicle/raw_vehicle_cmd_converter/src/node.cpp @@ -102,7 +102,9 @@ void RawVehicleCommandConverterNode::publishActuationCmd() } if (!current_twist_ptr_ || !control_cmd_ptr_ || !current_steer_ptr_) { RCLCPP_WARN_EXPRESSION( - get_logger(), is_debugging_, "some of twist/control_cmd/steer pointer is null"); + get_logger(), is_debugging_, "some pointers are null: %s, %s, %s", + !current_twist_ptr_ ? "twist" : "", !control_cmd_ptr_ ? "cmd" : "", + !current_steer_ptr_ ? "steer" : ""); return; } double desired_accel_cmd = 0.0; @@ -124,9 +126,9 @@ void RawVehicleCommandConverterNode::publishActuationCmd() if (accel_cmd_is_zero) { desired_brake_cmd = calculateBrakeMap(vel, acc); } - } else { - // if conversion is disabled use acceleration as brake cmd - desired_brake_cmd = (acc < 0) ? acc : 0; + } else if (acc < 0) { + // if conversion is disabled use negative acceleration as brake cmd + desired_brake_cmd = -acc; } if (convert_steer_cmd_) { desired_steer_cmd = calculateSteer(vel, steer, steer_rate); diff --git a/vehicle/vehicle_info_util/CMakeLists.txt b/vehicle/vehicle_info_util/CMakeLists.txt index eff57c488e0fc..f490b0680a19a 100644 --- a/vehicle/vehicle_info_util/CMakeLists.txt +++ b/vehicle/vehicle_info_util/CMakeLists.txt @@ -13,3 +13,8 @@ ament_auto_package( config launch ) + +install(PROGRAMS + scripts/min_turning_radius_calculator.py + DESTINATION lib/${PROJECT_NAME} +) diff --git a/vehicle/vehicle_info_util/Readme.md b/vehicle/vehicle_info_util/Readme.md index 03188b7072f63..c3d06a3b43260 100644 --- a/vehicle/vehicle_info_util/Readme.md +++ b/vehicle/vehicle_info_util/Readme.md @@ -8,6 +8,22 @@ This package is to get vehicle info parameters. In [here](https://autowarefoundation.github.io/autoware-documentation/main/design/autoware-interfaces/components/vehicle-dimensions/), you can check the vehicle dimensions with more detail. +### Scripts + +#### Minimum turning radius + +```sh +$ ros2 run vehicle_info_util min_turning_radius_calculator.py +yaml path is /home/autoware/pilot-auto/install/vehicle_info_util/share/vehicle_info_util/config/vehicle_info.param.yaml +Minimum turning radius is 3.253042620027102 [m] for rear, 4.253220695862465 [m] for front. +``` + +You can designate yaml file with `-y` option as follows. + +```sh +ros2 run vehicle_info_util min_turning_radius_calculator.py -y +``` + ## Assumptions / Known limits TBD. diff --git a/vehicle/vehicle_info_util/package.xml b/vehicle/vehicle_info_util/package.xml index 09a349be63e14..e963e4cda0c75 100644 --- a/vehicle/vehicle_info_util/package.xml +++ b/vehicle/vehicle_info_util/package.xml @@ -5,6 +5,12 @@ 0.1.0 The vehicle_info_util package Yamato ANDO + + + Taiki Tanaka + Tomoya Kimura + Shumpei Wakabayashi + Apache License 2.0 ament_cmake_auto diff --git a/vehicle/vehicle_info_util/scripts/min_turning_radius_calculator.py b/vehicle/vehicle_info_util/scripts/min_turning_radius_calculator.py new file mode 100755 index 0000000000000..9e25b928c8b74 --- /dev/null +++ b/vehicle/vehicle_info_util/scripts/min_turning_radius_calculator.py @@ -0,0 +1,55 @@ +#!/usr/bin/env python3 + +# Copyright 2022 Tier IV, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import argparse +import math + +from ament_index_python.packages import get_package_share_directory +import yaml + + +def main(yaml_path): + with open(yaml_path) as f: + config = yaml.safe_load(f) + + wheel_base = config["/**"]["ros__parameters"]["wheel_base"] + max_steer_angle = config["/**"]["ros__parameters"]["max_steer_angle"] + + rear_radius = wheel_base / math.tan(max_steer_angle) + front_radius = wheel_base / math.sin(max_steer_angle) + + print("yaml path is {}".format(yaml_path)) + print( + "Minimum turning radius is {} [m] for rear, {} [m] for front.".format( + rear_radius, front_radius + ) + ) + + +if __name__ == "__main__": + default_yaml_path = ( + get_package_share_directory("vehicle_info_util") + "/config/vehicle_info.param.yaml" + ) + + parser = argparse.ArgumentParser() + parser.add_argument( + "-y", "--yaml", default=default_yaml_path, help="vehicle_info.param.yaml path" + ) + + args = parser.parse_args() + yaml_path = args.yaml + + main(yaml_path)