diff --git a/.cspell-partial.json b/.cspell-partial.json index 45b85e779c39d..f45ae8961c56b 100644 --- a/.cspell-partial.json +++ b/.cspell-partial.json @@ -1,5 +1,9 @@ { - "ignorePaths": ["**/perception/**", "**/sensing/**", "perception/bytetrack/lib/**"], + "ignorePaths": [ + "**/perception/**", + "sensing/tier4_pcl_extensions/include/**", + "perception/bytetrack/lib/**" + ], "ignoreRegExpList": [], "words": [] } diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index fb6df15ab7679..9de27f82ffcef 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -14,17 +14,18 @@ common/component_interface_tools/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp m common/component_interface_utils/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp makoto.yabuta@tier4.jp yukihiro.saito@tier4.jp common/cuda_utils/** daisuke.nishimatsu@tier4.jp manato.hirabayashi@tier4.jp common/fake_test_node/** opensource@apex.ai satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp +common/geography_utils/** koji.minoda@tier4.jp common/global_parameter_loader/** ryohsuke.mitsudome@tier4.jp common/glog_component/** takamasa.horibe@tier4.jp common/goal_distance_calculator/** taiki.tanaka@tier4.jp common/grid_map_utils/** maxime.clement@tier4.jp common/interpolation/** fumiya.watanabe@tier4.jp takayuki.murooka@tier4.jp yutaka.shimizu@tier4.jp common/kalman_filter/** koji.minoda@tier4.jp takeshi.ishita@tier4.jp yukihiro.saito@tier4.jp -common/motion_utils/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp satoshi.ota@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp yutaka.shimizu@tier4.jp +common/motion_utils/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp mamoru.sobue@tier4.jp satoshi.ota@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp yutaka.shimizu@tier4.jp common/object_recognition_utils/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp takayuki.murooka@tier4.jp yusuke.muramatsu@tier4.jp common/osqp_interface/** fumiya.watanabe@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp common/path_distance_calculator/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp makoto.yabuta@tier4.jp -common/perception_utils/** mingyu.li@tier4.jp satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp yusuke.muramatsu@tier4.jp +common/perception_utils/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp yusuke.muramatsu@tier4.jp common/polar_grid/** yukihiro.saito@tier4.jp common/qp_interface/** fumiya.watanabe@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp common/rtc_manager_rviz_plugin/** taiki.tanaka@tier4.jp tomoya.kimura@tier4.jp @@ -33,18 +34,21 @@ common/tensorrt_common/** daisuke.nishimatsu@tier4.jp dan.umeda@tier4.jp manato. common/tier4_adapi_rviz_plugin/** hiroki.ota@tier4.jp isamu.takagi@tier4.jp kosuke.takeuchi@tier4.jp common/tier4_api_utils/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp makoto.yabuta@tier4.jp common/tier4_automatic_goal_rviz_plugin/** dawid.moszynski@robotec.ai shumpei.wakabayashi@tier4.jp -common/tier4_autoware_utils/** takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp yutaka.shimizu@tier4.jp +common/tier4_autoware_utils/** mamoru.sobue@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp yutaka.shimizu@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 kahhooi.tan@tier4.jp makoto.yabuta@tier4.jp common/tier4_debug_rviz_plugin/** takayuki.murooka@tier4.jp common/tier4_debug_tools/** ryohsuke.mitsudome@tier4.jp common/tier4_localization_rviz_plugin/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp makoto.yabuta@tier4.jp takamasa.horibe@tier4.jp +common/tier4_logging_level_configure_rviz_plugin/** kosuke.takeuchi@tier4.jp satoshi.ota@tier4.jp takamasa.horibe@tier4.jp common/tier4_perception_rviz_plugin/** yukihiro.saito@tier4.jp common/tier4_planning_rviz_plugin/** takayuki.murooka@tier4.jp yukihiro.saito@tier4.jp yutaka.shimizu@tier4.jp common/tier4_screen_capture_rviz_plugin/** taiki.tanaka@tier4.jp common/tier4_simulated_clock_rviz_plugin/** maxime.clement@tier4.jp common/tier4_state_rviz_plugin/** hiroki.ota@tier4.jp isamu.takagi@tier4.jp +common/tier4_system_rviz_plugin/** koji.minoda@tier4.jp +common/tier4_target_object_type_rviz_plugin/** takamasa.horibe@tier4.jp common/tier4_traffic_light_rviz_plugin/** satoshi.ota@tier4.jp common/tier4_vehicle_rviz_plugin/** yukihiro.saito@tier4.jp common/time_utils/** christopherj.ho@gmail.com shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp @@ -52,7 +56,7 @@ common/traffic_light_recognition_marker_publisher/** shumpei.wakabayashi@tier4.j common/traffic_light_utils/** mingyu.li@tier4.jp shunsuke.miura@tier4.jp common/trtexec_vendor/** daisuke.nishimatsu@tier4.jp yusuke.muramatsu@tier4.jp common/tvm_utility/** ambroise.vincent@arm.com xinyu.wang@tier4.jp -control/autonomous_emergency_braking/** takamasa.horibe@tier4.jp yutaka.shimizu@tier4.jp +control/autonomous_emergency_braking/** takamasa.horibe@tier4.jp tomoya.kimura@tier4.jp yutaka.shimizu@tier4.jp control/control_performance_analysis/** berkay@leodrive.ai fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp control/control_validator/** kyoichi.sugahara@tier4.jp makoto.kurihara@tier4.jp takamasa.horibe@tier4.jp control/external_cmd_selector/** fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp @@ -62,6 +66,7 @@ control/mpc_lateral_controller/** takamasa.horibe@tier4.jp takayuki.murooka@tier control/obstacle_collision_checker/** fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp control/operation_mode_transition_manager/** takamasa.horibe@tier4.jp tomoya.kimura@tier4.jp control/pid_longitudinal_controller/** takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp +control/predicted_path_checker/** berkay@leodrive.ai control/pure_pursuit/** takamasa.horibe@tier4.jp control/shift_decider/** takamasa.horibe@tier4.jp control/trajectory_follower_base/** takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp @@ -81,10 +86,11 @@ launch/tier4_sensing_launch/** yukihiro.saito@tier4.jp launch/tier4_simulator_launch/** keisuke.shima@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp launch/tier4_system_launch/** akihiro.sakurai@tier4.jp fumihito.ito@tier4.jp launch/tier4_vehicle_launch/** yukihiro.saito@tier4.jp -localization/ar_tag_based_localizer/** masahiro.sakamoto@tier4.jp shintaro.sakoda@tier4.jp localization/ekf_localizer/** koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp takamasa.horibe@tier4.jp takeshi.ishita@tier4.jp yamato.ando@tier4.jp localization/gyro_odometer/** masahiro.sakamoto@tier4.jp yamato.ando@tier4.jp -localization/localization_error_monitor/** masahiro.sakamoto@tier4.jp yamato.ando@tier4.jp +localization/landmark_based_localizer/ar_tag_based_localizer/** masahiro.sakamoto@tier4.jp shintaro.sakoda@tier4.jp +localization/landmark_based_localizer/landmark_tf_caster/** masahiro.sakamoto@tier4.jp shintaro.sakoda@tier4.jp +localization/localization_error_monitor/** koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp yamato.ando@tier4.jp localization/ndt_scan_matcher/** kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp yamato.ando@tier4.jp localization/pose2twist/** masahiro.sakamoto@tier4.jp yamato.ando@tier4.jp localization/pose_initializer/** isamu.takagi@tier4.jp masahiro.sakamoto@tier4.jp yamato.ando@tier4.jp @@ -101,8 +107,9 @@ map/map_projection_loader/** koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp yam map/map_tf_generator/** azumi.suzuki@tier4.jp map/util/lanelet2_map_preprocessor/** ryohsuke.mitsudome@tier4.jp perception/bytetrack/** manato.hirabayashi@tier4.jp +perception/cluster_merger/** dai.nguyen@tier4.jp shunsuke.miura@tier4.jp yukihiro.saito@tier4.jp perception/compare_map_segmentation/** abrahammonrroy@yahoo.com dai.nguyen@tier4.jp yukihiro.saito@tier4.jp -perception/crosswalk_traffic_light_estimator/** satoshi.ota@tier4.jp +perception/crosswalk_traffic_light_estimator/** satoshi.ota@tier4.jp shunsuke.miura@tier4.jp tao.zhong@tier4.jp perception/detected_object_feature_remover/** tomoya.kimura@tier4.jp perception/detected_object_validation/** shunsuke.miura@tier4.jp yukihiro.saito@tier4.jp perception/detection_by_tracker/** yoshi.ri@tier4.jp yukihiro.saito@tier4.jp @@ -117,7 +124,7 @@ perception/lidar_apollo_segmentation_tvm/** ambroise.vincent@arm.com xinyu.wang@ perception/lidar_apollo_segmentation_tvm_nodes/** ambroise.vincent@arm.com xinyu.wang@tier4.jp perception/lidar_centerpoint/** kenzo.lobos@tier4.jp yusuke.muramatsu@tier4.jp perception/lidar_centerpoint_tvm/** carl.liu@autocore.ai xinyu.wang@tier4.jp -perception/map_based_prediction/** shunsuke.miura@tier4.jp tomoya.kimura@tier4.jp yoshi.ri@tier4.jp yutaka.shimizu@tier4.jp +perception/map_based_prediction/** kotaro.uetake@tier4.jp kyoichi.sugahara@tier4.jp shunsuke.miura@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp yoshi.ri@tier4.jp yutaka.shimizu@tier4.jp perception/multi_object_tracker/** yoshi.ri@tier4.jp yukihiro.saito@tier4.jp perception/object_merger/** yoshi.ri@tier4.jp yukihiro.saito@tier4.jp perception/object_range_splitter/** yukihiro.saito@tier4.jp @@ -134,19 +141,20 @@ perception/simple_object_merger/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4. perception/tensorrt_classifier/** mingyu.li@tier4.jp perception/tensorrt_yolo/** daisuke.nishimatsu@tier4.jp perception/tensorrt_yolox/** daisuke.nishimatsu@tier4.jp dan.umeda@tier4.jp manato.hirabayashi@tier4.jp +perception/tracking_object_merger/** yoshi.ri@tier4.jp yukihiro.saito@tier4.jp perception/traffic_light_arbiter/** kenzo.lobos@tier4.jp shunsuke.miura@tier4.jp perception/traffic_light_classifier/** shunsuke.miura@tier4.jp yukihiro.saito@tier4.jp -perception/traffic_light_fine_detector/** mingyu.li@tier4.jp shunsuke.miura@tier4.jp +perception/traffic_light_fine_detector/** shunsuke.miura@tier4.jp tao.zhong@tier4.jp perception/traffic_light_map_based_detector/** yukihiro.saito@tier4.jp -perception/traffic_light_multi_camera_fusion/** mingyu.li@tier4.jp shunsuke.miura@tier4.jp -perception/traffic_light_occlusion_predictor/** mingyu.li@tier4.jp shunsuke.miura@tier4.jp +perception/traffic_light_multi_camera_fusion/** shunsuke.miura@tier4.jp tao.zhong@tier4.jp +perception/traffic_light_occlusion_predictor/** shunsuke.miura@tier4.jp tao.zhong@tier4.jp perception/traffic_light_ssd_fine_detector/** daisuke.nishimatsu@tier4.jp perception/traffic_light_visualization/** yukihiro.saito@tier4.jp planning/behavior_path_planner/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp yutaka.shimizu@tier4.jp zulfaqar.azmi@tier4.jp planning/behavior_velocity_blind_spot_module/** mamoru.sobue@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_crosswalk_module/** satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_detection_area_module/** kyoichi.sugahara@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp -planning/behavior_velocity_intersection_module/** mamoru.sobue@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp +planning/behavior_velocity_intersection_module/** kyoichi.sugahara@tier4.jp mamoru.sobue@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_no_drivable_lane_module/** ahmed.ebrahim@leodrive.ai fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_no_stopping_area_module/** kosuke.takeuchi@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_occlusion_spot_module/** shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp tomoya.kimura@tier4.jp @@ -156,6 +164,7 @@ planning/behavior_velocity_planner_common/** isamu.takagi@tier4.jp shumpei.wakab planning/behavior_velocity_run_out_module/** makoto.kurihara@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_speed_bump_module/** mdogru@leodrive.ai shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_stop_line_module/** shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp yutaka.shimizu@tier4.jp +planning/behavior_velocity_template_module/** daniel.sanchez@tier4.jp planning/behavior_velocity_traffic_light_module/** satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_virtual_traffic_light_module/** kosuke.takeuchi@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_walkway_module/** satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp @@ -166,15 +175,14 @@ planning/freespace_planning_algorithms/** kosuke.takeuchi@tier4.jp takamasa.hori planning/mission_planner/** isamu.takagi@tier4.jp kosuke.takeuchi@tier4.jp ryohsuke.mitsudome@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp yutaka.shimizu@tier4.jp planning/motion_velocity_smoother/** fumiya.watanabe@tier4.jp makoto.kurihara@tier4.jp takamasa.horibe@tier4.jp yutaka.shimizu@tier4.jp planning/obstacle_avoidance_planner/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp -planning/obstacle_cruise_planner/** takayuki.murooka@tier4.jp yutaka.shimizu@tier4.jp +planning/obstacle_cruise_planner/** kosuke.takeuchi@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp yutaka.shimizu@tier4.jp planning/obstacle_stop_planner/** berkay@leodrive.ai satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp tomoya.kimura@tier4.jp planning/obstacle_velocity_limiter/** maxime.clement@tier4.jp planning/path_smoother/** maxime.clement@tier4.jp takayuki.murooka@tier4.jp planning/planning_debug_tools/** taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp planning/planning_test_utils/** kyoichi.sugahara@tier4.jp takamasa.horibe@tier4.jp -planning/planning_validator/** takamasa.horibe@tier4.jp yutaka.shimizu@tier4.jp -planning/route_handler/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp takayuki.murooka@tier4.jp yutaka.shimizu@tier4.jp zulfaqar.azmi@tier4.jp -planning/rtc_auto_mode_manager/** fumiya.watanabe@tier4.jp taiki.tanaka@tier4.jp +planning/planning_validator/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp +planning/route_handler/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp mamoru.sobue@tier4.jp takayuki.murooka@tier4.jp yutaka.shimizu@tier4.jp zulfaqar.azmi@tier4.jp planning/rtc_interface/** fumiya.watanabe@tier4.jp taiki.tanaka@tier4.jp planning/rtc_replayer/** fumiya.watanabe@tier4.jp taiki.tanaka@tier4.jp planning/sampling_based_planner/bezier_sampler/** maxime.clement@tier4.jp @@ -184,7 +192,7 @@ planning/sampling_based_planner/sampler_common/** maxime.clement@tier4.jp planning/scenario_selector/** fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp planning/static_centerline_optimizer/** kosuke.takeuchi@tier4.jp takayuki.murooka@tier4.jp planning/surround_obstacle_checker/** satoshi.ota@tier4.jp -sensing/gnss_poser/** yamato.ando@tier4.jp +sensing/gnss_poser/** koji.minoda@tier4.jp yamato.ando@tier4.jp sensing/image_diagnostics/** dai.nguyen@tier4.jp sensing/image_transport_decompressor/** yukihiro.saito@tier4.jp sensing/imu_corrector/** koji.minoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp @@ -208,9 +216,12 @@ system/default_ad_api_helpers/ad_api_visualizers/** isamu.takagi@tier4.jp kahhoo system/default_ad_api_helpers/automatic_pose_initializer/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp makoto.yabuta@tier4.jp ryohsuke.mitsudome@tier4.jp yukihiro.saito@tier4.jp system/dummy_diag_publisher/** akihiro.sakurai@tier4.jp fumihito.ito@tier4.jp system/dummy_infrastructure/** ryohsuke.mitsudome@tier4.jp +system/duplicated_node_checker/** shumpei.wakabayashi@tier4.jp uken.ryu@tier4.jp system/emergency_handler/** makoto.kurihara@tier4.jp system/mrm_comfortable_stop_operator/** makoto.kurihara@tier4.jp system/mrm_emergency_stop_operator/** makoto.kurihara@tier4.jp +system/system_diagnostic_graph/** isamu.takagi@tier4.jp +system/system_diagnostic_monitor/** isamu.takagi@tier4.jp system/system_error_monitor/** fumihito.ito@tier4.jp system/system_monitor/** akihiro.sakurai@tier4.jp fumihito.ito@tier4.jp system/topic_state_monitor/** ryohsuke.mitsudome@tier4.jp @@ -220,4 +231,4 @@ vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/** taiki.tanaka@ti vehicle/external_cmd_converter/** takamasa.horibe@tier4.jp vehicle/raw_vehicle_cmd_converter/** makoto.kurihara@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp vehicle/steer_offset_estimator/** taiki.tanaka@tier4.jp -vehicle/vehicle_info_util/** shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp tomoya.kimura@tier4.jp yamato.ando@tier4.jp +vehicle/vehicle_info_util/** shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp tomoya.kimura@tier4.jp diff --git a/.github/sync-files.yaml b/.github/sync-files.yaml index 4a1f20cada359..f9cb3065f12a7 100644 --- a/.github/sync-files.yaml +++ b/.github/sync-files.yaml @@ -42,20 +42,6 @@ - \"\" - -cuda include:" {source} - - source: .github/workflows/build-and-test-differential.yaml - pre-commands: | - sd "container: ros:(\w+)" "container: ghcr.io/autowarefoundation/autoware-universe:\$1-latest" {source} - - sd -s 'container: ${{ matrix.container }}' 'container: ${{ matrix.container }}${{ matrix.container-suffix }}' {source} - sd -- \ - " include:" \ - " container-suffix: - - \"\" - - -cuda - include:" {source} - - sd "^ container: ghcr.io/autowarefoundation/autoware-universe:(\w+)-latest\$" \ - " container: ghcr.io/autowarefoundation/autoware-universe:\$1-latest-cuda" {source} - source: .github/workflows/build-and-test-differential-self-hosted.yaml pre-commands: | sd "container: ros:(\w+)" "container: ghcr.io/autowarefoundation/autoware-universe:\$1-latest" {source} diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp index b20a835f80e39..10bf11847c2f5 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp @@ -146,6 +146,11 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase m_marker_common.addMessage(markers_ptr); } + void deleteMarker(rviz_default_plugins::displays::MarkerID marker_id) + { + m_marker_common.deleteMarker(marker_id); + } + protected: /// \brief Convert given shape msg into a Marker /// \tparam ClassificationContainerT List type with ObjectClassificationMsg diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/predicted_objects_display.hpp b/common/autoware_auto_perception_rviz_plugin/include/object_detection/predicted_objects_display.hpp index ee2c7e0d5c3b9..5493f1dd594ce 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/predicted_objects_display.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/object_detection/predicted_objects_display.hpp @@ -24,6 +24,7 @@ #include #include +#include #include #include #include @@ -112,6 +113,7 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC PredictedObjectsDisplay std::mutex mutex; std::condition_variable condition; std::vector markers; + std::set existing_marker_ids; }; } // namespace object_detection diff --git a/common/autoware_auto_perception_rviz_plugin/src/object_detection/predicted_objects_display.cpp b/common/autoware_auto_perception_rviz_plugin/src/object_detection/predicted_objects_display.cpp index 870b8f2c77543..24e67a6f44e95 100644 --- a/common/autoware_auto_perception_rviz_plugin/src/object_detection/predicted_objects_display.cpp +++ b/common/autoware_auto_perception_rviz_plugin/src/object_detection/predicted_objects_display.cpp @@ -15,6 +15,7 @@ #include #include +#include namespace autoware { @@ -130,7 +131,7 @@ std::vector PredictedObjectsDisplay: auto acceleration_text_marker_ptr = acceleration_text_marker.value(); acceleration_text_marker_ptr->header = msg->header; acceleration_text_marker_ptr->id = uuid_to_marker_id(object.object_id); - add_marker(acceleration_text_marker_ptr); + markers.push_back(acceleration_text_marker_ptr); } // Get marker for twist @@ -195,11 +196,19 @@ void PredictedObjectsDisplay::update(float wall_dt, float ros_dt) std::unique_lock lock(mutex); if (!markers.empty()) { - clear_markers(); - + std::set new_marker_ids = std::set(); for (const auto & marker : markers) { + rviz_default_plugins::displays::MarkerID marker_id = + rviz_default_plugins::displays::MarkerID(marker->ns, marker->id); add_marker(marker); + new_marker_ids.insert(marker_id); + } + for (auto itr = existing_marker_ids.begin(); itr != existing_marker_ids.end(); itr++) { + if (new_marker_ids.find(*itr) == new_marker_ids.end()) { + deleteMarker(*itr); + } } + existing_marker_ids = new_marker_ids; markers.clear(); } diff --git a/common/autoware_auto_tf2/include/autoware_auto_tf2/tf2_autoware_auto_msgs.hpp b/common/autoware_auto_tf2/include/autoware_auto_tf2/tf2_autoware_auto_msgs.hpp index 9b20953a7b3c5..b35eb6e93ce6e 100644 --- a/common/autoware_auto_tf2/include/autoware_auto_tf2/tf2_autoware_auto_msgs.hpp +++ b/common/autoware_auto_tf2/include/autoware_auto_tf2/tf2_autoware_auto_msgs.hpp @@ -45,56 +45,6 @@ using BoundingBox = autoware_auto_perception_msgs::msg::BoundingBox; namespace tf2 { - -/*************/ -/** Point32 **/ -/*************/ - -/** \brief Apply a geometry_msgs TransformStamped to a geometry_msgs Point32 type. - * This function is a specialization of the doTransform template defined in tf2/convert.h. - * \param t_in The point to transform, as a Point32 message. - * \param t_out The transformed point, as a Point32 message. - * \param transform The timestamped transform to apply, as a TransformStamped message. - */ -template <> -inline void doTransform( - const geometry_msgs::msg::Point32 & t_in, geometry_msgs::msg::Point32 & t_out, - const geometry_msgs::msg::TransformStamped & transform) -{ - const KDL::Vector v_out = gmTransformToKDL(transform) * KDL::Vector(t_in.x, t_in.y, t_in.z); - t_out.x = static_cast(v_out[0]); - t_out.y = static_cast(v_out[1]); - t_out.z = static_cast(v_out[2]); -} - -/*************/ -/** Polygon **/ -/*************/ - -/** \brief Apply a geometry_msgs TransformStamped to a geometry_msgs Polygon type. - * This function is a specialization of the doTransform template defined in tf2/convert.h. - * \param t_in The polygon to transform. - * \param t_out The transformed polygon. - * \param transform The timestamped transform to apply, as a TransformStamped message. - */ -template <> -inline void doTransform( - const geometry_msgs::msg::Polygon & t_in, geometry_msgs::msg::Polygon & t_out, - const geometry_msgs::msg::TransformStamped & transform) -{ - // Don't call the Point32 doTransform to avoid doing this conversion every time - const auto kdl_frame = gmTransformToKDL(transform); - // We don't use std::back_inserter to allow aliasing between t_in and t_out - t_out.points.resize(t_in.points.size()); - for (size_t i = 0; i < t_in.points.size(); ++i) { - const KDL::Vector v_out = - kdl_frame * KDL::Vector(t_in.points[i].x, t_in.points[i].y, t_in.points[i].z); - t_out.points[i].x = static_cast(v_out[0]); - t_out.points[i].y = static_cast(v_out[1]); - t_out.points[i].z = static_cast(v_out[2]); - } -} - /******************/ /** Quaternion32 **/ /******************/ diff --git a/common/component_interface_specs/CMakeLists.txt b/common/component_interface_specs/CMakeLists.txt index 31c723600183f..b519a0ddce86a 100644 --- a/common/component_interface_specs/CMakeLists.txt +++ b/common/component_interface_specs/CMakeLists.txt @@ -4,4 +4,38 @@ project(component_interface_specs) find_package(autoware_cmake REQUIRED) autoware_package() +include_directories( + include + SYSTEM + ${rclcpp_INCLUDE_DIRS} + ${rosidl_runtime_cpp_INCLUDE_DIRS} + ${rcl_INCLUDE_DIRS} + ${autoware_adapi_v1_msgs_INCLUDE_DIRS} + ${autoware_auto_planning_msgs_INCLUDE_DIRS} + ${autoware_planning_msgs_INCLUDE_DIRS} + ${autoware_auto_vehicle_msgs_INCLUDE_DIRS} + ${tier4_control_msgs_INCLUDE_DIRS} + ${nav_msgs_INCLUDE_DIRS} + ${tier4_system_msgs_INCLUDE_DIRS} + ${tier4_vehicle_msgs_INCLUDE_DIRS} + ${autoware_auto_perception_msgs_INCLUDE_DIRS} + ${tier4_map_msgs_INCLUDE_DIRS} +) + +if(BUILD_TESTING) + ament_add_ros_isolated_gtest(test_component_interface_specs + test/gtest_main.cpp + test/test_planning.cpp + test/test_control.cpp + test/test_localization.cpp + test/test_system.cpp + test/test_map.cpp + test/test_perception.cpp + test/test_vehicle.cpp + ) + target_include_directories(test_component_interface_specs + PRIVATE include + ) +endif() + ament_auto_package() diff --git a/common/component_interface_specs/package.xml b/common/component_interface_specs/package.xml index 7ebc3f0d1bea9..93f77c651869d 100644 --- a/common/component_interface_specs/package.xml +++ b/common/component_interface_specs/package.xml @@ -11,13 +11,28 @@ Apache License 2.0 ament_cmake_auto + ament_cmake_core + ament_cmake_export_dependencies + ament_cmake_test autoware_cmake + ament_cmake_core + ament_cmake_test + autoware_adapi_v1_msgs + autoware_auto_perception_msgs + autoware_auto_planning_msgs autoware_auto_vehicle_msgs + autoware_planning_msgs + nav_msgs + rcl + rclcpp + rosidl_runtime_cpp + tier4_control_msgs tier4_map_msgs + tier4_system_msgs tier4_vehicle_msgs - + ament_cmake_ros ament_lint_auto autoware_lint_common diff --git a/common/motion_utils/src/motion_utils.cpp b/common/component_interface_specs/test/gtest_main.cpp similarity index 76% rename from common/motion_utils/src/motion_utils.cpp rename to common/component_interface_specs/test/gtest_main.cpp index e1c86d1ad2bfd..81d9d5345270d 100644 --- a/common/motion_utils/src/motion_utils.cpp +++ b/common/component_interface_specs/test/gtest_main.cpp @@ -1,4 +1,4 @@ -// Copyright 2022 Tier IV, Inc. +// Copyright 2023 The Autoware Contributors // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,4 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "motion_utils/motion_utils.hpp" +#include "gtest/gtest.h" + +int main(int argc, char * argv[]) +{ + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/common/component_interface_specs/test/test_control.cpp b/common/component_interface_specs/test/test_control.cpp new file mode 100644 index 0000000000000..366641eacbd23 --- /dev/null +++ b/common/component_interface_specs/test/test_control.cpp @@ -0,0 +1,46 @@ +// Copyright 2023 The Autoware Contributors +// +// 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 "component_interface_specs/control.hpp" +#include "gtest/gtest.h" + +TEST(control, interface) +{ + { + using control_interface::IsPaused; + IsPaused is_paused; + size_t depth = 1; + EXPECT_EQ(is_paused.depth, depth); + EXPECT_EQ(is_paused.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE); + EXPECT_EQ(is_paused.durability, RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL); + } + + { + using control_interface::IsStartRequested; + IsStartRequested is_start_requested; + size_t depth = 1; + EXPECT_EQ(is_start_requested.depth, depth); + EXPECT_EQ(is_start_requested.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE); + EXPECT_EQ(is_start_requested.durability, RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL); + } + + { + using control_interface::IsStopped; + IsStopped is_stopped; + size_t depth = 1; + EXPECT_EQ(is_stopped.depth, depth); + EXPECT_EQ(is_stopped.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE); + EXPECT_EQ(is_stopped.durability, RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL); + } +} diff --git a/common/component_interface_specs/test/test_localization.cpp b/common/component_interface_specs/test/test_localization.cpp new file mode 100644 index 0000000000000..31d8e139bd69a --- /dev/null +++ b/common/component_interface_specs/test/test_localization.cpp @@ -0,0 +1,46 @@ +// Copyright 2023 The Autoware Contributors +// +// 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 "component_interface_specs/localization.hpp" +#include "gtest/gtest.h" + +TEST(localization, interface) +{ + { + using localization_interface::InitializationState; + InitializationState initialization_state; + size_t depth = 1; + EXPECT_EQ(initialization_state.depth, depth); + EXPECT_EQ(initialization_state.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE); + EXPECT_EQ(initialization_state.durability, RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL); + } + + { + using localization_interface::KinematicState; + KinematicState kinematic_state; + size_t depth = 1; + EXPECT_EQ(kinematic_state.depth, depth); + EXPECT_EQ(kinematic_state.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE); + EXPECT_EQ(kinematic_state.durability, RMW_QOS_POLICY_DURABILITY_VOLATILE); + } + + { + using localization_interface::Acceleration; + Acceleration acceleration; + size_t depth = 1; + EXPECT_EQ(acceleration.depth, depth); + EXPECT_EQ(acceleration.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE); + EXPECT_EQ(acceleration.durability, RMW_QOS_POLICY_DURABILITY_VOLATILE); + } +} diff --git a/common/component_interface_specs/test/test_map.cpp b/common/component_interface_specs/test/test_map.cpp new file mode 100644 index 0000000000000..a65f2cb7120d1 --- /dev/null +++ b/common/component_interface_specs/test/test_map.cpp @@ -0,0 +1,28 @@ +// Copyright 2023 The Autoware Contributors +// +// 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 "component_interface_specs/map.hpp" +#include "gtest/gtest.h" + +TEST(map, interface) +{ + { + using map_interface::MapProjectorInfo; + MapProjectorInfo map_projector; + size_t depth = 1; + EXPECT_EQ(map_projector.depth, depth); + EXPECT_EQ(map_projector.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE); + EXPECT_EQ(map_projector.durability, RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL); + } +} diff --git a/common/component_interface_specs/test/test_perception.cpp b/common/component_interface_specs/test/test_perception.cpp new file mode 100644 index 0000000000000..ec80c06bb119a --- /dev/null +++ b/common/component_interface_specs/test/test_perception.cpp @@ -0,0 +1,28 @@ +// Copyright 2023 The Autoware Contributors +// +// 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 "component_interface_specs/perception.hpp" +#include "gtest/gtest.h" + +TEST(perception, interface) +{ + { + using perception_interface::ObjectRecognition; + ObjectRecognition object_recognition; + size_t depth = 1; + EXPECT_EQ(object_recognition.depth, depth); + EXPECT_EQ(object_recognition.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE); + EXPECT_EQ(object_recognition.durability, RMW_QOS_POLICY_DURABILITY_VOLATILE); + } +} diff --git a/common/component_interface_specs/test/test_planning.cpp b/common/component_interface_specs/test/test_planning.cpp new file mode 100644 index 0000000000000..8c504cb119854 --- /dev/null +++ b/common/component_interface_specs/test/test_planning.cpp @@ -0,0 +1,64 @@ +// Copyright 2023 The Autoware Contributors +// +// 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 "component_interface_specs/planning.hpp" +#include "gtest/gtest.h" + +TEST(planning, interface) +{ + { + using planning_interface::RouteState; + RouteState state; + size_t depth = 1; + EXPECT_EQ(state.depth, depth); + EXPECT_EQ(state.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE); + EXPECT_EQ(state.durability, RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL); + } + + { + using planning_interface::Route; + Route route; + size_t depth = 1; + EXPECT_EQ(route.depth, depth); + EXPECT_EQ(route.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE); + EXPECT_EQ(route.durability, RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL); + } + + { + using planning_interface::NormalRoute; + NormalRoute route; + size_t depth = 1; + EXPECT_EQ(route.depth, depth); + EXPECT_EQ(route.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE); + EXPECT_EQ(route.durability, RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL); + } + + { + using planning_interface::MrmRoute; + MrmRoute route; + size_t depth = 1; + EXPECT_EQ(route.depth, depth); + EXPECT_EQ(route.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE); + EXPECT_EQ(route.durability, RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL); + } + + { + using planning_interface::Trajectory; + Trajectory trajectory; + size_t depth = 1; + EXPECT_EQ(trajectory.depth, depth); + EXPECT_EQ(trajectory.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE); + EXPECT_EQ(trajectory.durability, RMW_QOS_POLICY_DURABILITY_VOLATILE); + } +} diff --git a/common/component_interface_specs/test/test_system.cpp b/common/component_interface_specs/test/test_system.cpp new file mode 100644 index 0000000000000..416d2effad766 --- /dev/null +++ b/common/component_interface_specs/test/test_system.cpp @@ -0,0 +1,37 @@ +// Copyright 2023 The Autoware Contributors +// +// 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 "component_interface_specs/system.hpp" +#include "gtest/gtest.h" + +TEST(system, interface) +{ + { + using system_interface::MrmState; + MrmState state; + size_t depth = 1; + EXPECT_EQ(state.depth, depth); + EXPECT_EQ(state.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE); + EXPECT_EQ(state.durability, RMW_QOS_POLICY_DURABILITY_VOLATILE); + } + + { + using system_interface::OperationModeState; + OperationModeState state; + size_t depth = 1; + EXPECT_EQ(state.depth, depth); + EXPECT_EQ(state.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE); + EXPECT_EQ(state.durability, RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL); + } +} diff --git a/common/component_interface_specs/test/test_vehicle.cpp b/common/component_interface_specs/test/test_vehicle.cpp new file mode 100644 index 0000000000000..1f2041b6cb79e --- /dev/null +++ b/common/component_interface_specs/test/test_vehicle.cpp @@ -0,0 +1,64 @@ +// Copyright 2023 The Autoware Contributors +// +// 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 "component_interface_specs/vehicle.hpp" +#include "gtest/gtest.h" + +TEST(vehicle, interface) +{ + { + using vehicle_interface::SteeringStatus; + SteeringStatus status; + size_t depth = 1; + EXPECT_EQ(status.depth, depth); + EXPECT_EQ(status.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE); + EXPECT_EQ(status.durability, RMW_QOS_POLICY_DURABILITY_VOLATILE); + } + + { + using vehicle_interface::GearStatus; + GearStatus status; + size_t depth = 1; + EXPECT_EQ(status.depth, depth); + EXPECT_EQ(status.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE); + EXPECT_EQ(status.durability, RMW_QOS_POLICY_DURABILITY_VOLATILE); + } + + { + using vehicle_interface::TurnIndicatorStatus; + TurnIndicatorStatus status; + size_t depth = 1; + EXPECT_EQ(status.depth, depth); + EXPECT_EQ(status.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE); + EXPECT_EQ(status.durability, RMW_QOS_POLICY_DURABILITY_VOLATILE); + } + + { + using vehicle_interface::HazardLightStatus; + HazardLightStatus status; + size_t depth = 1; + EXPECT_EQ(status.depth, depth); + EXPECT_EQ(status.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE); + EXPECT_EQ(status.durability, RMW_QOS_POLICY_DURABILITY_VOLATILE); + } + + { + using vehicle_interface::EnergyStatus; + EnergyStatus status; + size_t depth = 1; + EXPECT_EQ(status.depth, depth); + EXPECT_EQ(status.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE); + EXPECT_EQ(status.durability, RMW_QOS_POLICY_DURABILITY_VOLATILE); + } +} diff --git a/common/component_interface_utils/CMakeLists.txt b/common/component_interface_utils/CMakeLists.txt index d4f6343e381c7..435d5535e0a37 100644 --- a/common/component_interface_utils/CMakeLists.txt +++ b/common/component_interface_utils/CMakeLists.txt @@ -3,4 +3,24 @@ project(component_interface_utils) find_package(autoware_cmake REQUIRED) autoware_package() + +include_directories( + include + SYSTEM + ${rclcpp_INCLUDE_DIRS} + ${rosidl_runtime_cpp_INCLUDE_DIRS} + ${rcl_INCLUDE_DIRS} + ${autoware_adapi_v1_msgs_INCLUDE_DIRS} +) + +if(BUILD_TESTING) + ament_add_ros_isolated_gtest(test_component_interface_utils + test/test_component_interface_utils.cpp + ) + + target_include_directories(test_component_interface_utils + PRIVATE include + ) +endif() + ament_auto_package() diff --git a/common/component_interface_utils/package.xml b/common/component_interface_utils/package.xml old mode 100644 new mode 100755 index a1803a35cc1c9..dc82fda3f5c64 --- a/common/component_interface_utils/package.xml +++ b/common/component_interface_utils/package.xml @@ -19,6 +19,7 @@ rclcpp rclcpp_components + ament_cmake_ros ament_lint_auto autoware_lint_common diff --git a/common/component_interface_utils/test/test_component_interface_utils.cpp b/common/component_interface_utils/test/test_component_interface_utils.cpp new file mode 100644 index 0000000000000..3c41f4a85b4f9 --- /dev/null +++ b/common/component_interface_utils/test/test_component_interface_utils.cpp @@ -0,0 +1,76 @@ +// Copyright 2023 The Autoware Contributors +// +// 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 "component_interface_utils/rclcpp/exceptions.hpp" +#include "component_interface_utils/specs.hpp" +#include "component_interface_utils/status.hpp" +#include "gtest/gtest.h" + +TEST(interface, utils) +{ + { + using component_interface_utils::ServiceException; + using ResponseStatus = autoware_adapi_v1_msgs::msg::ResponseStatus; + using ResponseStatusCode = ResponseStatus::_code_type; + + ResponseStatusCode code = 10; + const std::string message = "test_exception"; + ServiceException service(code, message); + ResponseStatus code_back; + code_back = service.status(); + EXPECT_EQ(code_back.code, code); + EXPECT_EQ(code_back.message, message); + } + + { + using component_interface_utils::ServiceException; + using ResponseStatus = autoware_adapi_v1_msgs::msg::ResponseStatus; + using ResponseStatusCode = ResponseStatus::_code_type; + + ResponseStatusCode code = 10; + const std::string message = "test_exception"; + ServiceException service(code, message); + ResponseStatus code_set; + service.set(code_set); + EXPECT_EQ(code_set.code, code); + EXPECT_EQ(code_set.message, message); + } + + { + using component_interface_utils::ServiceException; + using ResponseStatus = autoware_adapi_v1_msgs::msg::ResponseStatus; + using ResponseStatusCode = ResponseStatus::_code_type; + using component_interface_utils::status::copy; + + class status_test + { + public: + status_test(ResponseStatusCode code, const std::string & message, bool success = false) + { + status.code = code; + status.message = message; + status.success = success; + } + ResponseStatus status; + }; + + const status_test status_in(10, "test_exception", true); + auto status_copy = std::make_shared(100, "test_exception_copy", false); + copy(&status_in, status_copy); + + EXPECT_EQ(status_in.status.code, status_copy->status.code); + EXPECT_EQ(status_in.status.message, status_copy->status.message); + EXPECT_EQ(status_in.status.success, status_copy->status.success); + } +} diff --git a/common/geography_utils/CMakeLists.txt b/common/geography_utils/CMakeLists.txt new file mode 100644 index 0000000000000..b705e819c059b --- /dev/null +++ b/common/geography_utils/CMakeLists.txt @@ -0,0 +1,37 @@ +cmake_minimum_required(VERSION 3.14) +project(geography_utils) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +# GeographicLib +find_package(PkgConfig) +find_path(GeographicLib_INCLUDE_DIR GeographicLib/Config.h + PATH_SUFFIXES GeographicLib +) +set(GeographicLib_INCLUDE_DIRS ${GeographicLib_INCLUDE_DIR}) +find_library(GeographicLib_LIBRARIES NAMES Geographic) + +ament_auto_add_library(geography_utils SHARED + src/height.cpp + src/projection.cpp + src/lanelet2_projector.cpp +) + +target_link_libraries(geography_utils + ${GeographicLib_LIBRARIES} +) + +if(BUILD_TESTING) + find_package(ament_cmake_ros REQUIRED) + + file(GLOB_RECURSE test_files test/*.cpp) + + ament_add_ros_isolated_gtest(test_geography_utils ${test_files}) + + target_link_libraries(test_geography_utils + geography_utils + ) +endif() + +ament_auto_package() diff --git a/common/geography_utils/README.md b/common/geography_utils/README.md new file mode 100644 index 0000000000000..fb4c2dc3a8312 --- /dev/null +++ b/common/geography_utils/README.md @@ -0,0 +1,5 @@ +# geography_utils + +## Purpose + +This package contains geography-related functions used by other packages, so please refer to them as needed. diff --git a/common/geography_utils/include/geography_utils/height.hpp b/common/geography_utils/include/geography_utils/height.hpp new file mode 100644 index 0000000000000..8bcda41f84cda --- /dev/null +++ b/common/geography_utils/include/geography_utils/height.hpp @@ -0,0 +1,33 @@ +// Copyright 2023 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 GEOGRAPHY_UTILS__HEIGHT_HPP_ +#define GEOGRAPHY_UTILS__HEIGHT_HPP_ + +#include + +namespace geography_utils +{ + +typedef double (*HeightConversionFunction)( + const double height, const double latitude, const double longitude); +double convert_wgs84_to_egm2008(const double height, const double latitude, const double longitude); +double convert_egm2008_to_wgs84(const double height, const double latitude, const double longitude); +double convert_height( + const double height, const double latitude, const double longitude, + const std::string & source_vertical_datum, const std::string & target_vertical_datum); + +} // namespace geography_utils + +#endif // GEOGRAPHY_UTILS__HEIGHT_HPP_ diff --git a/common/motion_utils/include/motion_utils/motion_utils.hpp b/common/geography_utils/include/geography_utils/lanelet2_projector.hpp similarity index 50% rename from common/motion_utils/include/motion_utils/motion_utils.hpp rename to common/geography_utils/include/geography_utils/lanelet2_projector.hpp index f6c94fbdacc93..739abe7019023 100644 --- a/common/motion_utils/include/motion_utils/motion_utils.hpp +++ b/common/geography_utils/include/geography_utils/lanelet2_projector.hpp @@ -1,4 +1,4 @@ -// Copyright 2022 Tier IV, Inc. +// Copyright 2023 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. @@ -12,16 +12,21 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MOTION_UTILS__MOTION_UTILS_HPP_ -#define MOTION_UTILS__MOTION_UTILS_HPP_ +#ifndef GEOGRAPHY_UTILS__LANELET2_PROJECTOR_HPP_ +#define GEOGRAPHY_UTILS__LANELET2_PROJECTOR_HPP_ -#include "motion_utils/constants.hpp" -#include "motion_utils/marker/marker_helper.hpp" -#include "motion_utils/resample/resample.hpp" -#include "motion_utils/trajectory/interpolation.hpp" -#include "motion_utils/trajectory/path_with_lane_id.hpp" -#include "motion_utils/trajectory/tmp_conversion.hpp" -#include "motion_utils/trajectory/trajectory.hpp" -#include "motion_utils/vehicle/vehicle_state_checker.hpp" +#include -#endif // MOTION_UTILS__MOTION_UTILS_HPP_ +#include + +#include + +namespace geography_utils +{ +using MapProjectorInfo = tier4_map_msgs::msg::MapProjectorInfo; + +std::unique_ptr get_lanelet2_projector(const MapProjectorInfo & projector_info); + +} // namespace geography_utils + +#endif // GEOGRAPHY_UTILS__LANELET2_PROJECTOR_HPP_ diff --git a/common/geography_utils/include/geography_utils/projection.hpp b/common/geography_utils/include/geography_utils/projection.hpp new file mode 100644 index 0000000000000..be616d854a466 --- /dev/null +++ b/common/geography_utils/include/geography_utils/projection.hpp @@ -0,0 +1,33 @@ +// Copyright 2023 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 GEOGRAPHY_UTILS__PROJECTION_HPP_ +#define GEOGRAPHY_UTILS__PROJECTION_HPP_ + +#include +#include +#include + +namespace geography_utils +{ +using MapProjectorInfo = tier4_map_msgs::msg::MapProjectorInfo; +using GeoPoint = geographic_msgs::msg::GeoPoint; +using LocalPoint = geometry_msgs::msg::Point; + +LocalPoint project_forward(const GeoPoint & geo_point, const MapProjectorInfo & projector_info); +GeoPoint project_reverse(const LocalPoint & local_point, const MapProjectorInfo & projector_info); + +} // namespace geography_utils + +#endif // GEOGRAPHY_UTILS__PROJECTION_HPP_ diff --git a/common/geography_utils/package.xml b/common/geography_utils/package.xml new file mode 100644 index 0000000000000..414364e36ad29 --- /dev/null +++ b/common/geography_utils/package.xml @@ -0,0 +1,27 @@ + + + + geography_utils + 0.1.0 + The geography_utils package + Koji Minoda + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + + geographic_msgs + geographiclib + geometry_msgs + lanelet2_extension + lanelet2_io + tier4_map_msgs + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/common/geography_utils/src/height.cpp b/common/geography_utils/src/height.cpp new file mode 100644 index 0000000000000..fe69557f25a76 --- /dev/null +++ b/common/geography_utils/src/height.cpp @@ -0,0 +1,63 @@ +// Copyright 2023 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 +#include + +#include +#include +#include +#include + +namespace geography_utils +{ + +double convert_wgs84_to_egm2008(const double height, const double latitude, const double longitude) +{ + GeographicLib::Geoid egm2008("egm2008-1"); + // cSpell: ignore ELLIPSOIDTOGEOID + return egm2008.ConvertHeight(latitude, longitude, height, GeographicLib::Geoid::ELLIPSOIDTOGEOID); +} + +double convert_egm2008_to_wgs84(const double height, const double latitude, const double longitude) +{ + GeographicLib::Geoid egm2008("egm2008-1"); + // cSpell: ignore GEOIDTOELLIPSOID + return egm2008.ConvertHeight(latitude, longitude, height, GeographicLib::Geoid::GEOIDTOELLIPSOID); +} + +double convert_height( + const double height, const double latitude, const double longitude, + const std::string & source_vertical_datum, const std::string & target_vertical_datum) +{ + if (source_vertical_datum == target_vertical_datum) { + return height; + } + std::map, HeightConversionFunction> conversion_map; + conversion_map[{"WGS84", "EGM2008"}] = convert_wgs84_to_egm2008; + conversion_map[{"EGM2008", "WGS84"}] = convert_egm2008_to_wgs84; + + auto key = std::make_pair(source_vertical_datum, target_vertical_datum); + if (conversion_map.find(key) != conversion_map.end()) { + return conversion_map[key](height, latitude, longitude); + } else { + std::string error_message = + "Invalid conversion types: " + std::string(source_vertical_datum.c_str()) + " to " + + std::string(target_vertical_datum.c_str()); + + throw std::invalid_argument(error_message); + } +} + +} // namespace geography_utils diff --git a/common/geography_utils/src/lanelet2_projector.cpp b/common/geography_utils/src/lanelet2_projector.cpp new file mode 100644 index 0000000000000..3e982ac2ccf4d --- /dev/null +++ b/common/geography_utils/src/lanelet2_projector.cpp @@ -0,0 +1,53 @@ +// Copyright 2023 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 +#include +#include +#include + +#include + +namespace geography_utils +{ + +std::unique_ptr get_lanelet2_projector(const MapProjectorInfo & projector_info) +{ + if (projector_info.projector_type == MapProjectorInfo::LOCAL_CARTESIAN_UTM) { + lanelet::GPSPoint position{ + projector_info.map_origin.latitude, projector_info.map_origin.longitude, + projector_info.map_origin.altitude}; + lanelet::Origin origin{position}; + lanelet::projection::UtmProjector projector{origin}; + return std::make_unique(projector); + + } else if (projector_info.projector_type == MapProjectorInfo::MGRS) { + lanelet::projection::MGRSProjector projector{}; + return std::make_unique(projector); + + } else if (projector_info.projector_type == MapProjectorInfo::TRANSVERSE_MERCATOR) { + lanelet::GPSPoint position{ + projector_info.map_origin.latitude, projector_info.map_origin.longitude, + projector_info.map_origin.altitude}; + lanelet::Origin origin{position}; + lanelet::projection::TransverseMercatorProjector projector{origin}; + return std::make_unique(projector); + } + const std::string error_msg = + "Invalid map projector type: " + projector_info.projector_type + + ". Currently supported types: MGRS, LocalCartesianUTM, and TransverseMercator"; + throw std::invalid_argument(error_msg); +} + +} // namespace geography_utils diff --git a/common/geography_utils/src/projection.cpp b/common/geography_utils/src/projection.cpp new file mode 100644 index 0000000000000..ac9ce19052702 --- /dev/null +++ b/common/geography_utils/src/projection.cpp @@ -0,0 +1,95 @@ +// Copyright 2023 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 +#include +#include +#include + +namespace geography_utils +{ + +Eigen::Vector3d to_basic_point_3d_pt(const LocalPoint src) +{ + Eigen::Vector3d dst; + dst.x() = src.x; + dst.y() = src.y; + dst.z() = src.z; + return dst; +} + +LocalPoint project_forward(const GeoPoint & geo_point, const MapProjectorInfo & projector_info) +{ + std::unique_ptr projector = get_lanelet2_projector(projector_info); + lanelet::GPSPoint position{geo_point.latitude, geo_point.longitude, geo_point.altitude}; + + lanelet::BasicPoint3d projected_local_point; + if (projector_info.projector_type == MapProjectorInfo::MGRS) { + const int mgrs_precision = 9; // set precision as 100 micro meter + const auto mgrs_projector = dynamic_cast(projector.get()); + + // project x and y using projector + // note that the altitude is ignored in MGRS projection conventionally + projected_local_point = mgrs_projector->forward(position, mgrs_precision); + } else { + // project x and y using projector + // note that the original projector such as UTM projector does not compensate for the altitude + // offset + projected_local_point = projector->forward(position); + + // correct z based on the map origin + // note that the converted altitude in local point is in the same vertical datum as the geo + // point + projected_local_point.z() = geo_point.altitude - projector_info.map_origin.altitude; + } + + LocalPoint local_point; + local_point.x = projected_local_point.x(); + local_point.y = projected_local_point.y(); + local_point.z = projected_local_point.z(); + + return local_point; +} + +GeoPoint project_reverse(const LocalPoint & local_point, const MapProjectorInfo & projector_info) +{ + std::unique_ptr projector = get_lanelet2_projector(projector_info); + + lanelet::GPSPoint projected_gps_point; + if (projector_info.projector_type == MapProjectorInfo::MGRS) { + const auto mgrs_projector = dynamic_cast(projector.get()); + // project latitude and longitude using projector + // note that the z is ignored in MGRS projection conventionally + projected_gps_point = + mgrs_projector->reverse(to_basic_point_3d_pt(local_point), projector_info.mgrs_grid); + } else { + // project latitude and longitude using projector + // note that the original projector such as UTM projector does not compensate for the altitude + // offset + projected_gps_point = projector->reverse(to_basic_point_3d_pt(local_point)); + + // correct altitude based on the map origin + // note that the converted altitude in local point is in the same vertical datum as the geo + // point + projected_gps_point.ele = local_point.z + projector_info.map_origin.altitude; + } + + GeoPoint geo_point; + geo_point.latitude = projected_gps_point.lat; + geo_point.longitude = projected_gps_point.lon; + geo_point.altitude = projected_gps_point.ele; + return geo_point; +} + +} // namespace geography_utils diff --git a/common/geography_utils/test/test_geography_utils.cpp b/common/geography_utils/test/test_geography_utils.cpp new file mode 100644 index 0000000000000..480e17b8f49a4 --- /dev/null +++ b/common/geography_utils/test/test_geography_utils.cpp @@ -0,0 +1,26 @@ +// Copyright 2023 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 "geography_utils/height.hpp" +#include "geography_utils/lanelet2_projector.hpp" +#include "geography_utils/projection.hpp" + +#include + +int main(int argc, char * argv[]) +{ + testing::InitGoogleTest(&argc, argv); + bool result = RUN_ALL_TESTS(); + return result; +} diff --git a/common/geography_utils/test/test_height.cpp b/common/geography_utils/test/test_height.cpp new file mode 100644 index 0000000000000..15297089ee131 --- /dev/null +++ b/common/geography_utils/test/test_height.cpp @@ -0,0 +1,86 @@ +// Copyright 2023 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 + +#include + +#include +#include + +// Test case to verify if same source and target datums return original height +TEST(GeographyUtils, SameSourceTargetDatum) +{ + const double height = 10.0; + const double latitude = 35.0; + const double longitude = 139.0; + const std::string datum = "WGS84"; + + double converted_height = + geography_utils::convert_height(height, latitude, longitude, datum, datum); + + EXPECT_DOUBLE_EQ(height, converted_height); +} + +// Test case to verify valid source and target datums +TEST(GeographyUtils, ValidSourceTargetDatum) +{ + // Calculated with + // https://www.unavco.org/software/geodetic-utilities/geoid-height-calculator/geoid-height-calculator.html + const double height = 10.0; + const double latitude = 35.0; + const double longitude = 139.0; + const double target_height = -30.18; + + double converted_height = + geography_utils::convert_height(height, latitude, longitude, "WGS84", "EGM2008"); + + EXPECT_NEAR(target_height, converted_height, 0.1); +} + +// Test case to verify invalid source and target datums +TEST(GeographyUtils, InvalidSourceTargetDatum) +{ + const double height = 10.0; + const double latitude = 35.0; + const double longitude = 139.0; + + EXPECT_THROW( + geography_utils::convert_height(height, latitude, longitude, "INVALID1", "INVALID2"), + std::invalid_argument); +} + +// Test case to verify invalid source datums +TEST(GeographyUtils, InvalidSourceDatum) +{ + const double height = 10.0; + const double latitude = 35.0; + const double longitude = 139.0; + + EXPECT_THROW( + geography_utils::convert_height(height, latitude, longitude, "INVALID1", "WGS84"), + std::invalid_argument); +} + +// Test case to verify invalid target datums +TEST(GeographyUtils, InvalidTargetDatum) +{ + const double height = 10.0; + const double latitude = 35.0; + const double longitude = 139.0; + + EXPECT_THROW( + geography_utils::convert_height(height, latitude, longitude, "WGS84", "INVALID2"), + std::invalid_argument); +} diff --git a/common/geography_utils/test/test_projection.cpp b/common/geography_utils/test/test_projection.cpp new file mode 100644 index 0000000000000..74ffb616cde6f --- /dev/null +++ b/common/geography_utils/test/test_projection.cpp @@ -0,0 +1,161 @@ +// Copyright 2023 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 + +#include + +#include +#include + +TEST(GeographyUtilsProjection, ProjectForwardToMGRS) +{ + // source point + geographic_msgs::msg::GeoPoint geo_point; + geo_point.latitude = 35.62426; + geo_point.longitude = 139.74252; + geo_point.altitude = 10.0; + + // target point + geometry_msgs::msg::Point local_point; + local_point.x = 86128.0; + local_point.y = 43002.0; + local_point.z = 10.0; + + // projector info + tier4_map_msgs::msg::MapProjectorInfo projector_info; + projector_info.projector_type = tier4_map_msgs::msg::MapProjectorInfo::MGRS; + projector_info.mgrs_grid = "54SUE"; + projector_info.vertical_datum = tier4_map_msgs::msg::MapProjectorInfo::WGS84; + + // conversion + const geometry_msgs::msg::Point converted_point = + geography_utils::project_forward(geo_point, projector_info); + + EXPECT_NEAR(converted_point.x, local_point.x, 1.0); + EXPECT_NEAR(converted_point.y, local_point.y, 1.0); + EXPECT_NEAR(converted_point.z, local_point.z, 1.0); +} + +TEST(GeographyUtilsProjection, ProjectReverseFromMGRS) +{ + // source point + geometry_msgs::msg::Point local_point; + local_point.x = 86128.0; + local_point.y = 43002.0; + local_point.z = 10.0; + + // target point + geographic_msgs::msg::GeoPoint geo_point; + geo_point.latitude = 35.62426; + geo_point.longitude = 139.74252; + geo_point.altitude = 10.0; + + // projector info + tier4_map_msgs::msg::MapProjectorInfo projector_info; + projector_info.projector_type = tier4_map_msgs::msg::MapProjectorInfo::MGRS; + projector_info.mgrs_grid = "54SUE"; + projector_info.vertical_datum = tier4_map_msgs::msg::MapProjectorInfo::WGS84; + + // conversion + const geographic_msgs::msg::GeoPoint converted_point = + geography_utils::project_reverse(local_point, projector_info); + + EXPECT_NEAR(converted_point.latitude, geo_point.latitude, 0.0001); + EXPECT_NEAR(converted_point.longitude, geo_point.longitude, 0.0001); + EXPECT_NEAR(converted_point.altitude, geo_point.altitude, 0.0001); +} + +TEST(GeographyUtilsProjection, ProjectForwardAndReverseMGRS) +{ + // source point + geographic_msgs::msg::GeoPoint geo_point; + geo_point.latitude = 35.62426; + geo_point.longitude = 139.74252; + geo_point.altitude = 10.0; + + // projector info + tier4_map_msgs::msg::MapProjectorInfo projector_info; + projector_info.projector_type = tier4_map_msgs::msg::MapProjectorInfo::MGRS; + projector_info.mgrs_grid = "54SUE"; + projector_info.vertical_datum = tier4_map_msgs::msg::MapProjectorInfo::WGS84; + + // conversion + const geometry_msgs::msg::Point converted_local_point = + geography_utils::project_forward(geo_point, projector_info); + const geographic_msgs::msg::GeoPoint converted_geo_point = + geography_utils::project_reverse(converted_local_point, projector_info); + + EXPECT_NEAR(converted_geo_point.latitude, geo_point.latitude, 0.0001); + EXPECT_NEAR(converted_geo_point.longitude, geo_point.longitude, 0.0001); + EXPECT_NEAR(converted_geo_point.altitude, geo_point.altitude, 0.0001); +} + +TEST(GeographyUtilsProjection, ProjectForwardToLocalCartesianUTMOrigin) +{ + // source point + geographic_msgs::msg::GeoPoint geo_point; + geo_point.latitude = 35.62406; + geo_point.longitude = 139.74252; + geo_point.altitude = 10.0; + + // target point + geometry_msgs::msg::Point local_point; + local_point.x = 0.0; + local_point.y = -22.18; + local_point.z = 20.0; + + // projector info + tier4_map_msgs::msg::MapProjectorInfo projector_info; + projector_info.projector_type = tier4_map_msgs::msg::MapProjectorInfo::LOCAL_CARTESIAN_UTM; + projector_info.vertical_datum = tier4_map_msgs::msg::MapProjectorInfo::WGS84; + projector_info.map_origin.latitude = 35.62426; + projector_info.map_origin.longitude = 139.74252; + projector_info.map_origin.altitude = -10.0; + + // conversion + const geometry_msgs::msg::Point converted_point = + geography_utils::project_forward(geo_point, projector_info); + + EXPECT_NEAR(converted_point.x, local_point.x, 1.0); + EXPECT_NEAR(converted_point.y, local_point.y, 1.0); + EXPECT_NEAR(converted_point.z, local_point.z, 1.0); +} + +TEST(GeographyUtilsProjection, ProjectForwardAndReverseLocalCartesianUTMOrigin) +{ + // source point + geographic_msgs::msg::GeoPoint geo_point; + geo_point.latitude = 35.62426; + geo_point.longitude = 139.74252; + geo_point.altitude = 10.0; + + // projector info + tier4_map_msgs::msg::MapProjectorInfo projector_info; + projector_info.projector_type = tier4_map_msgs::msg::MapProjectorInfo::LOCAL_CARTESIAN_UTM; + projector_info.vertical_datum = tier4_map_msgs::msg::MapProjectorInfo::WGS84; + projector_info.map_origin.latitude = 35.0; + projector_info.map_origin.longitude = 139.0; + projector_info.map_origin.altitude = 0.0; + + // conversion + const geometry_msgs::msg::Point converted_local_point = + geography_utils::project_forward(geo_point, projector_info); + const geographic_msgs::msg::GeoPoint converted_geo_point = + geography_utils::project_reverse(converted_local_point, projector_info); + + EXPECT_NEAR(converted_geo_point.latitude, geo_point.latitude, 0.0001); + EXPECT_NEAR(converted_geo_point.longitude, geo_point.longitude, 0.0001); + EXPECT_NEAR(converted_geo_point.altitude, geo_point.altitude, 0.0001); +} diff --git a/common/kalman_filter/CMakeLists.txt b/common/kalman_filter/CMakeLists.txt index dd59da727605d..6cbdf67a0affa 100644 --- a/common/kalman_filter/CMakeLists.txt +++ b/common/kalman_filter/CMakeLists.txt @@ -19,4 +19,11 @@ ament_auto_add_library(kalman_filter SHARED include/kalman_filter/time_delay_kalman_filter.hpp ) +if(BUILD_TESTING) + file(GLOB_RECURSE test_files test/*.cpp) + ament_add_ros_isolated_gtest(test_kalman_filter ${test_files}) + + target_link_libraries(test_kalman_filter kalman_filter) +endif() + ament_auto_package() diff --git a/common/kalman_filter/package.xml b/common/kalman_filter/package.xml index ea061f3f23bb8..200440b5774c7 100644 --- a/common/kalman_filter/package.xml +++ b/common/kalman_filter/package.xml @@ -18,7 +18,9 @@ eigen3_cmake_module ament_cmake_cppcheck + ament_cmake_ros ament_lint_auto + autoware_lint_common ament_cmake diff --git a/common/kalman_filter/test/test_kalman_filter.cpp b/common/kalman_filter/test/test_kalman_filter.cpp new file mode 100644 index 0000000000000..4f4e9ce44669b --- /dev/null +++ b/common/kalman_filter/test/test_kalman_filter.cpp @@ -0,0 +1,94 @@ +// Copyright 2023 The 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 "kalman_filter/kalman_filter.hpp" + +#include + +TEST(kalman_filter, kf) +{ + KalmanFilter kf_; + + Eigen::MatrixXd x_t(2, 1); + x_t << 1, 2; + + Eigen::MatrixXd P_t(2, 2); + P_t << 1, 0, 0, 1; + + Eigen::MatrixXd Q_t(2, 2); + Q_t << 0.01, 0, 0, 0.01; + + Eigen::MatrixXd R_t(2, 2); + R_t << 0.09, 0, 0, 0.09; + + Eigen::MatrixXd C_t(2, 2); + C_t << 1, 0, 0, 1; + + Eigen::MatrixXd A_t(2, 2); + A_t << 1, 0, 0, 1; + + Eigen::MatrixXd B_t(2, 2); + B_t << 1, 0, 0, 1; + + // Initialize the filter and check if initialization was successful + EXPECT_TRUE(kf_.init(x_t, A_t, B_t, C_t, Q_t, R_t, P_t)); + + // Perform prediction + Eigen::MatrixXd u_t(2, 1); + u_t << 0.1, 0.1; + EXPECT_TRUE(kf_.predict(u_t)); + + // Check the updated state and covariance matrix + Eigen::MatrixXd x_predict_expected = A_t * x_t + B_t * u_t; + Eigen::MatrixXd P_predict_expected = A_t * P_t * A_t.transpose() + Q_t; + + Eigen::MatrixXd x_predict; + kf_.getX(x_predict); + Eigen::MatrixXd P_predict; + kf_.getP(P_predict); + + EXPECT_NEAR(x_predict(0, 0), x_predict_expected(0, 0), 1e-5); + EXPECT_NEAR(x_predict(1, 0), x_predict_expected(1, 0), 1e-5); + EXPECT_NEAR(P_predict(0, 0), P_predict_expected(0, 0), 1e-5); + EXPECT_NEAR(P_predict(1, 1), P_predict_expected(1, 1), 1e-5); + + // Perform update + Eigen::MatrixXd y_t(2, 1); + y_t << 1.05, 2.05; + EXPECT_TRUE(kf_.update(y_t)); + + // Check the updated state and covariance matrix + const Eigen::MatrixXd PCT_t = P_predict_expected * C_t.transpose(); + const Eigen::MatrixXd K_t = PCT_t * ((R_t + C_t * PCT_t).inverse()); + const Eigen::MatrixXd y_pred = C_t * x_predict_expected; + Eigen::MatrixXd x_update_expected = x_predict_expected + K_t * (y_t - y_pred); + Eigen::MatrixXd P_update_expected = P_predict_expected - K_t * (C_t * P_predict_expected); + + Eigen::MatrixXd x_update; + kf_.getX(x_update); + Eigen::MatrixXd P_update; + kf_.getP(P_update); + + EXPECT_NEAR(x_update(0, 0), x_update_expected(0, 0), 1e-5); + EXPECT_NEAR(x_update(1, 0), x_update_expected(1, 0), 1e-5); + EXPECT_NEAR(P_update(0, 0), P_update_expected(0, 0), 1e-5); + EXPECT_NEAR(P_update(1, 1), P_update_expected(1, 1), 1e-5); +} + +int main(int argc, char * argv[]) +{ + testing::InitGoogleTest(&argc, argv); + bool result = RUN_ALL_TESTS(); + return result; +} diff --git a/common/kalman_filter/test/test_time_delay_kalman_filter.cpp b/common/kalman_filter/test/test_time_delay_kalman_filter.cpp new file mode 100644 index 0000000000000..32fefd8ceff70 --- /dev/null +++ b/common/kalman_filter/test/test_time_delay_kalman_filter.cpp @@ -0,0 +1,121 @@ +// Copyright 2023 The 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 "kalman_filter/time_delay_kalman_filter.hpp" + +#include + +TEST(time_delay_kalman_filter, td_kf) +{ + TimeDelayKalmanFilter td_kf_; + + Eigen::MatrixXd x_t(3, 1); + x_t << 1.0, 2.0, 3.0; + Eigen::MatrixXd P_t(3, 3); + P_t << 0.1, 0.0, 0.0, 0.0, 0.2, 0.0, 0.0, 0.0, 0.3; + const int max_delay_step = 5; + const int dim_x = x_t.rows(); + const int dim_x_ex = dim_x * max_delay_step; + // Initialize the filter + td_kf_.init(x_t, P_t, max_delay_step); + + // Check if initialization was successful + Eigen::MatrixXd x_init = td_kf_.getLatestX(); + Eigen::MatrixXd P_init = td_kf_.getLatestP(); + Eigen::MatrixXd x_ex_t = Eigen::MatrixXd::Zero(dim_x_ex, 1); + Eigen::MatrixXd P_ex_t = Eigen::MatrixXd::Zero(dim_x_ex, dim_x_ex); + for (int i = 0; i < max_delay_step; ++i) { + x_ex_t.block(i * dim_x, 0, dim_x, 1) = x_t; + P_ex_t.block(i * dim_x, i * dim_x, dim_x, dim_x) = P_t; + } + + EXPECT_EQ(x_init.rows(), 3); + EXPECT_EQ(x_init.cols(), 1); + EXPECT_EQ(P_init.rows(), 3); + EXPECT_EQ(P_init.cols(), 3); + EXPECT_NEAR(x_init(0, 0), x_t(0, 0), 1e-5); + EXPECT_NEAR(x_init(1, 0), x_t(1, 0), 1e-5); + EXPECT_NEAR(x_init(2, 0), x_t(2, 0), 1e-5); + EXPECT_NEAR(P_init(0, 0), P_t(0, 0), 1e-5); + EXPECT_NEAR(P_init(1, 1), P_t(1, 1), 1e-5); + EXPECT_NEAR(P_init(2, 2), P_t(2, 2), 1e-5); + + // Define prediction parameters + Eigen::MatrixXd A_t(3, 3); + A_t << 2.0, 0.0, 0.0, 0.0, 2.0, 0.0, 0.0, 0.0, 2.0; + Eigen::MatrixXd Q_t(3, 3); + Q_t << 0.01, 0.0, 0.0, 0.0, 0.02, 0.0, 0.0, 0.0, 0.03; + Eigen::MatrixXd x_next(3, 1); + x_next << 2.0, 4.0, 6.0; + + // Perform prediction + EXPECT_TRUE(td_kf_.predictWithDelay(x_next, A_t, Q_t)); + + // Check the prediction state and covariance matrix + Eigen::MatrixXd x_predict = td_kf_.getLatestX(); + Eigen::MatrixXd P_predict = td_kf_.getLatestP(); + Eigen::MatrixXd x_tmp = Eigen::MatrixXd::Zero(dim_x_ex, 1); + x_tmp.block(0, 0, dim_x, 1) = A_t * x_t; + x_tmp.block(dim_x, 0, dim_x_ex - dim_x, 1) = x_ex_t.block(0, 0, dim_x_ex - dim_x, 1); + x_ex_t = x_tmp; + Eigen::MatrixXd x_predict_expected = x_ex_t.block(0, 0, dim_x, 1); + Eigen::MatrixXd P_tmp = Eigen::MatrixXd::Zero(dim_x_ex, dim_x_ex); + P_tmp.block(0, 0, dim_x, dim_x) = A_t * P_ex_t.block(0, 0, dim_x, dim_x) * A_t.transpose() + Q_t; + P_tmp.block(0, dim_x, dim_x, dim_x_ex - dim_x) = + A_t * P_ex_t.block(0, 0, dim_x, dim_x_ex - dim_x); + P_tmp.block(dim_x, 0, dim_x_ex - dim_x, dim_x) = + P_ex_t.block(0, 0, dim_x_ex - dim_x, dim_x) * A_t.transpose(); + P_tmp.block(dim_x, dim_x, dim_x_ex - dim_x, dim_x_ex - dim_x) = + P_ex_t.block(0, 0, dim_x_ex - dim_x, dim_x_ex - dim_x); + P_ex_t = P_tmp; + Eigen::MatrixXd P_predict_expected = P_ex_t.block(0, 0, dim_x, dim_x); + EXPECT_NEAR(x_predict(0, 0), x_predict_expected(0, 0), 1e-5); + EXPECT_NEAR(x_predict(1, 0), x_predict_expected(1, 0), 1e-5); + EXPECT_NEAR(x_predict(2, 0), x_predict_expected(2, 0), 1e-5); + EXPECT_NEAR(P_predict(0, 0), P_predict_expected(0, 0), 1e-5); + EXPECT_NEAR(P_predict(1, 1), P_predict_expected(1, 1), 1e-5); + EXPECT_NEAR(P_predict(2, 2), P_predict_expected(2, 2), 1e-5); + + // Define update parameters + Eigen::MatrixXd C_t(3, 3); + C_t << 0.5, 0.0, 0.0, 0.0, 0.5, 0.0, 0.0, 0.0, 0.5; + Eigen::MatrixXd R_t(3, 3); + R_t << 0.001, 0.0, 0.0, 0.0, 0.002, 0.0, 0.0, 0.0, 0.003; + Eigen::MatrixXd y_t(3, 1); + y_t << 1.05, 2.05, 3.05; + const int delay_step = 2; // Choose an appropriate delay step + const int dim_y = y_t.rows(); + + // Perform update + EXPECT_TRUE(td_kf_.updateWithDelay(y_t, C_t, R_t, delay_step)); + + // Check the updated state and covariance matrix + Eigen::MatrixXd x_update = td_kf_.getLatestX(); + Eigen::MatrixXd P_update = td_kf_.getLatestP(); + + Eigen::MatrixXd C_ex_t = Eigen::MatrixXd::Zero(dim_y, dim_x_ex); + const Eigen::MatrixXd PCT_t = P_ex_t * C_ex_t.transpose(); + const Eigen::MatrixXd K_t = PCT_t * ((R_t + C_ex_t * PCT_t).inverse()); + const Eigen::MatrixXd y_pred = C_ex_t * x_ex_t; + x_ex_t = x_ex_t + K_t * (y_t - y_pred); + P_ex_t = P_ex_t - K_t * (C_ex_t * P_ex_t); + Eigen::MatrixXd x_update_expected = x_ex_t.block(0, 0, dim_x, 1); + Eigen::MatrixXd P_update_expected = P_ex_t.block(0, 0, dim_x, dim_x); + EXPECT_NEAR(x_update(0, 0), x_update_expected(0, 0), 1e-5); + EXPECT_NEAR(x_update(1, 0), x_update_expected(1, 0), 1e-5); + EXPECT_NEAR(x_update(2, 0), x_update_expected(2, 0), 1e-5); + EXPECT_NEAR(P_update(0, 0), P_update_expected(0, 0), 1e-5); + EXPECT_NEAR(P_update(1, 1), P_update_expected(1, 1), 1e-5); + EXPECT_NEAR(P_update(2, 2), P_update_expected(2, 2), 1e-5); +} diff --git a/common/motion_utils/CMakeLists.txt b/common/motion_utils/CMakeLists.txt index d1abf9dd730b9..43c8158b2f98b 100644 --- a/common/motion_utils/CMakeLists.txt +++ b/common/motion_utils/CMakeLists.txt @@ -7,13 +7,14 @@ autoware_package() find_package(Boost REQUIRED) ament_auto_add_library(motion_utils SHARED - src/motion_utils.cpp src/distance/distance.cpp src/marker/marker_helper.cpp src/marker/virtual_wall_marker_creator.cpp src/resample/resample.cpp + src/trajectory/trajectory.cpp src/trajectory/interpolation.cpp src/trajectory/path_with_lane_id.cpp + src/trajectory/tmp_conversion.cpp src/vehicle/vehicle_state_checker.cpp ) diff --git a/common/motion_utils/README.md b/common/motion_utils/README.md index 430014a300298..925ee5b162db3 100644 --- a/common/motion_utils/README.md +++ b/common/motion_utils/README.md @@ -109,3 +109,9 @@ const size_t ego_nearest_seg_idx = findFirstNearestSegmentIndex(points, ego_pose const size_t dyn_obj_nearest_seg_idx = findFirstNearestSegmentIndex(points, dyn_obj_pose, dyn_obj_nearest_dist_threshold); const double length_from_ego_to_obj = calcSignedArcLength(points, ego_pose, ego_nearest_seg_idx, dyn_obj_pose, dyn_obj_nearest_seg_idx); ``` + +## For developers + +Some of the template functions in `trajectory.hpp` are mostly used for specific types (`autoware_auto_planning_msgs::msg::PathPoint`, `autoware_auto_planning_msgs::msg::PathPoint`, `autoware_auto_planning_msgs::msg::TrajectoryPoint`), so they are exported as `extern template` functions to speed-up compilation time. + +`motion_utils.hpp` header file was removed because the source files that directly/indirectly include this file took a long time for preprocessing. diff --git a/common/motion_utils/include/motion_utils/marker/marker_helper.hpp b/common/motion_utils/include/motion_utils/marker/marker_helper.hpp index 6433f1b9ae2f9..0604eee65372a 100644 --- a/common/motion_utils/include/motion_utils/marker/marker_helper.hpp +++ b/common/motion_utils/include/motion_utils/marker/marker_helper.hpp @@ -15,12 +15,11 @@ #ifndef MOTION_UTILS__MARKER__MARKER_HELPER_HPP_ #define MOTION_UTILS__MARKER__MARKER_HELPER_HPP_ -#include "motion_utils/resample/resample_utils.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" +#include + +#include -#include #include -#include namespace motion_utils { @@ -28,15 +27,18 @@ using geometry_msgs::msg::Pose; visualization_msgs::msg::MarkerArray createStopVirtualWallMarker( const Pose & pose, const std::string & module_name, const rclcpp::Time & now, const int32_t id, - const double longitudinal_offset = 0.0, const std::string & ns_prefix = ""); + const double longitudinal_offset = 0.0, const std::string & ns_prefix = "", + const bool is_driving_forward = true); visualization_msgs::msg::MarkerArray createSlowDownVirtualWallMarker( const Pose & pose, const std::string & module_name, const rclcpp::Time & now, const int32_t id, - const double longitudinal_offset = 0.0, const std::string & ns_prefix = ""); + const double longitudinal_offset = 0.0, const std::string & ns_prefix = "", + const bool is_driving_forward = true); visualization_msgs::msg::MarkerArray createDeadLineVirtualWallMarker( const Pose & pose, const std::string & module_name, const rclcpp::Time & now, const int32_t id, - const double longitudinal_offset = 0.0, const std::string & ns_prefix = ""); + const double longitudinal_offset = 0.0, const std::string & ns_prefix = "", + const bool is_driving_forward = true); visualization_msgs::msg::MarkerArray createDeletedStopVirtualWallMarker( const rclcpp::Time & now, const int32_t id); diff --git a/common/motion_utils/include/motion_utils/marker/virtual_wall_marker_creator.hpp b/common/motion_utils/include/motion_utils/marker/virtual_wall_marker_creator.hpp index 331b3dd5275fd..95bbcd93e30df 100644 --- a/common/motion_utils/include/motion_utils/marker/virtual_wall_marker_creator.hpp +++ b/common/motion_utils/include/motion_utils/marker/virtual_wall_marker_creator.hpp @@ -15,16 +15,14 @@ #ifndef MOTION_UTILS__MARKER__VIRTUAL_WALL_MARKER_CREATOR_HPP_ #define MOTION_UTILS__MARKER__VIRTUAL_WALL_MARKER_CREATOR_HPP_ -#include "motion_utils/marker/marker_helper.hpp" +#include #include -#include #include #include #include #include - namespace motion_utils { @@ -38,6 +36,7 @@ struct VirtualWall std::string ns{}; VirtualWallType style = stop; double longitudinal_offset{}; + bool is_driving_forward{true}; }; typedef std::vector VirtualWalls; @@ -54,7 +53,7 @@ class VirtualWallMarkerCreator using create_wall_function = std::function; + const std::string & ns_prefix, const bool is_driving_forward)>; VirtualWalls virtual_walls; std::unordered_map marker_count_per_namespace; diff --git a/common/motion_utils/include/motion_utils/resample/resample.hpp b/common/motion_utils/include/motion_utils/resample/resample.hpp index cfb945270ed7f..106ed32b18576 100644 --- a/common/motion_utils/include/motion_utils/resample/resample.hpp +++ b/common/motion_utils/include/motion_utils/resample/resample.hpp @@ -15,17 +15,6 @@ #ifndef MOTION_UTILS__RESAMPLE__RESAMPLE_HPP_ #define MOTION_UTILS__RESAMPLE__RESAMPLE_HPP_ -#include "interpolation/interpolation_utils.hpp" -#include "interpolation/linear_interpolation.hpp" -#include "interpolation/spline_interpolation.hpp" -#include "interpolation/zero_order_hold.hpp" -#include "motion_utils/trajectory/trajectory.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" -#include "tier4_autoware_utils/geometry/pose_deviation.hpp" -#include "tier4_autoware_utils/math/constants.hpp" - -#include - #include "autoware_auto_planning_msgs/msg/path.hpp" #include "autoware_auto_planning_msgs/msg/path_with_lane_id.hpp" #include "autoware_auto_planning_msgs/msg/trajectory.hpp" diff --git a/common/motion_utils/include/motion_utils/resample/resample_utils.hpp b/common/motion_utils/include/motion_utils/resample/resample_utils.hpp index 2221bd0147104..1e4d0d633325e 100644 --- a/common/motion_utils/include/motion_utils/resample/resample_utils.hpp +++ b/common/motion_utils/include/motion_utils/resample/resample_utils.hpp @@ -18,7 +18,6 @@ #include #include #include -#include #include diff --git a/common/motion_utils/include/motion_utils/trajectory/path_with_lane_id.hpp b/common/motion_utils/include/motion_utils/trajectory/path_with_lane_id.hpp index bd6b9f10facca..74862a6229a46 100644 --- a/common/motion_utils/include/motion_utils/trajectory/path_with_lane_id.hpp +++ b/common/motion_utils/include/motion_utils/trajectory/path_with_lane_id.hpp @@ -15,90 +15,24 @@ #ifndef MOTION_UTILS__TRAJECTORY__PATH_WITH_LANE_ID_HPP_ #define MOTION_UTILS__TRAJECTORY__PATH_WITH_LANE_ID_HPP_ -#include "motion_utils/trajectory/trajectory.hpp" -#include "tier4_autoware_utils/geometry/path_with_lane_id_geometry.hpp" +#include "autoware_auto_planning_msgs/msg/path_with_lane_id.hpp" +#include #include -#include #include -#include - namespace motion_utils { -inline boost::optional> getPathIndexRangeWithLaneId( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int64_t target_lane_id) -{ - size_t start_idx = 0; // NOTE: to prevent from maybe-uninitialized error - size_t end_idx = 0; // NOTE: to prevent from maybe-uninitialized error - - bool found_first_idx = false; - for (size_t i = 0; i < path.points.size(); ++i) { - const auto & p = path.points.at(i); - for (const auto & id : p.lane_ids) { - if (id == target_lane_id) { - if (!found_first_idx) { - start_idx = i; - found_first_idx = true; - } - end_idx = i; - } - } - } - - if (found_first_idx) { - // NOTE: In order to fully cover lanes with target_lane_id, start_idx and end_idx are expanded. - start_idx = static_cast(std::max(0, static_cast(start_idx) - 1)); - end_idx = std::min(path.points.size() - 1, end_idx + 1); +boost::optional> getPathIndexRangeWithLaneId( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int64_t target_lane_id); - return std::make_pair(start_idx, end_idx); - } - - return {}; -} - -inline size_t findNearestIndexFromLaneId( +size_t findNearestIndexFromLaneId( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const geometry_msgs::msg::Point & pos, const int64_t lane_id) -{ - const auto opt_range = getPathIndexRangeWithLaneId(path, lane_id); - if (opt_range) { - const size_t start_idx = opt_range->first; - const size_t end_idx = opt_range->second; - - validateNonEmpty(path.points); + const geometry_msgs::msg::Point & pos, const int64_t lane_id); - const auto sub_points = std::vector{ - path.points.begin() + start_idx, path.points.begin() + end_idx + 1}; - validateNonEmpty(sub_points); - - return start_idx + findNearestIndex(sub_points, pos); - } - - return findNearestIndex(path.points, pos); -} - -inline size_t findNearestSegmentIndexFromLaneId( +size_t findNearestSegmentIndexFromLaneId( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const geometry_msgs::msg::Point & pos, const int64_t lane_id) -{ - const size_t nearest_idx = findNearestIndexFromLaneId(path, pos, lane_id); - - if (nearest_idx == 0) { - return 0; - } - if (nearest_idx == path.points.size() - 1) { - return path.points.size() - 2; - } - - const double signed_length = calcLongitudinalOffsetToSegment(path.points, nearest_idx, pos); - - if (signed_length <= 0) { - return nearest_idx - 1; - } - - return nearest_idx; -} + const geometry_msgs::msg::Point & pos, const int64_t lane_id); // @brief Calculates the path to be followed by the rear wheel center in order to make the vehicle // center follow the input path diff --git a/common/motion_utils/include/motion_utils/trajectory/tmp_conversion.hpp b/common/motion_utils/include/motion_utils/trajectory/tmp_conversion.hpp index 3bf06fa241315..6921c56e0199e 100644 --- a/common/motion_utils/include/motion_utils/trajectory/tmp_conversion.hpp +++ b/common/motion_utils/include/motion_utils/trajectory/tmp_conversion.hpp @@ -15,15 +15,9 @@ #ifndef MOTION_UTILS__TRAJECTORY__TMP_CONVERSION_HPP_ #define MOTION_UTILS__TRAJECTORY__TMP_CONVERSION_HPP_ -#include "tier4_autoware_utils/geometry/geometry.hpp" -#include "tier4_autoware_utils/geometry/pose_deviation.hpp" - #include "autoware_auto_planning_msgs/msg/trajectory.hpp" #include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" -#include - -#include #include namespace motion_utils @@ -39,30 +33,15 @@ namespace motion_utils * @todo Decide how to handle the situation that we need to use the trajectory with the size of * points larger than the capacity. (Tier IV) */ -inline autoware_auto_planning_msgs::msg::Trajectory convertToTrajectory( - const std::vector & trajectory) -{ - autoware_auto_planning_msgs::msg::Trajectory output{}; - for (const auto & pt : trajectory) { - output.points.push_back(pt); - if (output.points.size() >= output.CAPACITY) { - break; - } - } - return output; -} +autoware_auto_planning_msgs::msg::Trajectory convertToTrajectory( + const std::vector & trajectory); /** * @brief Convert autoware_auto_planning_msgs::msg::Trajectory to * std::vector. */ -inline std::vector convertToTrajectoryPointArray( - const autoware_auto_planning_msgs::msg::Trajectory & trajectory) -{ - std::vector output(trajectory.points.size()); - std::copy(trajectory.points.begin(), trajectory.points.end(), output.begin()); - return output; -} +std::vector convertToTrajectoryPointArray( + const autoware_auto_planning_msgs::msg::Trajectory & trajectory); } // namespace motion_utils diff --git a/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp b/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp index c65a7eae189e3..5d773c5be32d9 100644 --- a/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp +++ b/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp @@ -15,10 +15,16 @@ #ifndef MOTION_UTILS__TRAJECTORY__TRAJECTORY_HPP_ #define MOTION_UTILS__TRAJECTORY__TRAJECTORY_HPP_ -#include "interpolation/spline_interpolation_points_2d.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" #include "tier4_autoware_utils/geometry/pose_deviation.hpp" #include "tier4_autoware_utils/math/constants.hpp" +#include "tier4_autoware_utils/system/backtrace.hpp" + +#include + +#include +#include +#include #include @@ -27,7 +33,6 @@ #include #include #include - namespace motion_utils { @@ -39,10 +44,20 @@ template void validateNonEmpty(const T & points) { if (points.empty()) { + tier4_autoware_utils::print_backtrace(); throw std::invalid_argument("Points is empty."); } } +extern template void validateNonEmpty>( + const std::vector &); +extern template void +validateNonEmpty>( + const std::vector &); +extern template void +validateNonEmpty>( + const std::vector &); + /** * @brief validate a point is in a non-sharp angle between two points or not * @param point1 front point @@ -67,6 +82,7 @@ void validateNonSharpAngle( constexpr double epsilon = 1e-3; if (std::cos(angle_threshold) < product / dist_1to2 / dist_3to2 + epsilon) { + tier4_autoware_utils::print_backtrace(); throw std::invalid_argument("Sharp angle."); } } @@ -77,7 +93,7 @@ void validateNonSharpAngle( * @return (forward / backward) driving (true / false) */ template -boost::optional isDrivingForward(const T points) +boost::optional isDrivingForward(const T & points) { if (points.size() < 2) { return boost::none; @@ -90,6 +106,16 @@ boost::optional isDrivingForward(const T points) return tier4_autoware_utils::isDrivingForward(first_pose, second_pose); } +extern template boost::optional +isDrivingForward>( + const std::vector &); +extern template boost::optional +isDrivingForward>( + const std::vector &); +extern template boost::optional +isDrivingForward>( + const std::vector &); + /** * @brief checks whether a path of trajectory has forward driving direction using its longitudinal * velocity @@ -97,7 +123,7 @@ boost::optional isDrivingForward(const T points) * @return (forward / backward) driving (true, false, none "if velocity is zero") */ template -boost::optional isDrivingForwardWithTwist(const T points_with_twist) +boost::optional isDrivingForwardWithTwist(const T & points_with_twist) { if (points_with_twist.empty()) { return boost::none; @@ -115,6 +141,16 @@ boost::optional isDrivingForwardWithTwist(const T points_with_twist) return isDrivingForward(points_with_twist); } +extern template boost::optional +isDrivingForwardWithTwist>( + const std::vector &); +extern template boost::optional +isDrivingForwardWithTwist>( + const std::vector &); +extern template boost::optional +isDrivingForwardWithTwist>( + const std::vector &); + /** * @brief remove overlapping points through points container. * Overlapping is determined by calculating the distance between 2 consecutive points. @@ -125,7 +161,7 @@ boost::optional isDrivingForwardWithTwist(const T points_with_twist) * @return points container without overlapping points */ template -T removeOverlapPoints(const T & points, const size_t & start_idx = 0) +T removeOverlapPoints(const T & points, const size_t start_idx = 0) { if (points.size() < start_idx + 1) { return points; @@ -141,8 +177,7 @@ T removeOverlapPoints(const T & points, const size_t & start_idx = 0) for (size_t i = start_idx + 1; i < points.size(); ++i) { const auto prev_p = tier4_autoware_utils::getPoint(dst.back()); const auto curr_p = tier4_autoware_utils::getPoint(points.at(i)); - const double dist = tier4_autoware_utils::calcDistance2d(prev_p, curr_p); - if (dist < eps) { + if (std::abs(prev_p.x - curr_p.x) < eps && std::abs(prev_p.y - curr_p.y) < eps) { continue; } dst.push_back(points.at(i)); @@ -151,6 +186,19 @@ T removeOverlapPoints(const T & points, const size_t & start_idx = 0) return dst; } +extern template std::vector +removeOverlapPoints>( + const std::vector & points, + const size_t start_idx = 0); +extern template std::vector +removeOverlapPoints>( + const std::vector & points, + const size_t start_idx = 0); +extern template std::vector +removeOverlapPoints>( + const std::vector & points, + const size_t start_idx = 0); + /** * @brief search through points container from specified start and end indices about first matching * index of a zero longitudinal velocity point. @@ -180,6 +228,11 @@ boost::optional searchZeroVelocityIndex( return {}; } +extern template boost::optional +searchZeroVelocityIndex>( + const std::vector & points_with_twist, + const size_t src_idx, const size_t dst_idx); + /** * @brief search through points container from specified start index till end of points container * about first matching index of a zero longitudinal velocity point. @@ -188,7 +241,7 @@ boost::optional searchZeroVelocityIndex( * @return first matching index of a zero velocity point inside the points container. */ template -boost::optional searchZeroVelocityIndex(const T & points_with_twist, const size_t & src_idx) +boost::optional searchZeroVelocityIndex(const T & points_with_twist, const size_t src_idx) { try { validateNonEmpty(points_with_twist); @@ -200,6 +253,11 @@ boost::optional searchZeroVelocityIndex(const T & points_with_twist, con return searchZeroVelocityIndex(points_with_twist, src_idx, points_with_twist.size()); } +extern template boost::optional +searchZeroVelocityIndex>( + const std::vector & points_with_twist, + const size_t src_idx); + /** * @brief search through points container from its start to end about first matching index of a zero * longitudinal velocity point. @@ -212,6 +270,10 @@ boost::optional searchZeroVelocityIndex(const T & points_with_twist) return searchZeroVelocityIndex(points_with_twist, 0, points_with_twist.size()); } +extern template boost::optional +searchZeroVelocityIndex>( + const std::vector & points_with_twist); + /** * @brief find nearest point index through points container for a given point. * Finding nearest point is determined by looping through the points container, @@ -240,6 +302,18 @@ size_t findNearestIndex(const T & points, const geometry_msgs::msg::Point & poin return min_idx; } +extern template size_t findNearestIndex>( + const std::vector & points, + const geometry_msgs::msg::Point & point); +extern template size_t +findNearestIndex>( + const std::vector & points, + const geometry_msgs::msg::Point & point); +extern template size_t +findNearestIndex>( + const std::vector & points, + const geometry_msgs::msg::Point & point); + /** * @brief find nearest point index through points container for a given pose. * Finding nearest point is determined by looping through the points container, @@ -291,6 +365,22 @@ boost::optional findNearestIndex( return is_nearest_found ? boost::optional(min_idx) : boost::none; } +extern template boost::optional +findNearestIndex>( + const std::vector & points, + const geometry_msgs::msg::Pose & pose, const double max_dist = std::numeric_limits::max(), + const double max_yaw = std::numeric_limits::max()); +extern template boost::optional +findNearestIndex>( + const std::vector & points, + const geometry_msgs::msg::Pose & pose, const double max_dist = std::numeric_limits::max(), + const double max_yaw = std::numeric_limits::max()); +extern template boost::optional +findNearestIndex>( + const std::vector & points, + const geometry_msgs::msg::Pose & pose, const double max_dist = std::numeric_limits::max(), + const double max_yaw = std::numeric_limits::max()); + /** * @brief calculate longitudinal offset (length along trajectory from seg_idx point to nearest point * to p_target on trajectory). If seg_idx point is after that nearest point, length is negative. @@ -308,6 +398,7 @@ double calcLongitudinalOffsetToSegment( { if (seg_idx >= points.size() - 1) { const std::out_of_range e("Segment index is invalid."); + tier4_autoware_utils::print_backtrace(); if (throw_exception) { throw e; } @@ -330,6 +421,7 @@ double calcLongitudinalOffsetToSegment( if (seg_idx >= overlap_removed_points.size() - 1) { const std::runtime_error e("Same points are given."); + tier4_autoware_utils::print_backtrace(); if (throw_exception) { throw e; } @@ -346,6 +438,21 @@ double calcLongitudinalOffsetToSegment( return segment_vec.dot(target_vec) / segment_vec.norm(); } +extern template double +calcLongitudinalOffsetToSegment>( + const std::vector & points, const size_t seg_idx, + const geometry_msgs::msg::Point & p_target, const bool throw_exception = false); +extern template double +calcLongitudinalOffsetToSegment>( + const std::vector & points, + const size_t seg_idx, const geometry_msgs::msg::Point & p_target, + const bool throw_exception = false); +extern template double +calcLongitudinalOffsetToSegment>( + const std::vector & points, + const size_t seg_idx, const geometry_msgs::msg::Point & p_target, + const bool throw_exception = false); + /** * @brief find nearest segment index to point. * Segment is straight path between two continuous points of trajectory. @@ -375,6 +482,19 @@ size_t findNearestSegmentIndex(const T & points, const geometry_msgs::msg::Point return nearest_idx; } +extern template size_t +findNearestSegmentIndex>( + const std::vector & points, + const geometry_msgs::msg::Point & point); +extern template size_t +findNearestSegmentIndex>( + const std::vector & points, + const geometry_msgs::msg::Point & point); +extern template size_t +findNearestSegmentIndex>( + const std::vector & points, + const geometry_msgs::msg::Point & point); + /** * @brief find nearest segment index to pose * Segment is straight path between two continuous points of trajectory. @@ -413,6 +533,22 @@ boost::optional findNearestSegmentIndex( return *nearest_idx; } +extern template boost::optional +findNearestSegmentIndex>( + const std::vector & points, + const geometry_msgs::msg::Pose & pose, const double max_dist = std::numeric_limits::max(), + const double max_yaw = std::numeric_limits::max()); +extern template boost::optional +findNearestSegmentIndex>( + const std::vector & points, + const geometry_msgs::msg::Pose & pose, const double max_dist = std::numeric_limits::max(), + const double max_yaw = std::numeric_limits::max()); +extern template boost::optional +findNearestSegmentIndex>( + const std::vector & points, + const geometry_msgs::msg::Pose & pose, const double max_dist = std::numeric_limits::max(), + const double max_yaw = std::numeric_limits::max()); + /** * @brief calculate lateral offset from p_target (length from p_target to trajectory) using given * segment index. Segment is straight path between two continuous points of trajectory. @@ -442,6 +578,7 @@ double calcLateralOffset( if (overlap_removed_points.size() == 1) { const std::runtime_error e("Same points are given."); + tier4_autoware_utils::print_backtrace(); if (throw_exception) { throw e; } @@ -459,6 +596,21 @@ double calcLateralOffset( return cross_vec(2) / segment_vec.norm(); } +extern template double calcLateralOffset>( + const std::vector & points, + const geometry_msgs::msg::Point & p_target, const size_t seg_idx, + const bool throw_exception = false); +extern template double +calcLateralOffset>( + const std::vector & points, + const geometry_msgs::msg::Point & p_target, const size_t seg_idx, + const bool throw_exception = false); +extern template double +calcLateralOffset>( + const std::vector & points, + const geometry_msgs::msg::Point & p_target, const size_t seg_idx, + const bool throw_exception = false); + /** * @brief calculate lateral offset from p_target (length from p_target to trajectory). * The function gets the nearest segment index between the points of trajectory and the given target @@ -488,6 +640,7 @@ double calcLateralOffset( if (overlap_removed_points.size() == 1) { const std::runtime_error e("Same points are given."); + tier4_autoware_utils::print_backtrace(); if (throw_exception) { throw e; } @@ -499,6 +652,18 @@ double calcLateralOffset( return calcLateralOffset(points, p_target, seg_idx, throw_exception); } +extern template double calcLateralOffset>( + const std::vector & points, + const geometry_msgs::msg::Point & p_target, const bool throw_exception = false); +extern template double +calcLateralOffset>( + const std::vector & points, + const geometry_msgs::msg::Point & p_target, const bool throw_exception = false); +extern template double +calcLateralOffset>( + const std::vector & points, + const geometry_msgs::msg::Point & p_target, const bool throw_exception = false); + /** * @brief calculate length of 2D distance between two points, specified by start and end points * indicies through points container. @@ -530,6 +695,19 @@ double calcSignedArcLength(const T & points, const size_t src_idx, const size_t return dist_sum; } +extern template double +calcSignedArcLength>( + const std::vector & points, const size_t src_idx, + const size_t dst_idx); +extern template double +calcSignedArcLength>( + const std::vector & points, + const size_t src_idx, const size_t dst_idx); +extern template double +calcSignedArcLength>( + const std::vector & points, + const size_t src_idx, const size_t dst_idx); + /** * @brief Computes the partial sums of the elements in the sub-ranges of the range [src_idx, * dst_idx) and return these sum as vector @@ -567,6 +745,19 @@ std::vector calcSignedArcLengthPartialSum( return partial_dist; } +extern template std::vector +calcSignedArcLengthPartialSum>( + const std::vector & points, const size_t src_idx, + const size_t dst_idx); +extern template std::vector +calcSignedArcLengthPartialSum>( + const std::vector & points, + const size_t src_idx, const size_t dst_idx); +extern template std::vector +calcSignedArcLengthPartialSum>( + const std::vector & points, + const size_t src_idx, const size_t dst_idx); + /** * @brief calculate length of 2D distance between two points, specified by start point and end point * index of points container. @@ -597,6 +788,19 @@ double calcSignedArcLength( return signed_length_on_traj - signed_length_src_offset; } +extern template double +calcSignedArcLength>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const size_t dst_idx); +extern template double +calcSignedArcLength>( + const std::vector &, + const geometry_msgs::msg::Point & src_point, const size_t dst_idx); +extern template double +calcSignedArcLength>( + const std::vector &, + const geometry_msgs::msg::Point & src_point, const size_t dst_idx); + /** * @brief calculate length of 2D distance between two points, specified by start index of points * container and end point. @@ -621,6 +825,19 @@ double calcSignedArcLength( return -calcSignedArcLength(points, dst_point, src_idx); } +extern template double +calcSignedArcLength>( + const std::vector & points, const size_t src_idx, + const geometry_msgs::msg::Point & dst_point); +extern template double +calcSignedArcLength>( + const std::vector & points, + const size_t src_idx, const geometry_msgs::msg::Point & dst_point); +extern template double +calcSignedArcLength>( + const std::vector & points, + const size_t src_idx, const geometry_msgs::msg::Point & dst_point); + /** * @brief calculate length of 2D distance between two points, specified by start point and end * point. @@ -656,6 +873,19 @@ double calcSignedArcLength( return signed_length_on_traj - signed_length_src_offset + signed_length_dst_offset; } +extern template double +calcSignedArcLength>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const geometry_msgs::msg::Point & dst_point); +extern template double +calcSignedArcLength>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const geometry_msgs::msg::Point & dst_point); +extern template double +calcSignedArcLength>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const geometry_msgs::msg::Point & dst_point); + /** * @brief calculate length of 2D distance for whole points container, from its start to its end. * @param points points of trajectory, path, ... @@ -674,6 +904,15 @@ double calcArcLength(const T & points) return calcSignedArcLength(points, 0, points.size() - 1); } +extern template double calcArcLength>( + const std::vector & points); +extern template double +calcArcLength>( + const std::vector & points); +extern template double +calcArcLength>( + const std::vector & points); + /** * @brief calculate curvature through points container. * The method used for calculating the curvature is using 3 consecutive points through the points @@ -684,7 +923,7 @@ double calcArcLength(const T & points) * @return calculated curvature container through points container */ template -inline std::vector calcCurvature(const T & points) +std::vector calcCurvature(const T & points) { std::vector curvature_vec(points.size()); @@ -700,6 +939,16 @@ inline std::vector calcCurvature(const T & points) return curvature_vec; } +extern template std::vector +calcCurvature>( + const std::vector & points); +extern template std::vector +calcCurvature>( + const std::vector & points); +extern template std::vector +calcCurvature>( + const std::vector & points); + /** * @brief calculate curvature through points container and length of 2d distance for segment used * for curvature calculation. The method used for calculating the curvature is using 3 consecutive @@ -711,7 +960,7 @@ inline std::vector calcCurvature(const T & points) * curvature calculation */ template -inline std::vector> calcCurvatureAndArcLength(const T & points) +std::vector> calcCurvatureAndArcLength(const T & points) { // Note that arclength is for the segment, not the sum. std::vector> curvature_arc_length_vec; @@ -730,6 +979,16 @@ inline std::vector> calcCurvatureAndArcLength(const T return curvature_arc_length_vec; } +extern template std::vector> +calcCurvatureAndArcLength>( + const std::vector & points); +extern template std::vector> +calcCurvatureAndArcLength>( + const std::vector & points); +extern template std::vector> +calcCurvatureAndArcLength>( + const std::vector & points); + /** * @brief calculate length of 2D distance between given start point index in points container and * first point in container with zero longitudinal velocity @@ -757,6 +1016,11 @@ boost::optional calcDistanceToForwardStopPoint( return std::max(0.0, calcSignedArcLength(points_with_twist, src_idx, *closest_stop_idx)); } +extern template boost::optional +calcDistanceToForwardStopPoint>( + const std::vector & points_with_twist, + const size_t src_idx = 0); + /** * @brief calculate the point offset from source point index along the trajectory (or path) (points * container) @@ -767,7 +1031,7 @@ boost::optional calcDistanceToForwardStopPoint( * @return offset point */ template -inline boost::optional calcLongitudinalOffsetPoint( +boost::optional calcLongitudinalOffsetPoint( const T & points, const size_t src_idx, const double offset, const bool throw_exception = false) { try { @@ -779,6 +1043,7 @@ inline boost::optional calcLongitudinalOffsetPoint( if (points.size() - 1 < src_idx) { const auto e = std::out_of_range("Invalid source index"); + tier4_autoware_utils::print_backtrace(); if (throw_exception) { throw e; } @@ -821,6 +1086,19 @@ inline boost::optional calcLongitudinalOffsetPoint( return {}; } +extern template boost::optional +calcLongitudinalOffsetPoint>( + const std::vector & points, const size_t src_idx, + const double offset, const bool throw_exception = false); +extern template boost::optional +calcLongitudinalOffsetPoint>( + const std::vector & points, + const size_t src_idx, const double offset, const bool throw_exception = false); +extern template boost::optional +calcLongitudinalOffsetPoint>( + const std::vector & points, + const size_t src_idx, const double offset, const bool throw_exception = false); + /** * @brief calculate the point offset from source point along the trajectory (or path) (points * container) @@ -830,7 +1108,7 @@ inline boost::optional calcLongitudinalOffsetPoint( * @return offset point */ template -inline boost::optional calcLongitudinalOffsetPoint( +boost::optional calcLongitudinalOffsetPoint( const T & points, const geometry_msgs::msg::Point & src_point, const double offset) { try { @@ -853,6 +1131,19 @@ inline boost::optional calcLongitudinalOffsetPoint( return calcLongitudinalOffsetPoint(points, src_seg_idx, offset + signed_length_src_offset); } +extern template boost::optional +calcLongitudinalOffsetPoint>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const double offset); +extern template boost::optional +calcLongitudinalOffsetPoint>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const double offset); +extern template boost::optional +calcLongitudinalOffsetPoint>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const double offset); + /** * @brief calculate the point offset from source point index along the trajectory (or path) (points * container) @@ -864,7 +1155,7 @@ inline boost::optional calcLongitudinalOffsetPoint( * @return offset pose */ template -inline boost::optional calcLongitudinalOffsetPose( +boost::optional calcLongitudinalOffsetPose( const T & points, const size_t src_idx, const double offset, const bool set_orientation_from_position_direction = true, const bool throw_exception = false) { @@ -877,6 +1168,7 @@ inline boost::optional calcLongitudinalOffsetPose( if (points.size() - 1 < src_idx) { const auto e = std::out_of_range("Invalid source index"); + tier4_autoware_utils::print_backtrace(); if (throw_exception) { throw e; } @@ -935,6 +1227,22 @@ inline boost::optional calcLongitudinalOffsetPose( return {}; } +extern template boost::optional +calcLongitudinalOffsetPose>( + const std::vector & points, const size_t src_idx, + const double offset, const bool set_orientation_from_position_direction = true, + const bool throw_exception = false); +extern template boost::optional +calcLongitudinalOffsetPose>( + const std::vector & points, + const size_t src_idx, const double offset, + const bool set_orientation_from_position_direction = true, const bool throw_exception = false); +extern template boost::optional +calcLongitudinalOffsetPose>( + const std::vector & points, + const size_t src_idx, const double offset, + const bool set_orientation_from_position_direction = true, const bool throw_exception = false); + /** * @brief calculate the point offset from source point along the trajectory (or path) (points * container) @@ -946,7 +1254,7 @@ inline boost::optional calcLongitudinalOffsetPose( * @return offset pase */ template -inline boost::optional calcLongitudinalOffsetPose( +boost::optional calcLongitudinalOffsetPose( const T & points, const geometry_msgs::msg::Point & src_point, const double offset, const bool set_orientation_from_position_direction = true) { @@ -966,6 +1274,22 @@ inline boost::optional calcLongitudinalOffsetPose( set_orientation_from_position_direction); } +extern template boost::optional +calcLongitudinalOffsetPose>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const double offset, + const bool set_orientation_from_position_direction = true); +extern template boost::optional +calcLongitudinalOffsetPose>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const double offset, + const bool set_orientation_from_position_direction = true); +extern template boost::optional +calcLongitudinalOffsetPose>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const double offset, + const bool set_orientation_from_position_direction = true); + /** * @brief insert a point in points container (trajectory, path, ...) using segment id * @param seg_idx segment index of point at beginning of length @@ -976,7 +1300,7 @@ inline boost::optional calcLongitudinalOffsetPose( * @return index of segment id, where point is inserted */ template -inline boost::optional insertTargetPoint( +boost::optional insertTargetPoint( const size_t seg_idx, const geometry_msgs::msg::Point & p_target, T & points, const double overlap_threshold = 1e-3) { @@ -1052,6 +1376,22 @@ inline boost::optional insertTargetPoint( return seg_idx; } +extern template boost::optional +insertTargetPoint>( + const size_t seg_idx, const geometry_msgs::msg::Point & p_target, + std::vector & points, + const double overlap_threshold = 1e-3); +extern template boost::optional +insertTargetPoint>( + const size_t seg_idx, const geometry_msgs::msg::Point & p_target, + std::vector & points, + const double overlap_threshold = 1e-3); +extern template boost::optional +insertTargetPoint>( + const size_t seg_idx, const geometry_msgs::msg::Point & p_target, + std::vector & points, + const double overlap_threshold = 1e-3); + /** * @brief insert a point in points container (trajectory, path, ...) using length of point to be * inserted @@ -1063,7 +1403,7 @@ inline boost::optional insertTargetPoint( * @return index of segment id, where point is inserted */ template -inline boost::optional insertTargetPoint( +boost::optional insertTargetPoint( const double insert_point_length, const geometry_msgs::msg::Point & p_target, T & points, const double overlap_threshold = 1e-3) { @@ -1076,6 +1416,7 @@ inline boost::optional insertTargetPoint( // Get Nearest segment index boost::optional segment_idx = boost::none; for (size_t i = 1; i < points.size(); ++i) { + // TODO(Mamoru Sobue): find accumulated sum beforehand const double length = calcSignedArcLength(points, 0, i); if (insert_point_length <= length) { segment_idx = i - 1; @@ -1090,6 +1431,22 @@ inline boost::optional insertTargetPoint( return insertTargetPoint(*segment_idx, p_target, points, overlap_threshold); } +extern template boost::optional +insertTargetPoint>( + const double insert_point_length, const geometry_msgs::msg::Point & p_target, + std::vector & points, + const double overlap_threshold = 1e-3); +extern template boost::optional +insertTargetPoint>( + const double insert_point_length, const geometry_msgs::msg::Point & p_target, + std::vector & points, + const double overlap_threshold = 1e-3); +extern template boost::optional +insertTargetPoint>( + const double insert_point_length, const geometry_msgs::msg::Point & p_target, + std::vector & points, + const double overlap_threshold = 1e-3); + /** * @brief insert a point in points container (trajectory, path, ...) using segment index and length * of point to be inserted @@ -1101,7 +1458,7 @@ inline boost::optional insertTargetPoint( * @return index of insert point */ template -inline boost::optional insertTargetPoint( +boost::optional insertTargetPoint( const size_t src_segment_idx, const double insert_point_length, T & points, const double overlap_threshold = 1e-3) { @@ -1147,6 +1504,22 @@ inline boost::optional insertTargetPoint( return insertTargetPoint(*segment_idx, p_target, points, overlap_threshold); } +extern template boost::optional +insertTargetPoint>( + const size_t src_segment_idx, const double insert_point_length, + std::vector & points, + const double overlap_threshold = 1e-3); +extern template boost::optional +insertTargetPoint>( + const size_t src_segment_idx, const double insert_point_length, + std::vector & points, + const double overlap_threshold = 1e-3); +extern template boost::optional +insertTargetPoint>( + const size_t src_segment_idx, const double insert_point_length, + std::vector & points, + const double overlap_threshold = 1e-3); + /** * @brief Insert a target point from a source pose on the trajectory * @param src_pose source pose on the trajectory @@ -1161,7 +1534,7 @@ inline boost::optional insertTargetPoint( * @return index of insert point */ template -inline boost::optional insertTargetPoint( +boost::optional insertTargetPoint( const geometry_msgs::msg::Pose & src_pose, const double insert_point_length, T & points, const double max_dist = std::numeric_limits::max(), const double max_yaw = std::numeric_limits::max(), const double overlap_threshold = 1e-3) @@ -1184,6 +1557,25 @@ inline boost::optional insertTargetPoint( *nearest_segment_idx, insert_point_length + offset_length, points, overlap_threshold); } +extern template boost::optional +insertTargetPoint>( + const geometry_msgs::msg::Pose & src_pose, const double insert_point_length, + std::vector & points, + const double max_dist = std::numeric_limits::max(), + const double max_yaw = std::numeric_limits::max(), const double overlap_threshold = 1e-3); +extern template boost::optional +insertTargetPoint>( + const geometry_msgs::msg::Pose & src_pose, const double insert_point_length, + std::vector & points, + const double max_dist = std::numeric_limits::max(), + const double max_yaw = std::numeric_limits::max(), const double overlap_threshold = 1e-3); +extern template boost::optional +insertTargetPoint>( + const geometry_msgs::msg::Pose & src_pose, const double insert_point_length, + std::vector & points, + const double max_dist = std::numeric_limits::max(), + const double max_yaw = std::numeric_limits::max(), const double overlap_threshold = 1e-3); + /** * @brief Insert stop point from the source segment index * @param src_segment_idx start segment index on the trajectory @@ -1194,7 +1586,7 @@ inline boost::optional insertTargetPoint( * @return index of stop point */ template -inline boost::optional insertStopPoint( +boost::optional insertStopPoint( const size_t src_segment_idx, const double distance_to_stop_point, T & points_with_twist, const double overlap_threshold = 1e-3) { @@ -1217,6 +1609,22 @@ inline boost::optional insertStopPoint( return stop_idx; } +extern template boost::optional +insertStopPoint>( + const size_t src_segment_idx, const double distance_to_stop_point, + std::vector & points_with_twist, + const double overlap_threshold = 1e-3); +extern template boost::optional +insertStopPoint>( + const size_t src_segment_idx, const double distance_to_stop_point, + std::vector & points_with_twist, + const double overlap_threshold = 1e-3); +extern template boost::optional +insertStopPoint>( + const size_t src_segment_idx, const double distance_to_stop_point, + std::vector & points_with_twist, + const double overlap_threshold = 1e-3); + /** * @brief Insert stop point from the front point of the path * @param distance_to_stop_point distance to stop point from the front point of the path @@ -1226,7 +1634,7 @@ inline boost::optional insertStopPoint( * @return index of stop point */ template -inline boost::optional insertStopPoint( +boost::optional insertStopPoint( const double distance_to_stop_point, T & points_with_twist, const double overlap_threshold = 1e-3) { validateNonEmpty(points_with_twist); @@ -1250,6 +1658,22 @@ inline boost::optional insertStopPoint( return boost::none; } +extern template boost::optional +insertStopPoint>( + const double distance_to_stop_point, + std::vector & points_with_twist, + const double overlap_threshold = 1e-3); +extern template boost::optional +insertStopPoint>( + const double distance_to_stop_point, + std::vector & points_with_twist, + const double overlap_threshold = 1e-3); +extern template boost::optional +insertStopPoint>( + const double distance_to_stop_point, + std::vector & points_with_twist, + const double overlap_threshold = 1e-3); + /** * @brief Insert Stop point from the source pose * @param src_pose source pose @@ -1264,7 +1688,7 @@ inline boost::optional insertStopPoint( * @return index of stop point */ template -inline boost::optional insertStopPoint( +boost::optional insertStopPoint( const geometry_msgs::msg::Pose & src_pose, const double distance_to_stop_point, T & points_with_twist, const double max_dist = std::numeric_limits::max(), const double max_yaw = std::numeric_limits::max(), const double overlap_threshold = 1e-3) @@ -1289,6 +1713,25 @@ inline boost::optional insertStopPoint( return stop_idx; } +extern template boost::optional +insertStopPoint>( + const geometry_msgs::msg::Pose & src_pose, const double distance_to_stop_point, + std::vector & points_with_twist, + const double max_dist = std::numeric_limits::max(), + const double max_yaw = std::numeric_limits::max(), const double overlap_threshold = 1e-3); +extern template boost::optional +insertStopPoint>( + const geometry_msgs::msg::Pose & src_pose, const double distance_to_stop_point, + std::vector & points_with_twist, + const double max_dist = std::numeric_limits::max(), + const double max_yaw = std::numeric_limits::max(), const double overlap_threshold = 1e-3); +extern template boost::optional +insertStopPoint>( + const geometry_msgs::msg::Pose & src_pose, const double distance_to_stop_point, + std::vector & points_with_twist, + const double max_dist = std::numeric_limits::max(), + const double max_yaw = std::numeric_limits::max(), const double overlap_threshold = 1e-3); + /** * @brief Insert Stop point that is in the stop segment index * @param stop_seg_idx segment index of the stop pose @@ -1317,6 +1760,12 @@ boost::optional insertStopPoint( return insert_idx; } +extern template boost::optional +insertStopPoint>( + const size_t stop_seg_idx, const geometry_msgs::msg::Point & stop_point, + std::vector & points_with_twist, + const double overlap_threshold = 1e-3); + /** * @brief Insert deceleration point from the source pose * @param src_point source point @@ -1353,6 +1802,12 @@ boost::optional insertDecelPoint( return insert_idx; } +extern template boost::optional +insertDecelPoint>( + const geometry_msgs::msg::Point & src_point, const double distance_to_decel_point, + const double velocity, + std::vector & points_with_twist); + /** * @brief Insert orientation to each point in points container (trajectory, path, ...) * @param points points of trajectory, path, ... (input / output) @@ -1390,9 +1845,21 @@ void insertOrientation(T & points, const bool is_driving_forward) } } +extern template void insertOrientation>( + std::vector & points, const bool is_driving_forward); +extern template void +insertOrientation>( + std::vector & points, + const bool is_driving_forward); +extern template void +insertOrientation>( + std::vector & points, + const bool is_driving_forward); + /** * @brief Remove points with invalid orientation differences from a given points container - * (trajectory, path, ...) + * (trajectory, path, ...). Check the difference between the angles of two points and the difference + * between the azimuth angle between the two points and the angle of the next point. * @param points Points of trajectory, path, or other point container (input / output) * @param max_yaw_diff Maximum acceptable yaw angle difference between two consecutive points in * radians (default: M_PI_2) @@ -1401,10 +1868,14 @@ template void removeInvalidOrientationPoints(T & points, const double max_yaw_diff = M_PI_2) { for (size_t i = 1; i < points.size();) { - const double yaw1 = tf2::getYaw(tier4_autoware_utils::getPose(points.at(i - 1)).orientation); - const double yaw2 = tf2::getYaw(tier4_autoware_utils::getPose(points.at(i)).orientation); - - if (max_yaw_diff < std::abs(tier4_autoware_utils::normalizeRadian(yaw1 - yaw2))) { + const auto p1 = tier4_autoware_utils::getPose(points.at(i - 1)); + const auto p2 = tier4_autoware_utils::getPose(points.at(i)); + const double yaw1 = tf2::getYaw(p1.orientation); + const double yaw2 = tf2::getYaw(p2.orientation); + + if ( + max_yaw_diff < std::abs(tier4_autoware_utils::normalizeRadian(yaw1 - yaw2)) || + !tier4_autoware_utils::isDrivingForward(p1, p2)) { points.erase(points.begin() + i); } else { ++i; @@ -1440,6 +1911,22 @@ double calcSignedArcLength( return signed_length_on_traj - signed_length_src_offset + signed_length_dst_offset; } +extern template double +calcSignedArcLength>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, + const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); +extern template double +calcSignedArcLength>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, + const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); +extern template double +calcSignedArcLength>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, + const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); + /** * @brief calculate length of 2D distance between two points, specified by start point and its * segment index in points container and end point index in points container @@ -1465,6 +1952,19 @@ double calcSignedArcLength( return signed_length_on_traj - signed_length_src_offset; } +extern template double +calcSignedArcLength>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, const size_t dst_idx); +extern template double +calcSignedArcLength>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, const size_t dst_idx); +extern template double +calcSignedArcLength>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, const size_t dst_idx); + /** * @brief calculate length of 2D distance between two points, specified by start point index in * points container and end point and its segment index in points container @@ -1490,6 +1990,19 @@ double calcSignedArcLength( return signed_length_on_traj + signed_length_dst_offset; } +extern template double +calcSignedArcLength>( + const std::vector & points, const size_t src_idx, + const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); +extern template double +calcSignedArcLength>( + const std::vector & points, + const size_t src_idx, const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); +extern template double +calcSignedArcLength>( + const std::vector & points, + const size_t src_idx, const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); + /** * @brief find first nearest point index through points container for a given pose with soft * distance and yaw constraints. Finding nearest point is determined by looping through the points @@ -1580,6 +2093,25 @@ size_t findFirstNearestIndexWithSoftConstraints( return findNearestIndex(points, pose.position); } +extern template size_t +findFirstNearestIndexWithSoftConstraints>( + const std::vector & points, + const geometry_msgs::msg::Pose & pose, + const double dist_threshold = std::numeric_limits::max(), + const double yaw_threshold = std::numeric_limits::max()); +extern template size_t findFirstNearestIndexWithSoftConstraints< + std::vector>( + const std::vector & points, + const geometry_msgs::msg::Pose & pose, + const double dist_threshold = std::numeric_limits::max(), + const double yaw_threshold = std::numeric_limits::max()); +extern template size_t findFirstNearestIndexWithSoftConstraints< + std::vector>( + const std::vector & points, + const geometry_msgs::msg::Pose & pose, + const double dist_threshold = std::numeric_limits::max(), + const double yaw_threshold = std::numeric_limits::max()); + /** * @brief find nearest segment index to pose with soft constraints * Segment is straight path between two continuous points of trajectory @@ -1617,6 +2149,25 @@ size_t findFirstNearestSegmentIndexWithSoftConstraints( return nearest_idx; } +extern template size_t findFirstNearestSegmentIndexWithSoftConstraints< + std::vector>( + const std::vector & points, + const geometry_msgs::msg::Pose & pose, + const double dist_threshold = std::numeric_limits::max(), + const double yaw_threshold = std::numeric_limits::max()); +extern template size_t findFirstNearestSegmentIndexWithSoftConstraints< + std::vector>( + const std::vector & points, + const geometry_msgs::msg::Pose & pose, + const double dist_threshold = std::numeric_limits::max(), + const double yaw_threshold = std::numeric_limits::max()); +extern template size_t findFirstNearestSegmentIndexWithSoftConstraints< + std::vector>( + const std::vector & points, + const geometry_msgs::msg::Pose & pose, + const double dist_threshold = std::numeric_limits::max(), + const double yaw_threshold = std::numeric_limits::max()); + /** * @brief calculate the point offset from source point along the trajectory (or path) * @brief calculate length of 2D distance between given pose and first point in container with zero @@ -1663,6 +2214,22 @@ boost::optional calcDistanceToForwardStopPoint( return std::max(0.0, closest_stop_dist); } +extern template boost::optional +calcDistanceToForwardStopPoint>( + const std::vector & points_with_twist, + const geometry_msgs::msg::Pose & pose, const double max_dist = std::numeric_limits::max(), + const double max_yaw = std::numeric_limits::max()); +extern template boost::optional +calcDistanceToForwardStopPoint>( + const std::vector & points_with_twist, + const geometry_msgs::msg::Pose & pose, const double max_dist = std::numeric_limits::max(), + const double max_yaw = std::numeric_limits::max()); +extern template boost::optional +calcDistanceToForwardStopPoint>( + const std::vector & points_with_twist, + const geometry_msgs::msg::Pose & pose, const double max_dist = std::numeric_limits::max(), + const double max_yaw = std::numeric_limits::max()); + // NOTE: Points after forward length from the point will be cropped // forward_length is assumed to be positive. template @@ -1687,6 +2254,22 @@ T cropForwardPoints( return points; } +extern template std::vector +cropForwardPoints>( + const std::vector & points, + const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, + const double forward_length); +extern template std::vector +cropForwardPoints>( + const std::vector & points, + const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, + const double forward_length); +extern template std::vector +cropForwardPoints>( + const std::vector & points, + const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, + const double forward_length); + // NOTE: Points before backward length from the point will be cropped // backward_length is assumed to be positive. template @@ -1711,6 +2294,22 @@ T cropBackwardPoints( return points; } +extern template std::vector +cropBackwardPoints>( + const std::vector & points, + const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, + const double backward_length); +extern template std::vector +cropBackwardPoints>( + const std::vector & points, + const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, + const double backward_length); +extern template std::vector +cropBackwardPoints>( + const std::vector & points, + const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, + const double backward_length); + template T cropPoints( const T & points, const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, @@ -1739,6 +2338,22 @@ T cropPoints( return cropped_points; } +extern template std::vector +cropPoints>( + const std::vector & points, + const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, + const double forward_length, const double backward_length); +extern template std::vector +cropPoints>( + const std::vector & points, + const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, + const double forward_length, const double backward_length); +extern template std::vector +cropPoints>( + const std::vector & points, + const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, + const double forward_length, const double backward_length); + /** * @brief Calculate the angle of the input pose with respect to the nearest trajectory segment. * The function gets the nearest segment index between the points of the trajectory and the given @@ -1768,6 +2383,7 @@ double calcYawDeviation( if (overlap_removed_points.size() <= 1) { const std::runtime_error e("points size is less than 2"); + tier4_autoware_utils::print_backtrace(); if (throw_exception) { throw e; } @@ -1785,6 +2401,18 @@ double calcYawDeviation( return tier4_autoware_utils::normalizeRadian(pose_yaw - path_yaw); } +extern template double calcYawDeviation>( + const std::vector & points, + const geometry_msgs::msg::Pose & pose, const bool throw_exception = false); +extern template double +calcYawDeviation>( + const std::vector & points, + const geometry_msgs::msg::Pose & pose, const bool throw_exception = false); +extern template double +calcYawDeviation>( + const std::vector & points, + const geometry_msgs::msg::Pose & pose, const bool throw_exception = false); + /** * @brief Check if the given target point is in front of the based pose from the trajectory. * if the points is empty, the function returns false @@ -1808,6 +2436,22 @@ bool isTargetPointFront( return s_target - s_base > threshold; } + +extern template bool isTargetPointFront>( + const std::vector & points, + const geometry_msgs::msg::Point & base_point, const geometry_msgs::msg::Point & target_point, + const double threshold = 0.0); +extern template bool +isTargetPointFront>( + const std::vector & points, + const geometry_msgs::msg::Point & base_point, const geometry_msgs::msg::Point & target_point, + const double threshold = 0.0); +extern template bool +isTargetPointFront>( + const std::vector & points, + const geometry_msgs::msg::Point & base_point, const geometry_msgs::msg::Point & target_point, + const double threshold = 0.0); + } // namespace motion_utils #endif // MOTION_UTILS__TRAJECTORY__TRAJECTORY_HPP_ diff --git a/common/motion_utils/include/motion_utils/vehicle/vehicle_state_checker.hpp b/common/motion_utils/include/motion_utils/vehicle/vehicle_state_checker.hpp index f5f423f881ba2..6a9bd01a5a9a0 100644 --- a/common/motion_utils/include/motion_utils/vehicle/vehicle_state_checker.hpp +++ b/common/motion_utils/include/motion_utils/vehicle/vehicle_state_checker.hpp @@ -55,11 +55,11 @@ class VehicleStopChecker : public VehicleStopCheckerBase protected: rclcpp::Subscription::SharedPtr sub_odom_; - Odometry::SharedPtr odometry_ptr_; + Odometry::ConstSharedPtr odometry_ptr_; private: static constexpr double velocity_buffer_time_sec = 10.0; - void onOdom(const Odometry::SharedPtr msg); + void onOdom(const Odometry::ConstSharedPtr msg); }; class VehicleArrivalChecker : public VehicleStopChecker @@ -74,9 +74,9 @@ class VehicleArrivalChecker : public VehicleStopChecker rclcpp::Subscription::SharedPtr sub_trajectory_; - Trajectory::SharedPtr trajectory_ptr_; + Trajectory::ConstSharedPtr trajectory_ptr_; - void onTrajectory(const Trajectory::SharedPtr msg); + void onTrajectory(const Trajectory::ConstSharedPtr msg); }; } // namespace motion_utils diff --git a/common/motion_utils/package.xml b/common/motion_utils/package.xml index 834342624bfcd..334b023f7f29a 100644 --- a/common/motion_utils/package.xml +++ b/common/motion_utils/package.xml @@ -13,6 +13,7 @@ Taiki Tanaka Takamasa Horibe Tomoya Kimura + Mamoru Sobue Apache License 2.0 Yutaka Shimizu diff --git a/common/motion_utils/src/marker/marker_helper.cpp b/common/motion_utils/src/marker/marker_helper.cpp index 25d8616949b02..50def253b6d16 100644 --- a/common/motion_utils/src/marker/marker_helper.cpp +++ b/common/motion_utils/src/marker/marker_helper.cpp @@ -14,7 +14,12 @@ #include "motion_utils/marker/marker_helper.hpp" -#include +#include "motion_utils/resample/resample_utils.hpp" +#include "tier4_autoware_utils/ros/marker_helper.hpp" + +#include + +#include using tier4_autoware_utils::appendMarkerArray; using tier4_autoware_utils::createDefaultMarker; @@ -86,10 +91,11 @@ namespace motion_utils { visualization_msgs::msg::MarkerArray createStopVirtualWallMarker( const geometry_msgs::msg::Pose & pose, const std::string & module_name, const rclcpp::Time & now, - const int32_t id, const double longitudinal_offset, const std::string & ns_prefix) + const int32_t id, const double longitudinal_offset, const std::string & ns_prefix, + const bool is_driving_forward) { - const auto pose_with_offset = - tier4_autoware_utils::calcOffsetPose(pose, longitudinal_offset, 0.0, 0.0); + const auto pose_with_offset = tier4_autoware_utils::calcOffsetPose( + pose, longitudinal_offset * (is_driving_forward ? 1.0 : -1.0), 0.0, 0.0); return createVirtualWallMarkerArray( pose_with_offset, module_name, ns_prefix + "stop_", now, id, createMarkerColor(1.0, 0.0, 0.0, 0.5)); @@ -97,10 +103,11 @@ visualization_msgs::msg::MarkerArray createStopVirtualWallMarker( visualization_msgs::msg::MarkerArray createSlowDownVirtualWallMarker( const geometry_msgs::msg::Pose & pose, const std::string & module_name, const rclcpp::Time & now, - const int32_t id, const double longitudinal_offset, const std::string & ns_prefix) + const int32_t id, const double longitudinal_offset, const std::string & ns_prefix, + const bool is_driving_forward) { - const auto pose_with_offset = - tier4_autoware_utils::calcOffsetPose(pose, longitudinal_offset, 0.0, 0.0); + const auto pose_with_offset = tier4_autoware_utils::calcOffsetPose( + pose, longitudinal_offset * (is_driving_forward ? 1.0 : -1.0), 0.0, 0.0); return createVirtualWallMarkerArray( pose_with_offset, module_name, ns_prefix + "slow_down_", now, id, createMarkerColor(1.0, 1.0, 0.0, 0.5)); @@ -108,10 +115,11 @@ visualization_msgs::msg::MarkerArray createSlowDownVirtualWallMarker( visualization_msgs::msg::MarkerArray createDeadLineVirtualWallMarker( const geometry_msgs::msg::Pose & pose, const std::string & module_name, const rclcpp::Time & now, - const int32_t id, const double longitudinal_offset, const std::string & ns_prefix) + const int32_t id, const double longitudinal_offset, const std::string & ns_prefix, + const bool is_driving_forward) { - const auto pose_with_offset = - tier4_autoware_utils::calcOffsetPose(pose, longitudinal_offset, 0.0, 0.0); + const auto pose_with_offset = tier4_autoware_utils::calcOffsetPose( + pose, longitudinal_offset * (is_driving_forward ? 1.0 : -1.0), 0.0, 0.0); return createVirtualWallMarkerArray( pose_with_offset, module_name, ns_prefix + "dead_line_", now, id, createMarkerColor(0.0, 1.0, 0.0, 0.5)); diff --git a/common/motion_utils/src/marker/virtual_wall_marker_creator.cpp b/common/motion_utils/src/marker/virtual_wall_marker_creator.cpp index 63c456ab24fb0..c22aaa0d5e87c 100644 --- a/common/motion_utils/src/marker/virtual_wall_marker_creator.cpp +++ b/common/motion_utils/src/marker/virtual_wall_marker_creator.cpp @@ -14,6 +14,8 @@ #include "motion_utils/marker/virtual_wall_marker_creator.hpp" +#include "motion_utils/marker/marker_helper.hpp" + namespace motion_utils { @@ -64,7 +66,7 @@ visualization_msgs::msg::MarkerArray VirtualWallMarkerCreator::create_markers( } auto markers = create_fn( virtual_wall.pose, virtual_wall.text, now, 0, virtual_wall.longitudinal_offset, - virtual_wall.ns); + virtual_wall.ns, virtual_wall.is_driving_forward); for (auto & marker : markers.markers) { marker.id = marker_count_per_namespace[marker.ns].current++; marker_array.markers.push_back(marker); diff --git a/common/motion_utils/src/resample/resample.cpp b/common/motion_utils/src/resample/resample.cpp index 091f3405e2815..6bada921804d4 100644 --- a/common/motion_utils/src/resample/resample.cpp +++ b/common/motion_utils/src/resample/resample.cpp @@ -14,8 +14,15 @@ #include "motion_utils/resample/resample.hpp" +#include "interpolation/interpolation_utils.hpp" +#include "interpolation/linear_interpolation.hpp" +#include "interpolation/spline_interpolation.hpp" +#include "interpolation/zero_order_hold.hpp" #include "motion_utils/resample/resample_utils.hpp" +#include "motion_utils/trajectory/trajectory.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" +#include "tier4_autoware_utils/geometry/pose_deviation.hpp" +#include "tier4_autoware_utils/math/constants.hpp" namespace motion_utils { @@ -109,13 +116,16 @@ std::vector resamplePointVector( } std::vector resamplePoseVector( - const std::vector & points, + const std::vector & points_raw, const std::vector & resampled_arclength, const bool use_akima_spline_for_xy, const bool use_lerp_for_z) { + // Remove overlap points for resampling + const auto points = motion_utils::removeOverlapPoints(points_raw); + // validate arguments if (!resample_utils::validate_arguments(points, resampled_arclength)) { - return points; + return points_raw; } std::vector position(points.size()); diff --git a/common/motion_utils/src/trajectory/path_with_lane_id.cpp b/common/motion_utils/src/trajectory/path_with_lane_id.cpp index 759f0ef4ed790..c74db0f61d6fb 100644 --- a/common/motion_utils/src/trajectory/path_with_lane_id.cpp +++ b/common/motion_utils/src/trajectory/path_with_lane_id.cpp @@ -14,8 +14,90 @@ #include "motion_utils/trajectory/path_with_lane_id.hpp" +#include "interpolation/spline_interpolation_points_2d.hpp" +#include "motion_utils/trajectory/trajectory.hpp" + +#include +#include +#include + namespace motion_utils { + +boost::optional> getPathIndexRangeWithLaneId( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int64_t target_lane_id) +{ + size_t start_idx = 0; // NOTE: to prevent from maybe-uninitialized error + size_t end_idx = 0; // NOTE: to prevent from maybe-uninitialized error + + bool found_first_idx = false; + for (size_t i = 0; i < path.points.size(); ++i) { + const auto & p = path.points.at(i); + for (const auto & id : p.lane_ids) { + if (id == target_lane_id) { + if (!found_first_idx) { + start_idx = i; + found_first_idx = true; + } + end_idx = i; + } + } + } + + if (found_first_idx) { + // NOTE: In order to fully cover lanes with target_lane_id, start_idx and end_idx are expanded. + start_idx = static_cast(std::max(0, static_cast(start_idx) - 1)); + end_idx = std::min(path.points.size() - 1, end_idx + 1); + + return std::make_pair(start_idx, end_idx); + } + + return {}; +} + +size_t findNearestIndexFromLaneId( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const geometry_msgs::msg::Point & pos, const int64_t lane_id) +{ + const auto opt_range = getPathIndexRangeWithLaneId(path, lane_id); + if (opt_range) { + const size_t start_idx = opt_range->first; + const size_t end_idx = opt_range->second; + + validateNonEmpty(path.points); + + const auto sub_points = std::vector{ + path.points.begin() + start_idx, path.points.begin() + end_idx + 1}; + validateNonEmpty(sub_points); + + return start_idx + findNearestIndex(sub_points, pos); + } + + return findNearestIndex(path.points, pos); +} + +size_t findNearestSegmentIndexFromLaneId( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const geometry_msgs::msg::Point & pos, const int64_t lane_id) +{ + const size_t nearest_idx = findNearestIndexFromLaneId(path, pos, lane_id); + + if (nearest_idx == 0) { + return 0; + } + if (nearest_idx == path.points.size() - 1) { + return path.points.size() - 2; + } + + const double signed_length = calcLongitudinalOffsetToSegment(path.points, nearest_idx, pos); + + if (signed_length <= 0) { + return nearest_idx - 1; + } + + return nearest_idx; +} + // NOTE: rear_to_cog is supposed to be positive autoware_auto_planning_msgs::msg::PathWithLaneId convertToRearWheelCenter( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const double rear_to_cog, diff --git a/common/motion_utils/src/trajectory/tmp_conversion.cpp b/common/motion_utils/src/trajectory/tmp_conversion.cpp new file mode 100644 index 0000000000000..5c907a5926046 --- /dev/null +++ b/common/motion_utils/src/trajectory/tmp_conversion.cpp @@ -0,0 +1,60 @@ +// Copyright 2023 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 "motion_utils/trajectory/tmp_conversion.hpp" + +#include "tier4_autoware_utils/geometry/geometry.hpp" +#include "tier4_autoware_utils/geometry/pose_deviation.hpp" + +#include + +namespace motion_utils +{ +/** + * @brief Convert std::vector to + * autoware_auto_planning_msgs::msg::Trajectory. This function is temporarily added for porting to + * autoware_auto_msgs. We should consider whether to remove this function after the porting is done. + * @attention This function just clips + * std::vector up to the capacity of Trajectory. + * Therefore, the error handling out of this function is necessary if the size of the input greater + * than the capacity. + * @todo Decide how to handle the situation that we need to use the trajectory with the size of + * points larger than the capacity. (Tier IV) + */ +autoware_auto_planning_msgs::msg::Trajectory convertToTrajectory( + const std::vector & trajectory) +{ + autoware_auto_planning_msgs::msg::Trajectory output{}; + for (const auto & pt : trajectory) { + output.points.push_back(pt); + if (output.points.size() >= output.CAPACITY) { + break; + } + } + return output; +} + +/** + * @brief Convert autoware_auto_planning_msgs::msg::Trajectory to + * std::vector. + */ +std::vector convertToTrajectoryPointArray( + const autoware_auto_planning_msgs::msg::Trajectory & trajectory) +{ + std::vector output(trajectory.points.size()); + std::copy(trajectory.points.begin(), trajectory.points.end(), output.begin()); + return output; +} + +} // namespace motion_utils diff --git a/common/motion_utils/src/trajectory/trajectory.cpp b/common/motion_utils/src/trajectory/trajectory.cpp new file mode 100644 index 0000000000000..12074e537c106 --- /dev/null +++ b/common/motion_utils/src/trajectory/trajectory.cpp @@ -0,0 +1,625 @@ +// Copyright 2023 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 "motion_utils/trajectory/trajectory.hpp" + +namespace motion_utils +{ + +// +template void validateNonEmpty>( + const std::vector &); +template void validateNonEmpty>( + const std::vector &); +template void validateNonEmpty>( + const std::vector &); + +// +template boost::optional +isDrivingForward>( + const std::vector &); +template boost::optional +isDrivingForward>( + const std::vector &); +template boost::optional +isDrivingForward>( + const std::vector &); + +// +template boost::optional +isDrivingForwardWithTwist>( + const std::vector &); +template boost::optional +isDrivingForwardWithTwist>( + const std::vector &); +template boost::optional +isDrivingForwardWithTwist>( + const std::vector &); + +// +template std::vector +removeOverlapPoints>( + const std::vector & points, const size_t start_idx); +template std::vector +removeOverlapPoints>( + const std::vector & points, + const size_t start_idx); +template std::vector +removeOverlapPoints>( + const std::vector & points, + const size_t start_idx); + +// +template boost::optional +searchZeroVelocityIndex>( + const std::vector & points_with_twist, + const size_t src_idx, const size_t dst_idx); + +// +template boost::optional +searchZeroVelocityIndex>( + const std::vector & points_with_twist, + const size_t src_idx); + +// +template boost::optional +searchZeroVelocityIndex>( + const std::vector & points_with_twist); + +// +template size_t findNearestIndex>( + const std::vector & points, + const geometry_msgs::msg::Point & point); +template size_t +findNearestIndex>( + const std::vector & points, + const geometry_msgs::msg::Point & point); +template size_t findNearestIndex>( + const std::vector & points, + const geometry_msgs::msg::Point & point); + +// +template boost::optional +findNearestIndex>( + const std::vector & points, + const geometry_msgs::msg::Pose & pose, const double max_dist, const double max_yaw); +template boost::optional +findNearestIndex>( + const std::vector & points, + const geometry_msgs::msg::Pose & pose, const double max_dist, const double max_yaw); +template boost::optional +findNearestIndex>( + const std::vector & points, + const geometry_msgs::msg::Pose & pose, const double max_dist, const double max_yaw); + +// +template double +calcLongitudinalOffsetToSegment>( + const std::vector & points, const size_t seg_idx, + const geometry_msgs::msg::Point & p_target, const bool throw_exception); +template double +calcLongitudinalOffsetToSegment>( + const std::vector & points, + const size_t seg_idx, const geometry_msgs::msg::Point & p_target, const bool throw_exception); +template double +calcLongitudinalOffsetToSegment>( + const std::vector & points, + const size_t seg_idx, const geometry_msgs::msg::Point & p_target, const bool throw_exception); + +// +template size_t findNearestSegmentIndex>( + const std::vector & points, + const geometry_msgs::msg::Point & point); +template size_t +findNearestSegmentIndex>( + const std::vector & points, + const geometry_msgs::msg::Point & point); +template size_t +findNearestSegmentIndex>( + const std::vector & points, + const geometry_msgs::msg::Point & point); + +// +template boost::optional +findNearestSegmentIndex>( + const std::vector & points, + const geometry_msgs::msg::Pose & pose, const double max_dist, const double max_yaw); +template boost::optional +findNearestSegmentIndex>( + const std::vector & points, + const geometry_msgs::msg::Pose & pose, const double max_dist, const double max_yaw); +template boost::optional +findNearestSegmentIndex>( + const std::vector & points, + const geometry_msgs::msg::Pose & pose, const double max_dist, const double max_yaw); + +// +template double calcLateralOffset>( + const std::vector & points, + const geometry_msgs::msg::Point & p_target, const size_t seg_idx, const bool throw_exception); +template double +calcLateralOffset>( + const std::vector & points, + const geometry_msgs::msg::Point & p_target, const size_t seg_idx, const bool throw_exception); +template double calcLateralOffset>( + const std::vector & points, + const geometry_msgs::msg::Point & p_target, const size_t seg_idx, const bool throw_exception); + +// +template double calcLateralOffset>( + const std::vector & points, + const geometry_msgs::msg::Point & p_target, const bool throw_exception); +template double +calcLateralOffset>( + const std::vector & points, + const geometry_msgs::msg::Point & p_target, const bool throw_exception); +template double calcLateralOffset>( + const std::vector & points, + const geometry_msgs::msg::Point & p_target, const bool throw_exception); + +// +template double calcSignedArcLength>( + const std::vector & points, const size_t src_idx, + const size_t dst_idx); +template double +calcSignedArcLength>( + const std::vector & points, + const size_t src_idx, const size_t dst_idx); +template double calcSignedArcLength>( + const std::vector & points, + const size_t src_idx, const size_t dst_idx); + +// +template std::vector +calcSignedArcLengthPartialSum>( + const std::vector & points, const size_t src_idx, + const size_t dst_idx); +template std::vector +calcSignedArcLengthPartialSum>( + const std::vector & points, + const size_t src_idx, const size_t dst_idx); +template std::vector +calcSignedArcLengthPartialSum>( + const std::vector & points, + const size_t src_idx, const size_t dst_idx); + +// +template double calcSignedArcLength>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const size_t dst_idx); +template double +calcSignedArcLength>( + const std::vector &, + const geometry_msgs::msg::Point & src_point, const size_t dst_idx); +template double calcSignedArcLength>( + const std::vector &, + const geometry_msgs::msg::Point & src_point, const size_t dst_idx); + +// +template double calcSignedArcLength>( + const std::vector & points, const size_t src_idx, + const geometry_msgs::msg::Point & dst_point); +template double +calcSignedArcLength>( + const std::vector & points, + const size_t src_idx, const geometry_msgs::msg::Point & dst_point); +template double calcSignedArcLength>( + const std::vector & points, + const size_t src_idx, const geometry_msgs::msg::Point & dst_point); + +// +template double calcSignedArcLength>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const geometry_msgs::msg::Point & dst_point); +template double +calcSignedArcLength>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const geometry_msgs::msg::Point & dst_point); +template double calcSignedArcLength>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const geometry_msgs::msg::Point & dst_point); + +// +template double calcArcLength>( + const std::vector & points); +template double calcArcLength>( + const std::vector & points); +template double calcArcLength>( + const std::vector & points); + +// +template std::vector +calcCurvature>( + const std::vector & points); +template std::vector +calcCurvature>( + const std::vector & points); +template std::vector +calcCurvature>( + const std::vector & points); + +// +template std::vector> +calcCurvatureAndArcLength>( + const std::vector & points); +template std::vector> +calcCurvatureAndArcLength>( + const std::vector & points); +template std::vector> +calcCurvatureAndArcLength>( + const std::vector & points); + +// +template boost::optional +calcDistanceToForwardStopPoint>( + const std::vector & points_with_twist, + const size_t src_idx); + +// +template boost::optional +calcLongitudinalOffsetPoint>( + const std::vector & points, const size_t src_idx, + const double offset, const bool throw_exception); +template boost::optional +calcLongitudinalOffsetPoint>( + const std::vector & points, + const size_t src_idx, const double offset, const bool throw_exception); +template boost::optional +calcLongitudinalOffsetPoint>( + const std::vector & points, + const size_t src_idx, const double offset, const bool throw_exception); + +// +template boost::optional +calcLongitudinalOffsetPoint>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const double offset); +template boost::optional +calcLongitudinalOffsetPoint>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const double offset); +template boost::optional +calcLongitudinalOffsetPoint>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const double offset); + +// +template boost::optional +calcLongitudinalOffsetPose>( + const std::vector & points, const size_t src_idx, + const double offset, const bool set_orientation_from_position_direction, + const bool throw_exception); +template boost::optional +calcLongitudinalOffsetPose>( + const std::vector & points, + const size_t src_idx, const double offset, const bool set_orientation_from_position_direction, + const bool throw_exception); +template boost::optional +calcLongitudinalOffsetPose>( + const std::vector & points, + const size_t src_idx, const double offset, const bool set_orientation_from_position_direction, + const bool throw_exception); + +// +template boost::optional +calcLongitudinalOffsetPose>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const double offset, + const bool set_orientation_from_position_direction); +template boost::optional +calcLongitudinalOffsetPose>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const double offset, + const bool set_orientation_from_position_direction); +template boost::optional +calcLongitudinalOffsetPose>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const double offset, + const bool set_orientation_from_position_direction); + +// +template boost::optional +insertTargetPoint>( + const size_t seg_idx, const geometry_msgs::msg::Point & p_target, + std::vector & points, + const double overlap_threshold); +template boost::optional +insertTargetPoint>( + const size_t seg_idx, const geometry_msgs::msg::Point & p_target, + std::vector & points, + const double overlap_threshold); +template boost::optional +insertTargetPoint>( + const size_t seg_idx, const geometry_msgs::msg::Point & p_target, + std::vector & points, + const double overlap_threshold); + +// +template boost::optional +insertTargetPoint>( + const double insert_point_length, const geometry_msgs::msg::Point & p_target, + std::vector & points, + const double overlap_threshold); +template boost::optional +insertTargetPoint>( + const double insert_point_length, const geometry_msgs::msg::Point & p_target, + std::vector & points, + const double overlap_threshold); +template boost::optional +insertTargetPoint>( + const double insert_point_length, const geometry_msgs::msg::Point & p_target, + std::vector & points, + const double overlap_threshold); + +// +template boost::optional +insertTargetPoint>( + const size_t src_segment_idx, const double insert_point_length, + std::vector & points, + const double overlap_threshold); +template boost::optional +insertTargetPoint>( + const size_t src_segment_idx, const double insert_point_length, + std::vector & points, + const double overlap_threshold); +template boost::optional +insertTargetPoint>( + const size_t src_segment_idx, const double insert_point_length, + std::vector & points, + const double overlap_threshold); + +// +template boost::optional +insertTargetPoint>( + const geometry_msgs::msg::Pose & src_pose, const double insert_point_length, + std::vector & points, const double max_dist, + const double max_yaw, const double overlap_threshold); +template boost::optional +insertTargetPoint>( + const geometry_msgs::msg::Pose & src_pose, const double insert_point_length, + std::vector & points, + const double max_dist, const double max_yaw, const double overlap_threshold); +template boost::optional +insertTargetPoint>( + const geometry_msgs::msg::Pose & src_pose, const double insert_point_length, + std::vector & points, const double max_dist, + const double max_yaw, const double overlap_threshold); + +// +template boost::optional +insertStopPoint>( + const size_t src_segment_idx, const double distance_to_stop_point, + std::vector & points_with_twist, + const double overlap_threshold = 1e-3); +template boost::optional +insertStopPoint>( + const size_t src_segment_idx, const double distance_to_stop_point, + std::vector & points_with_twist, + const double overlap_threshold = 1e-3); +template boost::optional +insertStopPoint>( + const size_t src_segment_idx, const double distance_to_stop_point, + std::vector & points_with_twist, + const double overlap_threshold = 1e-3); + +// +template boost::optional +insertStopPoint>( + const double distance_to_stop_point, + std::vector & points_with_twist, + const double overlap_threshold); +template boost::optional +insertStopPoint>( + const double distance_to_stop_point, + std::vector & points_with_twist, + const double overlap_threshold); +template boost::optional +insertStopPoint>( + const double distance_to_stop_point, + std::vector & points_with_twist, + const double overlap_threshold); + +// +template boost::optional +insertStopPoint>( + const geometry_msgs::msg::Pose & src_pose, const double distance_to_stop_point, + std::vector & points_with_twist, + const double max_dist, const double max_yaw, const double overlap_threshold); +template boost::optional +insertStopPoint>( + const geometry_msgs::msg::Pose & src_pose, const double distance_to_stop_point, + std::vector & points_with_twist, + const double max_dist, const double max_yaw, const double overlap_threshold); +template boost::optional +insertStopPoint>( + const geometry_msgs::msg::Pose & src_pose, const double distance_to_stop_point, + std::vector & points_with_twist, + const double max_dist, const double max_yaw, const double overlap_threshold); + +// +template boost::optional +insertStopPoint>( + const size_t stop_seg_idx, const geometry_msgs::msg::Point & stop_point, + std::vector & points_with_twist, + const double overlap_threshold); + +// +template boost::optional +insertDecelPoint>( + const geometry_msgs::msg::Point & src_point, const double distance_to_decel_point, + const double velocity, + std::vector & points_with_twist); + +// +template void insertOrientation>( + std::vector & points, const bool is_driving_forward); +template void insertOrientation>( + std::vector & points, + const bool is_driving_forward); +template void insertOrientation>( + std::vector & points, + const bool is_driving_forward); + +// +template double calcSignedArcLength>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, + const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); +template double +calcSignedArcLength>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, + const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); +template double calcSignedArcLength>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, + const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); + +// +template double calcSignedArcLength>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, const size_t dst_idx); +template double +calcSignedArcLength>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, const size_t dst_idx); +template double calcSignedArcLength>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, const size_t dst_idx); + +// +template double calcSignedArcLength>( + const std::vector & points, const size_t src_idx, + const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); +template double +calcSignedArcLength>( + const std::vector & points, + const size_t src_idx, const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); +template double calcSignedArcLength>( + const std::vector & points, + const size_t src_idx, const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); + +// +template size_t +findFirstNearestIndexWithSoftConstraints>( + const std::vector & points, + const geometry_msgs::msg::Pose & pose, const double dist_threshold, const double yaw_threshold); +template size_t findFirstNearestIndexWithSoftConstraints< + std::vector>( + const std::vector & points, + const geometry_msgs::msg::Pose & pose, const double dist_threshold, const double yaw_threshold); +template size_t findFirstNearestIndexWithSoftConstraints< + std::vector>( + const std::vector & points, + const geometry_msgs::msg::Pose & pose, const double dist_threshold, const double yaw_threshold); + +// +template size_t findFirstNearestSegmentIndexWithSoftConstraints< + std::vector>( + const std::vector & points, + const geometry_msgs::msg::Pose & pose, const double dist_threshold, const double yaw_threshold); +template size_t findFirstNearestSegmentIndexWithSoftConstraints< + std::vector>( + const std::vector & points, + const geometry_msgs::msg::Pose & pose, const double dist_threshold, const double yaw_threshold); +template size_t findFirstNearestSegmentIndexWithSoftConstraints< + std::vector>( + const std::vector & points, + const geometry_msgs::msg::Pose & pose, const double dist_threshold, const double yaw_threshold); + +// +template boost::optional +calcDistanceToForwardStopPoint>( + const std::vector & points_with_twist, + const geometry_msgs::msg::Pose & pose, const double max_dist, const double max_yaw); + +// +template std::vector +cropForwardPoints>( + const std::vector & points, + const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, + const double forward_length); +template std::vector +cropForwardPoints>( + const std::vector & points, + const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, + const double forward_length); +template std::vector +cropForwardPoints>( + const std::vector & points, + const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, + const double forward_length); + +// +template std::vector +cropBackwardPoints>( + const std::vector & points, + const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, + const double backward_length); +template std::vector +cropBackwardPoints>( + const std::vector & points, + const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, + const double backward_length); +template std::vector +cropBackwardPoints>( + const std::vector & points, + const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, + const double backward_length); + +// +template std::vector +cropPoints>( + const std::vector & points, + const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, + const double forward_length, const double backward_length); +template std::vector +cropPoints>( + const std::vector & points, + const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, + const double forward_length, const double backward_length); +template std::vector +cropPoints>( + const std::vector & points, + const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, + const double forward_length, const double backward_length); + +// +template double calcYawDeviation>( + const std::vector & points, + const geometry_msgs::msg::Pose & pose, const bool throw_exception); +template double +calcYawDeviation>( + const std::vector & points, + const geometry_msgs::msg::Pose & pose, const bool throw_exception); +template double calcYawDeviation>( + const std::vector & points, + const geometry_msgs::msg::Pose & pose, const bool throw_exception); + +// +template bool isTargetPointFront>( + const std::vector & points, + const geometry_msgs::msg::Point & base_point, const geometry_msgs::msg::Point & target_point, + const double threshold); +template bool +isTargetPointFront>( + const std::vector & points, + const geometry_msgs::msg::Point & base_point, const geometry_msgs::msg::Point & target_point, + const double threshold); +template bool isTargetPointFront>( + const std::vector & points, + const geometry_msgs::msg::Point & base_point, const geometry_msgs::msg::Point & target_point, + const double threshold); + +} // namespace motion_utils diff --git a/common/motion_utils/src/vehicle/vehicle_state_checker.cpp b/common/motion_utils/src/vehicle/vehicle_state_checker.cpp index 6ed9472d39e0f..194c49ae3bff8 100644 --- a/common/motion_utils/src/vehicle/vehicle_state_checker.cpp +++ b/common/motion_utils/src/vehicle/vehicle_state_checker.cpp @@ -88,7 +88,7 @@ VehicleStopChecker::VehicleStopChecker(rclcpp::Node * node) std::bind(&VehicleStopChecker::onOdom, this, _1)); } -void VehicleStopChecker::onOdom(const Odometry::SharedPtr msg) +void VehicleStopChecker::onOdom(const Odometry::ConstSharedPtr msg) { odometry_ptr_ = msg; @@ -128,7 +128,7 @@ bool VehicleArrivalChecker::isVehicleStoppedAtStopPoint(const double stop_durati th_arrived_distance_m; } -void VehicleArrivalChecker::onTrajectory(const Trajectory::SharedPtr msg) +void VehicleArrivalChecker::onTrajectory(const Trajectory::ConstSharedPtr msg) { trajectory_ptr_ = msg; } diff --git a/common/motion_utils/test/src/resample/test_resample.cpp b/common/motion_utils/test/src/resample/test_resample.cpp index d7ce15b4c30d9..373a12c5bbd76 100644 --- a/common/motion_utils/test/src/resample/test_resample.cpp +++ b/common/motion_utils/test/src/resample/test_resample.cpp @@ -15,6 +15,7 @@ #include "motion_utils/constants.hpp" #include "motion_utils/resample/resample.hpp" #include "tier4_autoware_utils/geometry/boost_geometry.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" #include "tier4_autoware_utils/math/constants.hpp" #include "tier4_autoware_utils/math/unit_conversion.hpp" diff --git a/common/motion_utils/test/src/test_motion_utils.cpp b/common/motion_utils/test/src/test_motion_utils.cpp index a8d9f76116018..d8b9b0ac78981 100644 --- a/common/motion_utils/test/src/test_motion_utils.cpp +++ b/common/motion_utils/test/src/test_motion_utils.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "motion_utils/motion_utils.hpp" +#include #include diff --git a/common/motion_utils/test/src/trajectory/test_path_with_lane_id.cpp b/common/motion_utils/test/src/trajectory/test_path_with_lane_id.cpp index 121a9fa69beab..4df1ef0a094f7 100644 --- a/common/motion_utils/test/src/trajectory/test_path_with_lane_id.cpp +++ b/common/motion_utils/test/src/trajectory/test_path_with_lane_id.cpp @@ -13,8 +13,7 @@ // limitations under the License. #include "motion_utils/trajectory/path_with_lane_id.hpp" -// #include "tier4_autoware_utils/geometry/geometry.hpp" -// #include "tier4_autoware_utils/math/unit_conversion.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" #include diff --git a/common/object_recognition_utils/include/object_recognition_utils/transform.hpp b/common/object_recognition_utils/include/object_recognition_utils/transform.hpp index 4446c87427e88..31892853a855f 100644 --- a/common/object_recognition_utils/include/object_recognition_utils/transform.hpp +++ b/common/object_recognition_utils/include/object_recognition_utils/transform.hpp @@ -15,15 +15,22 @@ #ifndef OBJECT_RECOGNITION_UTILS__TRANSFORM_HPP_ #define OBJECT_RECOGNITION_UTILS__TRANSFORM_HPP_ +#include + #include +#include +#include #include #include #include #ifdef ROS_DISTRO_GALACTIC +#include #include #else +#include + #include #endif @@ -45,6 +52,23 @@ namespace detail return boost::none; } } + +[[maybe_unused]] inline boost::optional getTransformMatrix( + const std::string & in_target_frame, const std_msgs::msg::Header & in_cloud_header, + const tf2_ros::Buffer & tf_buffer) +{ + try { + geometry_msgs::msg::TransformStamped transform_stamped; + transform_stamped = tf_buffer.lookupTransform( + in_target_frame, in_cloud_header.frame_id, in_cloud_header.stamp, + rclcpp::Duration::from_seconds(1.0)); + Eigen::Matrix4f mat = tf2::transformToEigen(transform_stamped.transform).matrix().cast(); + return mat; + } catch (tf2::TransformException & e) { + RCLCPP_WARN_STREAM(rclcpp::get_logger("detail::getTransformMatrix"), e.what()); + return boost::none; + } +} } // namespace detail namespace object_recognition_utils @@ -79,6 +103,46 @@ bool transformObjects( } return true; } +template +bool transformObjectsWithFeature( + const T & input_msg, const std::string & target_frame_id, const tf2_ros::Buffer & tf_buffer, + T & output_msg) +{ + output_msg = input_msg; + if (input_msg.header.frame_id != target_frame_id) { + output_msg.header.frame_id = target_frame_id; + tf2::Transform tf_target2objects_world; + tf2::Transform tf_target2objects; + tf2::Transform tf_objects_world2objects; + const auto ros_target2objects_world = detail::getTransform( + tf_buffer, input_msg.header.frame_id, target_frame_id, input_msg.header.stamp); + if (!ros_target2objects_world) { + return false; + } + const auto tf_matrix = detail::getTransformMatrix(target_frame_id, input_msg.header, tf_buffer); + if (!tf_matrix) { + RCLCPP_WARN( + rclcpp::get_logger("object_recognition_utils:"), "failed to get transformed matrix"); + return false; + } + for (auto & feature_object : output_msg.feature_objects) { + // transform object + tf2::fromMsg( + feature_object.object.kinematics.pose_with_covariance.pose, tf_objects_world2objects); + tf_target2objects = tf_target2objects_world * tf_objects_world2objects; + tf2::toMsg(tf_target2objects, feature_object.object.kinematics.pose_with_covariance.pose); + + // transform cluster + sensor_msgs::msg::PointCloud2 transformed_cluster; + pcl_ros::transformPointCloud(*tf_matrix, feature_object.feature.cluster, transformed_cluster); + transformed_cluster.header.frame_id = target_frame_id; + feature_object.feature.cluster = transformed_cluster; + } + output_msg.header.frame_id = target_frame_id; + return true; + } + return true; +} } // namespace object_recognition_utils #endif // OBJECT_RECOGNITION_UTILS__TRANSFORM_HPP_ diff --git a/common/object_recognition_utils/package.xml b/common/object_recognition_utils/package.xml index 95925e846a55c..2f2472515ebad 100644 --- a/common/object_recognition_utils/package.xml +++ b/common/object_recognition_utils/package.xml @@ -17,7 +17,13 @@ geometry_msgs interpolation libboost-dev + pcl_conversions + pcl_ros rclcpp + sensor_msgs + std_msgs + tf2 + tf2_eigen tier4_autoware_utils ament_cmake_ros diff --git a/common/path_distance_calculator/src/path_distance_calculator.cpp b/common/path_distance_calculator/src/path_distance_calculator.cpp index e4c9c8e027f7b..aa20dd1ffc7ce 100644 --- a/common/path_distance_calculator/src/path_distance_calculator.cpp +++ b/common/path_distance_calculator/src/path_distance_calculator.cpp @@ -14,8 +14,7 @@ #include "path_distance_calculator.hpp" -#include -#include +#include #include #include diff --git a/common/perception_utils/CMakeLists.txt b/common/perception_utils/CMakeLists.txt index 302bb63512937..d4bbe928c0be8 100644 --- a/common/perception_utils/CMakeLists.txt +++ b/common/perception_utils/CMakeLists.txt @@ -4,6 +4,4 @@ project(perception_utils) find_package(autoware_cmake REQUIRED) autoware_package() -find_package(Boost REQUIRED) - ament_auto_package() diff --git a/common/perception_utils/package.xml b/common/perception_utils/package.xml index 6cae7f9ce8c4f..5304d481737e5 100644 --- a/common/perception_utils/package.xml +++ b/common/perception_utils/package.xml @@ -4,7 +4,6 @@ perception_utils 0.1.0 The perception_utils package - Mingyu Li Satoshi Tanaka Yusuke Muramatsu Shunsuke Miura diff --git a/common/rtc_manager_rviz_plugin/src/rtc_manager_panel.cpp b/common/rtc_manager_rviz_plugin/src/rtc_manager_panel.cpp index ff448e694eb15..058a5d5deb5d6 100644 --- a/common/rtc_manager_rviz_plugin/src/rtc_manager_panel.cpp +++ b/common/rtc_manager_rviz_plugin/src/rtc_manager_panel.cpp @@ -366,7 +366,10 @@ void RTCManagerPanel::onRTCStatus(const CooperateStatusArray::ConstSharedPtr msg rtc_table_->clearContents(); num_rtc_status_ptr_->setText( QString::fromStdString("The Number of RTC Statuses: " + std::to_string(msg->statuses.size()))); - if (msg->statuses.empty()) return; + if (msg->statuses.empty()) { + rtc_table_->update(); + return; + } // this is to stable rtc display not to occupy too much size_t min_display_size{5}; size_t max_display_size{10}; @@ -374,8 +377,17 @@ void RTCManagerPanel::onRTCStatus(const CooperateStatusArray::ConstSharedPtr msg rtc_table_->setRowCount( std::max(min_display_size, std::min(msg->statuses.size(), max_display_size))); int cnt = 0; - for (auto status : msg->statuses) { - if (static_cast(cnt) >= max_display_size) return; + + auto sorted_statuses = msg->statuses; + std::partition(sorted_statuses.begin(), sorted_statuses.end(), [](const auto & status) { + return !status.auto_mode && !uint2bool(status.command_status.type); + }); + + for (auto status : sorted_statuses) { + if (static_cast(cnt) >= max_display_size) { + rtc_table_->update(); + return; + } // uuid { std::stringstream uuid; diff --git a/common/tier4_autoware_utils/CMakeLists.txt b/common/tier4_autoware_utils/CMakeLists.txt index 071b55177a581..9cb54e52362a5 100644 --- a/common/tier4_autoware_utils/CMakeLists.txt +++ b/common/tier4_autoware_utils/CMakeLists.txt @@ -7,11 +7,15 @@ autoware_package() find_package(Boost REQUIRED) ament_auto_add_library(tier4_autoware_utils SHARED - src/tier4_autoware_utils.cpp + src/geometry/geometry.cpp + src/geometry/pose_deviation.cpp src/geometry/boost_polygon_utils.cpp src/math/sin_table.cpp src/math/trigonometry.cpp src/ros/msg_operation.cpp + src/ros/marker_helper.cpp + src/ros/logger_level_configure.cpp + src/system/backtrace.cpp ) if(BUILD_TESTING) diff --git a/common/tier4_autoware_utils/README.md b/common/tier4_autoware_utils/README.md index 7a352e971a58d..ffcc414cd76ce 100644 --- a/common/tier4_autoware_utils/README.md +++ b/common/tier4_autoware_utils/README.md @@ -3,3 +3,7 @@ ## Purpose This package contains many common functions used by other packages, so please refer to them as needed. + +## For developers + +`tier4_autoware_utils.hpp` header file was removed because the source files that directly/indirectly include this file took a long time for preprocessing. diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/boost_geometry.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/boost_geometry.hpp index 91f9ab09a1e08..04bcfbacbbc17 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/boost_geometry.hpp +++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/boost_geometry.hpp @@ -21,7 +21,6 @@ #define EIGEN_MPL2_ONLY #include -#include #include diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/boost_polygon_utils.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/boost_polygon_utils.hpp index 46cbd1f141e87..5d9001affa308 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/boost_polygon_utils.hpp +++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/boost_polygon_utils.hpp @@ -22,9 +22,6 @@ #include #include -#include -#include - #include namespace tier4_autoware_utils diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/geometry.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/geometry.hpp index 8c3bcd966cc7a..6106e3a15f4c7 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/geometry.hpp +++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/geometry.hpp @@ -15,20 +15,20 @@ #ifndef TIER4_AUTOWARE_UTILS__GEOMETRY__GEOMETRY_HPP_ #define TIER4_AUTOWARE_UTILS__GEOMETRY__GEOMETRY_HPP_ -#include -#include -#include - -#define EIGEN_MPL2_ONLY #include "tier4_autoware_utils/geometry/boost_geometry.hpp" #include "tier4_autoware_utils/math/constants.hpp" #include "tier4_autoware_utils/math/normalization.hpp" #include "tier4_autoware_utils/ros/msg_covariance.hpp" +#include +#include +#include + +#define EIGEN_MPL2_ONLY #include -#include #include +#include #include #include #include @@ -38,27 +38,15 @@ #include #include #include +#include #include -#ifdef ROS_DISTRO_GALACTIC -#include -#else -#include -#endif - // TODO(wep21): Remove these apis // after they are implemented in ros2 geometry2. namespace tf2 { -inline void fromMsg(const geometry_msgs::msg::PoseStamped & msg, tf2::Stamped & out) -{ - out.stamp_ = tf2_ros::fromMsg(msg.header.stamp); - out.frame_id_ = msg.header.frame_id; - tf2::Transform tmp; - fromMsg(msg.pose, tmp); - out.setData(tmp); -} +void fromMsg(const geometry_msgs::msg::PoseStamped & msg, tf2::Stamped & out); #ifdef ROS_DISTRO_GALACTIC // Remove after this commit is released // https://github.com/ros2/geometry2/commit/e9da371d81e388a589540357c050e262442f1b4a @@ -146,6 +134,13 @@ inline geometry_msgs::msg::Point getPoint(const autoware_auto_planning_msgs::msg return p.pose.position; } +template <> +inline geometry_msgs::msg::Point getPoint( + const autoware_auto_planning_msgs::msg::PathPointWithLaneId & p) +{ + return p.point.pose.position; +} + template <> inline geometry_msgs::msg::Point getPoint( const autoware_auto_planning_msgs::msg::TrajectoryPoint & p) @@ -178,6 +173,13 @@ inline geometry_msgs::msg::Pose getPose(const autoware_auto_planning_msgs::msg:: return p.pose; } +template <> +inline geometry_msgs::msg::Pose getPose( + const autoware_auto_planning_msgs::msg::PathPointWithLaneId & p) +{ + return p.point.pose; +} + template <> inline geometry_msgs::msg::Pose getPose(const autoware_auto_planning_msgs::msg::TrajectoryPoint & p) { @@ -197,6 +199,13 @@ inline double getLongitudinalVelocity(const autoware_auto_planning_msgs::msg::Pa return p.longitudinal_velocity_mps; } +template <> +inline double getLongitudinalVelocity( + const autoware_auto_planning_msgs::msg::PathPointWithLaneId & p) +{ + return p.point.longitudinal_velocity_mps; +} + template <> inline double getLongitudinalVelocity(const autoware_auto_planning_msgs::msg::TrajectoryPoint & p) { @@ -229,6 +238,13 @@ inline void setPose( p.pose = pose; } +template <> +inline void setPose( + const geometry_msgs::msg::Pose & pose, autoware_auto_planning_msgs::msg::PathPointWithLaneId & p) +{ + p.point.pose = pose; +} + template <> inline void setPose( const geometry_msgs::msg::Pose & pose, autoware_auto_planning_msgs::msg::TrajectoryPoint & p) @@ -245,7 +261,7 @@ inline void setOrientation(const geometry_msgs::msg::Quaternion & orientation, T } template -void setLongitudinalVelocity([[maybe_unused]] const double velocity, [[maybe_unused]] T & p) +void setLongitudinalVelocity([[maybe_unused]] const float velocity, [[maybe_unused]] T & p) { static_assert(sizeof(T) == 0, "Only specializations of getLongitudinalVelocity can be used."); throw std::logic_error("Only specializations of getLongitudinalVelocity can be used."); @@ -253,18 +269,25 @@ void setLongitudinalVelocity([[maybe_unused]] const double velocity, [[maybe_unu template <> inline void setLongitudinalVelocity( - const double velocity, autoware_auto_planning_msgs::msg::TrajectoryPoint & p) + const float velocity, autoware_auto_planning_msgs::msg::TrajectoryPoint & p) { p.longitudinal_velocity_mps = velocity; } template <> inline void setLongitudinalVelocity( - const double velocity, autoware_auto_planning_msgs::msg::PathPoint & p) + const float velocity, autoware_auto_planning_msgs::msg::PathPoint & p) { p.longitudinal_velocity_mps = velocity; } +template <> +inline void setLongitudinalVelocity( + const float velocity, autoware_auto_planning_msgs::msg::PathPointWithLaneId & p) +{ + p.point.longitudinal_velocity_mps = velocity; +} + inline geometry_msgs::msg::Point createPoint(const double x, const double y, const double z) { geometry_msgs::msg::Point p; @@ -274,66 +297,25 @@ inline geometry_msgs::msg::Point createPoint(const double x, const double y, con return p; } -inline geometry_msgs::msg::Vector3 getRPY(const geometry_msgs::msg::Quaternion & quat) -{ - geometry_msgs::msg::Vector3 rpy; - tf2::Quaternion q(quat.x, quat.y, quat.z, quat.w); - tf2::Matrix3x3(q).getRPY(rpy.x, rpy.y, rpy.z); - return rpy; -} +geometry_msgs::msg::Vector3 getRPY(const geometry_msgs::msg::Quaternion & quat); -inline geometry_msgs::msg::Vector3 getRPY(const geometry_msgs::msg::Pose & pose) -{ - return getRPY(pose.orientation); -} +geometry_msgs::msg::Vector3 getRPY(const geometry_msgs::msg::Pose & pose); -inline geometry_msgs::msg::Vector3 getRPY(const geometry_msgs::msg::PoseStamped & pose) -{ - return getRPY(pose.pose); -} +geometry_msgs::msg::Vector3 getRPY(const geometry_msgs::msg::PoseStamped & pose); -inline geometry_msgs::msg::Vector3 getRPY( - const geometry_msgs::msg::PoseWithCovarianceStamped & pose) -{ - return getRPY(pose.pose.pose); -} +geometry_msgs::msg::Vector3 getRPY(const geometry_msgs::msg::PoseWithCovarianceStamped & pose); -inline geometry_msgs::msg::Quaternion createQuaternion( - const double x, const double y, const double z, const double w) -{ - geometry_msgs::msg::Quaternion q; - q.x = x; - q.y = y; - q.z = z; - q.w = w; - return q; -} +geometry_msgs::msg::Quaternion createQuaternion( + const double x, const double y, const double z, const double w); -inline geometry_msgs::msg::Vector3 createTranslation(const double x, const double y, const double z) -{ - geometry_msgs::msg::Vector3 v; - v.x = x; - v.y = y; - v.z = z; - return v; -} +geometry_msgs::msg::Vector3 createTranslation(const double x, const double y, const double z); // Revival of tf::createQuaternionFromRPY // https://answers.ros.org/question/304397/recommended-way-to-construct-quaternion-from-rollpitchyaw-with-tf2/ -inline geometry_msgs::msg::Quaternion createQuaternionFromRPY( - const double roll, const double pitch, const double yaw) -{ - tf2::Quaternion q; - q.setRPY(roll, pitch, yaw); - return tf2::toMsg(q); -} +geometry_msgs::msg::Quaternion createQuaternionFromRPY( + const double roll, const double pitch, const double yaw); -inline geometry_msgs::msg::Quaternion createQuaternionFromYaw(const double yaw) -{ - tf2::Quaternion q; - q.setRPY(0, 0, yaw); - return tf2::toMsg(q); -} +geometry_msgs::msg::Quaternion createQuaternionFromYaw(const double yaw); template double calcDistance2d(const Point1 & point1, const Point2 & point2) @@ -371,13 +353,8 @@ double calcDistance3d(const Point1 & point1, const Point2 & point2) * @param p_to target point * @return -pi/2 <= elevation angle <= pi/2 */ -inline double calcElevationAngle( - const geometry_msgs::msg::Point & p_from, const geometry_msgs::msg::Point & p_to) -{ - const double dz = p_to.z - p_from.z; - const double dist_2d = calcDistance2d(p_from, p_to); - return std::atan2(dz, dist_2d); -} +double calcElevationAngle( + const geometry_msgs::msg::Point & p_from, const geometry_msgs::msg::Point & p_to); /** * @brief calculate azimuth angle of two points. @@ -388,52 +365,18 @@ inline double calcElevationAngle( * @param p_to target point * @return -pi < azimuth angle < pi. */ -inline double calcAzimuthAngle( - const geometry_msgs::msg::Point & p_from, const geometry_msgs::msg::Point & p_to) -{ - const double dx = p_to.x - p_from.x; - const double dy = p_to.y - p_from.y; - return std::atan2(dy, dx); -} +double calcAzimuthAngle( + const geometry_msgs::msg::Point & p_from, const geometry_msgs::msg::Point & p_to); -inline geometry_msgs::msg::Pose transform2pose(const geometry_msgs::msg::Transform & transform) -{ - geometry_msgs::msg::Pose pose; - pose.position.x = transform.translation.x; - pose.position.y = transform.translation.y; - pose.position.z = transform.translation.z; - pose.orientation = transform.rotation; - return pose; -} +geometry_msgs::msg::Pose transform2pose(const geometry_msgs::msg::Transform & transform); -inline geometry_msgs::msg::PoseStamped transform2pose( - const geometry_msgs::msg::TransformStamped & transform) -{ - geometry_msgs::msg::PoseStamped pose; - pose.header = transform.header; - pose.pose = transform2pose(transform.transform); - return pose; -} +geometry_msgs::msg::PoseStamped transform2pose( + const geometry_msgs::msg::TransformStamped & transform); -inline geometry_msgs::msg::Transform pose2transform(const geometry_msgs::msg::Pose & pose) -{ - geometry_msgs::msg::Transform transform; - transform.translation.x = pose.position.x; - transform.translation.y = pose.position.y; - transform.translation.z = pose.position.z; - transform.rotation = pose.orientation; - return transform; -} +geometry_msgs::msg::Transform pose2transform(const geometry_msgs::msg::Pose & pose); -inline geometry_msgs::msg::TransformStamped pose2transform( - const geometry_msgs::msg::PoseStamped & pose, const std::string & child_frame_id) -{ - geometry_msgs::msg::TransformStamped transform; - transform.header = pose.header; - transform.transform = pose2transform(pose.pose); - transform.child_frame_id = child_frame_id; - return transform; -} +geometry_msgs::msg::TransformStamped pose2transform( + const geometry_msgs::msg::PoseStamped & pose, const std::string & child_frame_id); template tf2::Vector3 point2tfVector(const Point1 & src, const Point2 & dst) @@ -447,65 +390,18 @@ tf2::Vector3 point2tfVector(const Point1 & src, const Point2 & dst) return tf2::Vector3(dx, dy, dz); } -inline Point3d transformPoint( - const Point3d & point, const geometry_msgs::msg::Transform & transform) -{ - const auto & translation = transform.translation; - const auto & rotation = transform.rotation; - - const Eigen::Translation3d T(translation.x, translation.y, translation.z); - const Eigen::Quaterniond R(rotation.w, rotation.x, rotation.y, rotation.z); +Point3d transformPoint(const Point3d & point, const geometry_msgs::msg::Transform & transform); - const Eigen::Vector3d transformed(T * R * point); - - return Point3d{transformed.x(), transformed.y(), transformed.z()}; -} +Point2d transformPoint(const Point2d & point, const geometry_msgs::msg::Transform & transform); -inline Point2d transformPoint( - const Point2d & point, const geometry_msgs::msg::Transform & transform) -{ - Point3d point_3d{point.x(), point.y(), 0}; - const auto transformed = transformPoint(point_3d, transform); - return Point2d{transformed.x(), transformed.y()}; -} +Eigen::Vector3d transformPoint( + const Eigen::Vector3d & point, const geometry_msgs::msg::Pose & pose); -inline Eigen::Vector3d transformPoint( - const Eigen::Vector3d & point, const geometry_msgs::msg::Pose & pose) -{ - geometry_msgs::msg::Transform transform; - transform.translation.x = pose.position.x; - transform.translation.y = pose.position.y; - transform.translation.z = pose.position.z; - transform.rotation = pose.orientation; +geometry_msgs::msg::Point transformPoint( + const geometry_msgs::msg::Point & point, const geometry_msgs::msg::Pose & pose); - Point3d p = transformPoint(Point3d(point.x(), point.y(), point.z()), transform); - return Eigen::Vector3d(p.x(), p.y(), p.z()); -} - -inline geometry_msgs::msg::Point transformPoint( - const geometry_msgs::msg::Point & point, const geometry_msgs::msg::Pose & pose) -{ - const Eigen::Vector3d vec = Eigen::Vector3d(point.x, point.y, point.z); - auto transformed_vec = transformPoint(vec, pose); - - geometry_msgs::msg::Point transformed_point; - transformed_point.x = transformed_vec.x(); - transformed_point.y = transformed_vec.y(); - transformed_point.z = transformed_vec.z(); - return transformed_point; -} - -inline geometry_msgs::msg::Point32 transformPoint( - const geometry_msgs::msg::Point32 & point32, const geometry_msgs::msg::Pose & pose) -{ - const auto point = - geometry_msgs::build().x(point32.x).y(point32.y).z(point32.z); - const auto transformed_point = tier4_autoware_utils::transformPoint(point, pose); - return geometry_msgs::build() - .x(transformed_point.x) - .y(transformed_point.y) - .z(transformed_point.z); -} +geometry_msgs::msg::Point32 transformPoint( + const geometry_msgs::msg::Point32 & point32, const geometry_msgs::msg::Pose & pose); template T transformVector(const T & points, const geometry_msgs::msg::Transform & transform) @@ -517,110 +413,40 @@ T transformVector(const T & points, const geometry_msgs::msg::Transform & transf return transformed; } -inline geometry_msgs::msg::Pose transformPose( - const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::TransformStamped & transform) -{ - geometry_msgs::msg::Pose transformed_pose; - tf2::doTransform(pose, transformed_pose, transform); +geometry_msgs::msg::Pose transformPose( + const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::TransformStamped & transform); - return transformed_pose; -} - -inline geometry_msgs::msg::Pose transformPose( - const geometry_msgs::msg::Pose & pose, geometry_msgs::msg::Transform & transform) -{ - geometry_msgs::msg::TransformStamped transform_stamped; - transform_stamped.transform = transform; - - return transformPose(pose, transform_stamped); -} - -inline geometry_msgs::msg::Pose transformPose( - const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Pose & pose_transform) -{ - tf2::Transform transform; - tf2::convert(pose_transform, transform); +geometry_msgs::msg::Pose transformPose( + const geometry_msgs::msg::Pose & pose, geometry_msgs::msg::Transform & transform); - geometry_msgs::msg::TransformStamped transform_msg; - transform_msg.transform = tf2::toMsg(transform); - - return transformPose(pose, transform_msg); -} +geometry_msgs::msg::Pose transformPose( + const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Pose & pose_transform); // Transform pose in world coordinates to local coordinates -inline geometry_msgs::msg::Pose inverseTransformPose( - const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::TransformStamped & transform) -{ - tf2::Transform tf; - tf2::fromMsg(transform, tf); - geometry_msgs::msg::TransformStamped transform_stamped; - transform_stamped.transform = tf2::toMsg(tf.inverse()); - - return transformPose(pose, transform_stamped); -} +/* +geometry_msgs::msg::Pose inverseTransformPose( + const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::TransformStamped & transform); +*/ // Transform pose in world coordinates to local coordinates -inline geometry_msgs::msg::Pose inverseTransformPose( - const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Transform & transform) -{ - tf2::Transform tf; - tf2::fromMsg(transform, tf); - geometry_msgs::msg::TransformStamped transform_stamped; - transform_stamped.transform = tf2::toMsg(tf.inverse()); - - return transformPose(pose, transform_stamped); -} +geometry_msgs::msg::Pose inverseTransformPose( + const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Transform & transform); // Transform pose in world coordinates to local coordinates -inline geometry_msgs::msg::Pose inverseTransformPose( - const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Pose & transform_pose) -{ - tf2::Transform transform; - tf2::convert(transform_pose, transform); - - return inverseTransformPose(pose, tf2::toMsg(transform)); -} +geometry_msgs::msg::Pose inverseTransformPose( + const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Pose & transform_pose); // Transform point in world coordinates to local coordinates -inline Eigen::Vector3d inverseTransformPoint( - const Eigen::Vector3d & point, const geometry_msgs::msg::Pose & pose) -{ - const Eigen::Quaterniond q( - pose.orientation.w, pose.orientation.x, pose.orientation.y, pose.orientation.z); - const Eigen::Matrix3d R = q.normalized().toRotationMatrix(); - - const Eigen::Vector3d local_origin(pose.position.x, pose.position.y, pose.position.z); - Eigen::Vector3d local_point = R.transpose() * point - R.transpose() * local_origin; - - return local_point; -} +Eigen::Vector3d inverseTransformPoint( + const Eigen::Vector3d & point, const geometry_msgs::msg::Pose & pose); // Transform point in world coordinates to local coordinates -inline geometry_msgs::msg::Point inverseTransformPoint( - const geometry_msgs::msg::Point & point, const geometry_msgs::msg::Pose & pose) -{ - const Eigen::Vector3d local_vec = - inverseTransformPoint(Eigen::Vector3d(point.x, point.y, point.z), pose); - geometry_msgs::msg::Point local_point; - local_point.x = local_vec.x(); - local_point.y = local_vec.y(); - local_point.z = local_vec.z(); - return local_point; -} +geometry_msgs::msg::Point inverseTransformPoint( + const geometry_msgs::msg::Point & point, const geometry_msgs::msg::Pose & pose); -inline double calcCurvature( +double calcCurvature( const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2, - const geometry_msgs::msg::Point & p3) -{ - // Calculation details are described in the following page - // https://en.wikipedia.org/wiki/Menger_curvature - const double denominator = - calcDistance2d(p1, p2) * calcDistance2d(p2, p3) * calcDistance2d(p3, p1); - if (std::fabs(denominator) < 1e-10) { - throw std::runtime_error("points are too close for curvature calculation."); - } - return 2.0 * ((p2.x - p1.x) * (p3.y - p1.y) - (p2.y - p1.y) * (p3.x - p1.x)) / denominator; -} + const geometry_msgs::msg::Point & p3); template bool isDrivingForward(const Pose1 & src_pose, const Pose2 & dst_pose) @@ -635,20 +461,8 @@ bool isDrivingForward(const Pose1 & src_pose, const Pose2 & dst_pose) * @brief Calculate offset pose. The offset values are defined in the local coordinate of the input * pose. */ -inline geometry_msgs::msg::Pose calcOffsetPose( - const geometry_msgs::msg::Pose & p, const double x, const double y, const double z) -{ - geometry_msgs::msg::Pose pose; - geometry_msgs::msg::Transform transform; - transform.translation = createTranslation(x, y, z); - transform.rotation = createQuaternion(0.0, 0.0, 0.0, 1.0); - tf2::Transform tf_pose; - tf2::Transform tf_offset; - tf2::fromMsg(transform, tf_offset); - tf2::fromMsg(p, tf_pose); - tf2::toMsg(tf_pose * tf_offset, pose); - return pose; -} +geometry_msgs::msg::Pose calcOffsetPose( + const geometry_msgs::msg::Pose & p, const double x, const double y, const double z); /** * @brief Calculate a point by linear interpolation. @@ -759,39 +573,13 @@ inline double calcNorm(const geometry_msgs::msg::Vector3 & v) * @return If all element of covariance is 0, return false. */ // -inline bool isTwistCovarianceValid( - const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance) -{ - for (const auto & c : twist_with_covariance.covariance) { - if (c != 0.0) { - return true; - } - } - return false; -} +bool isTwistCovarianceValid(const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance); // NOTE: much faster than boost::geometry::intersects() -inline std::optional intersect( +std::optional intersect( const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2, - const geometry_msgs::msg::Point & p3, const geometry_msgs::msg::Point & p4) -{ - // calculate intersection point - const double det = (p1.x - p2.x) * (p4.y - p3.y) - (p4.x - p3.x) * (p1.y - p2.y); - if (det == 0.0) { - return std::nullopt; - } + const geometry_msgs::msg::Point & p3, const geometry_msgs::msg::Point & p4); - const double t = ((p4.y - p3.y) * (p4.x - p2.x) + (p3.x - p4.x) * (p4.y - p2.y)) / det; - const double s = ((p2.y - p1.y) * (p4.x - p2.x) + (p1.x - p2.x) * (p4.y - p2.y)) / det; - if (t < 0 || 1 < t || s < 0 || 1 < s) { - return std::nullopt; - } - - geometry_msgs::msg::Point intersect_point; - intersect_point.x = t * p1.x + (1.0 - t) * p2.x; - intersect_point.y = t * p1.y + (1.0 - t) * p2.y; - return intersect_point; -} } // namespace tier4_autoware_utils #endif // TIER4_AUTOWARE_UTILS__GEOMETRY__GEOMETRY_HPP_ diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/path_with_lane_id_geometry.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/path_with_lane_id_geometry.hpp deleted file mode 100644 index 91ba0c4d5de47..0000000000000 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/path_with_lane_id_geometry.hpp +++ /dev/null @@ -1,60 +0,0 @@ -// 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 TIER4_AUTOWARE_UTILS__GEOMETRY__PATH_WITH_LANE_ID_GEOMETRY_HPP_ -#define TIER4_AUTOWARE_UTILS__GEOMETRY__PATH_WITH_LANE_ID_GEOMETRY_HPP_ - -#include "tier4_autoware_utils/geometry/geometry.hpp" - -#include - -namespace tier4_autoware_utils -{ -template <> -inline geometry_msgs::msg::Point getPoint( - const autoware_auto_planning_msgs::msg::PathPointWithLaneId & p) -{ - return p.point.pose.position; -} - -template <> -inline geometry_msgs::msg::Pose getPose( - const autoware_auto_planning_msgs::msg::PathPointWithLaneId & p) -{ - return p.point.pose; -} - -template <> -inline double getLongitudinalVelocity( - const autoware_auto_planning_msgs::msg::PathPointWithLaneId & p) -{ - return p.point.longitudinal_velocity_mps; -} - -template <> -inline void setPose( - const geometry_msgs::msg::Pose & pose, autoware_auto_planning_msgs::msg::PathPointWithLaneId & p) -{ - p.point.pose = pose; -} - -template <> -inline void setLongitudinalVelocity( - const double velocity, autoware_auto_planning_msgs::msg::PathPointWithLaneId & p) -{ - p.point.longitudinal_velocity_mps = velocity; -} -} // namespace tier4_autoware_utils - -#endif // TIER4_AUTOWARE_UTILS__GEOMETRY__PATH_WITH_LANE_ID_GEOMETRY_HPP_ diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/pose_deviation.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/pose_deviation.hpp index 32b4ba663fcd7..dfa678eaa07d9 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/pose_deviation.hpp +++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/pose_deviation.hpp @@ -15,10 +15,8 @@ #ifndef TIER4_AUTOWARE_UTILS__GEOMETRY__POSE_DEVIATION_HPP_ #define TIER4_AUTOWARE_UTILS__GEOMETRY__POSE_DEVIATION_HPP_ -#include "tier4_autoware_utils/geometry/geometry.hpp" -#include "tier4_autoware_utils/math/normalization.hpp" - -#include +#include +#include namespace tier4_autoware_utils { @@ -29,57 +27,18 @@ struct PoseDeviation double yaw{0.0}; }; -inline double calcLateralDeviation( - const geometry_msgs::msg::Pose & base_pose, const geometry_msgs::msg::Point & target_point) -{ - const auto & base_point = base_pose.position; - - const auto yaw = tf2::getYaw(base_pose.orientation); - const Eigen::Vector3d base_unit_vec{std::cos(yaw), std::sin(yaw), 0}; - - const auto dx = target_point.x - base_point.x; - const auto dy = target_point.y - base_point.y; - const Eigen::Vector3d diff_vec{dx, dy, 0}; +double calcLateralDeviation( + const geometry_msgs::msg::Pose & base_pose, const geometry_msgs::msg::Point & target_point); - const Eigen::Vector3d cross_vec = base_unit_vec.cross(diff_vec); - - return cross_vec.z(); -} - -inline double calcLongitudinalDeviation( - const geometry_msgs::msg::Pose & base_pose, const geometry_msgs::msg::Point & target_point) -{ - const auto & base_point = base_pose.position; +double calcLongitudinalDeviation( + const geometry_msgs::msg::Pose & base_pose, const geometry_msgs::msg::Point & target_point); - const auto yaw = tf2::getYaw(base_pose.orientation); - const Eigen::Vector3d base_unit_vec{std::cos(yaw), std::sin(yaw), 0}; - - const auto dx = target_point.x - base_point.x; - const auto dy = target_point.y - base_point.y; - const Eigen::Vector3d diff_vec{dx, dy, 0}; - - return base_unit_vec.dot(diff_vec); -} - -inline double calcYawDeviation( - const geometry_msgs::msg::Pose & base_pose, const geometry_msgs::msg::Pose & target_pose) -{ - const auto base_yaw = tf2::getYaw(base_pose.orientation); - const auto target_yaw = tf2::getYaw(target_pose.orientation); - return normalizeRadian(target_yaw - base_yaw); -} - -inline PoseDeviation calcPoseDeviation( - const geometry_msgs::msg::Pose & base_pose, const geometry_msgs::msg::Pose & target_pose) -{ - PoseDeviation deviation{}; +double calcYawDeviation( + const geometry_msgs::msg::Pose & base_pose, const geometry_msgs::msg::Pose & target_pose); - deviation.lateral = calcLateralDeviation(base_pose, target_pose.position); - deviation.longitudinal = calcLongitudinalDeviation(base_pose, target_pose.position); - deviation.yaw = calcYawDeviation(base_pose, target_pose); +PoseDeviation calcPoseDeviation( + const geometry_msgs::msg::Pose & base_pose, const geometry_msgs::msg::Pose & target_pose); - return deviation; -} } // namespace tier4_autoware_utils #endif // TIER4_AUTOWARE_UTILS__GEOMETRY__POSE_DEVIATION_HPP_ diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/logger_level_configure.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/logger_level_configure.hpp new file mode 100644 index 0000000000000..5aee3a251dad2 --- /dev/null +++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/logger_level_configure.hpp @@ -0,0 +1,68 @@ +// Copyright 2023 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. + +// =============== Note =============== +// This is a util class implementation of the logger_config_component provided by ROS 2 +// https://github.com/ros2/demos/blob/humble/logging_demo/src/logger_config_component.cpp +// +// When ROS 2 officially supports the set_logger_level option in release version, this class can be +// removed. +// https://github.com/ros2/ros2/issues/1355 + +// =============== How to use =============== +// ___In your_node.hpp___ +// #include "tier4_autoware_utils/ros/logger_level_configure.hpp" +// class YourNode : public rclcpp::Node { +// ... +// +// // Define logger_configure as a node class member variable +// std::unique_ptr logger_configure_; +// } +// +// ___In your_node.cpp___ +// YourNode::YourNode() { +// ... +// +// // Set up logger_configure +// logger_configure_ = std::make_unique(this); +// } + +#ifndef TIER4_AUTOWARE_UTILS__ROS__LOGGER_LEVEL_CONFIGURE_HPP_ +#define TIER4_AUTOWARE_UTILS__ROS__LOGGER_LEVEL_CONFIGURE_HPP_ + +#include "logging_demo/srv/config_logger.hpp" + +#include + +namespace tier4_autoware_utils +{ +class LoggerLevelConfigure +{ +private: + using ConfigLogger = logging_demo::srv::ConfigLogger; + +public: + explicit LoggerLevelConfigure(rclcpp::Node * node); + +private: + rclcpp::Logger ros_logger_; + rclcpp::Service::SharedPtr srv_config_logger_; + + void onLoggerConfigService( + const ConfigLogger::Request::SharedPtr request, + const ConfigLogger::Response::SharedPtr response); +}; + +} // namespace tier4_autoware_utils +#endif // TIER4_AUTOWARE_UTILS__ROS__LOGGER_LEVEL_CONFIGURE_HPP_ diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/marker_helper.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/marker_helper.hpp index 78e7c5f9288da..1d72b49a553d1 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/marker_helper.hpp +++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/marker_helper.hpp @@ -15,7 +15,7 @@ #ifndef TIER4_AUTOWARE_UTILS__ROS__MARKER_HELPER_HPP_ #define TIER4_AUTOWARE_UTILS__ROS__MARKER_HELPER_HPP_ -#include +#include #include @@ -64,56 +64,19 @@ inline std_msgs::msg::ColorRGBA createMarkerColor(float r, float g, float b, flo return color; } -inline visualization_msgs::msg::Marker createDefaultMarker( +visualization_msgs::msg::Marker createDefaultMarker( const std::string & frame_id, const rclcpp::Time & now, const std::string & ns, const int32_t id, const int32_t type, const geometry_msgs::msg::Vector3 & scale, - const std_msgs::msg::ColorRGBA & color) -{ - visualization_msgs::msg::Marker marker; - - marker.header.frame_id = frame_id; - marker.header.stamp = now; - marker.ns = ns; - marker.id = id; - marker.type = type; - marker.action = visualization_msgs::msg::Marker::ADD; - marker.lifetime = rclcpp::Duration::from_seconds(0.5); - - marker.pose.position = createMarkerPosition(0.0, 0.0, 0.0); - marker.pose.orientation = createMarkerOrientation(0.0, 0.0, 0.0, 1.0); - marker.scale = scale; - marker.color = color; - marker.frame_locked = true; + const std_msgs::msg::ColorRGBA & color); - return marker; -} - -inline visualization_msgs::msg::Marker createDeletedDefaultMarker( - const rclcpp::Time & now, const std::string & ns, const int32_t id) -{ - visualization_msgs::msg::Marker marker; +visualization_msgs::msg::Marker createDeletedDefaultMarker( + const rclcpp::Time & now, const std::string & ns, const int32_t id); - marker.header.stamp = now; - marker.ns = ns; - marker.id = id; - marker.action = visualization_msgs::msg::Marker::DELETE; - - return marker; -} - -inline void appendMarkerArray( +void appendMarkerArray( const visualization_msgs::msg::MarkerArray & additional_marker_array, visualization_msgs::msg::MarkerArray * marker_array, - const boost::optional & current_time = {}) -{ - for (const auto & marker : additional_marker_array.markers) { - marker_array->markers.push_back(marker); + const boost::optional & current_time = {}); - if (current_time) { - marker_array->markers.back().header.stamp = current_time.get(); - } - } -} } // namespace tier4_autoware_utils #endif // TIER4_AUTOWARE_UTILS__ROS__MARKER_HELPER_HPP_ diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/parameter.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/parameter.hpp new file mode 100644 index 0000000000000..ac9b124a02cdb --- /dev/null +++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/parameter.hpp @@ -0,0 +1,35 @@ +// Copyright 2023 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 TIER4_AUTOWARE_UTILS__ROS__PARAMETER_HPP_ +#define TIER4_AUTOWARE_UTILS__ROS__PARAMETER_HPP_ + +#include + +#include + +namespace tier4_autoware_utils +{ +template +T getOrDeclareParameter(rclcpp::Node & node, const std::string & name) +{ + if (node.has_parameter(name)) { + return node.get_parameter(name).get_value(); + } + + return node.declare_parameter(name); +} +} // namespace tier4_autoware_utils + +#endif // TIER4_AUTOWARE_UTILS__ROS__PARAMETER_HPP_ diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/system/backtrace.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/system/backtrace.hpp new file mode 100644 index 0000000000000..c784d71ad5060 --- /dev/null +++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/system/backtrace.hpp @@ -0,0 +1,25 @@ +// Copyright 2023 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 TIER4_AUTOWARE_UTILS__SYSTEM__BACKTRACE_HPP_ +#define TIER4_AUTOWARE_UTILS__SYSTEM__BACKTRACE_HPP_ + +namespace tier4_autoware_utils +{ + +void print_backtrace(); + +} // namespace tier4_autoware_utils + +#endif // TIER4_AUTOWARE_UTILS__SYSTEM__BACKTRACE_HPP_ diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/tier4_autoware_utils.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/tier4_autoware_utils.hpp deleted file mode 100644 index 450549f75fdec..0000000000000 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/tier4_autoware_utils.hpp +++ /dev/null @@ -1,43 +0,0 @@ -// 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. - -#ifndef TIER4_AUTOWARE_UTILS__TIER4_AUTOWARE_UTILS_HPP_ -#define TIER4_AUTOWARE_UTILS__TIER4_AUTOWARE_UTILS_HPP_ - -#include "tier4_autoware_utils/geometry/boost_geometry.hpp" -#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" -#include "tier4_autoware_utils/geometry/path_with_lane_id_geometry.hpp" -#include "tier4_autoware_utils/geometry/pose_deviation.hpp" -#include "tier4_autoware_utils/math/constants.hpp" -#include "tier4_autoware_utils/math/normalization.hpp" -#include "tier4_autoware_utils/math/range.hpp" -#include "tier4_autoware_utils/math/sin_table.hpp" -#include "tier4_autoware_utils/math/trigonometry.hpp" -#include "tier4_autoware_utils/math/unit_conversion.hpp" -#include "tier4_autoware_utils/ros/debug_publisher.hpp" -#include "tier4_autoware_utils/ros/debug_traits.hpp" -#include "tier4_autoware_utils/ros/marker_helper.hpp" -#include "tier4_autoware_utils/ros/msg_covariance.hpp" -#include "tier4_autoware_utils/ros/msg_operation.hpp" -#include "tier4_autoware_utils/ros/processing_time_publisher.hpp" -#include "tier4_autoware_utils/ros/self_pose_listener.hpp" -#include "tier4_autoware_utils/ros/transform_listener.hpp" -#include "tier4_autoware_utils/ros/update_param.hpp" -#include "tier4_autoware_utils/ros/uuid_helper.hpp" -#include "tier4_autoware_utils/ros/wait_for_param.hpp" -#include "tier4_autoware_utils/system/stop_watch.hpp" -#include "tier4_autoware_utils/transform/transforms.hpp" - -#endif // TIER4_AUTOWARE_UTILS__TIER4_AUTOWARE_UTILS_HPP_ diff --git a/common/tier4_autoware_utils/package.xml b/common/tier4_autoware_utils/package.xml index fd9b315f8e4d5..701907929ecc0 100644 --- a/common/tier4_autoware_utils/package.xml +++ b/common/tier4_autoware_utils/package.xml @@ -7,6 +7,7 @@ Takamasa Horibe Takayuki Murooka Yutaka Shimizu + Mamoru Sobue Apache License 2.0 ament_cmake_auto @@ -19,6 +20,7 @@ diagnostic_msgs geometry_msgs libboost-dev + logging_demo pcl_conversions pcl_ros rclcpp diff --git a/common/tier4_autoware_utils/src/geometry/boost_polygon_utils.cpp b/common/tier4_autoware_utils/src/geometry/boost_polygon_utils.cpp index c212eb269d655..79c9f9937cd4d 100644 --- a/common/tier4_autoware_utils/src/geometry/boost_polygon_utils.cpp +++ b/common/tier4_autoware_utils/src/geometry/boost_polygon_utils.cpp @@ -16,6 +16,8 @@ #include "tier4_autoware_utils/geometry/geometry.hpp" +#include + #include namespace diff --git a/common/tier4_autoware_utils/src/geometry/geometry.cpp b/common/tier4_autoware_utils/src/geometry/geometry.cpp new file mode 100644 index 0000000000000..2c2de2d07a3dc --- /dev/null +++ b/common/tier4_autoware_utils/src/geometry/geometry.cpp @@ -0,0 +1,384 @@ +// Copyright 2023 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 "tier4_autoware_utils/geometry/geometry.hpp" + +#include + +#include + +#include + +namespace tf2 +{ +void fromMsg(const geometry_msgs::msg::PoseStamped & msg, tf2::Stamped & out) +{ + out.stamp_ = tf2_ros::fromMsg(msg.header.stamp); + out.frame_id_ = msg.header.frame_id; + tf2::Transform tmp; + fromMsg(msg.pose, tmp); + out.setData(tmp); +} +} // namespace tf2 + +namespace tier4_autoware_utils +{ +geometry_msgs::msg::Vector3 getRPY(const geometry_msgs::msg::Quaternion & quat) +{ + geometry_msgs::msg::Vector3 rpy; + tf2::Quaternion q(quat.x, quat.y, quat.z, quat.w); + tf2::Matrix3x3(q).getRPY(rpy.x, rpy.y, rpy.z); + return rpy; +} + +geometry_msgs::msg::Vector3 getRPY(const geometry_msgs::msg::Pose & pose) +{ + return getRPY(pose.orientation); +} + +geometry_msgs::msg::Vector3 getRPY(const geometry_msgs::msg::PoseStamped & pose) +{ + return getRPY(pose.pose); +} + +geometry_msgs::msg::Vector3 getRPY(const geometry_msgs::msg::PoseWithCovarianceStamped & pose) +{ + return getRPY(pose.pose.pose); +} + +geometry_msgs::msg::Quaternion createQuaternion( + const double x, const double y, const double z, const double w) +{ + geometry_msgs::msg::Quaternion q; + q.x = x; + q.y = y; + q.z = z; + q.w = w; + return q; +} + +geometry_msgs::msg::Vector3 createTranslation(const double x, const double y, const double z) +{ + geometry_msgs::msg::Vector3 v; + v.x = x; + v.y = y; + v.z = z; + return v; +} + +// Revival of tf::createQuaternionFromRPY +// https://answers.ros.org/question/304397/recommended-way-to-construct-quaternion-from-rollpitchyaw-with-tf2/ +geometry_msgs::msg::Quaternion createQuaternionFromRPY( + const double roll, const double pitch, const double yaw) +{ + tf2::Quaternion q; + q.setRPY(roll, pitch, yaw); + return tf2::toMsg(q); +} + +geometry_msgs::msg::Quaternion createQuaternionFromYaw(const double yaw) +{ + tf2::Quaternion q; + q.setRPY(0, 0, yaw); + return tf2::toMsg(q); +} + +double calcElevationAngle( + const geometry_msgs::msg::Point & p_from, const geometry_msgs::msg::Point & p_to) +{ + const double dz = p_to.z - p_from.z; + const double dist_2d = calcDistance2d(p_from, p_to); + return std::atan2(dz, dist_2d); +} + +/** + * @brief calculate azimuth angle of two points. + * @details This function returns the azimuth angle of the position of the two input points + * with respect to the origin of their coordinate system. + * If x and y of the two points are the same, the calculation result will be unstable. + * @param p_from source point + * @param p_to target point + * @return -pi < azimuth angle < pi. + */ +double calcAzimuthAngle( + const geometry_msgs::msg::Point & p_from, const geometry_msgs::msg::Point & p_to) +{ + const double dx = p_to.x - p_from.x; + const double dy = p_to.y - p_from.y; + return std::atan2(dy, dx); +} + +geometry_msgs::msg::Pose transform2pose(const geometry_msgs::msg::Transform & transform) +{ + geometry_msgs::msg::Pose pose; + pose.position.x = transform.translation.x; + pose.position.y = transform.translation.y; + pose.position.z = transform.translation.z; + pose.orientation = transform.rotation; + return pose; +} + +geometry_msgs::msg::PoseStamped transform2pose( + const geometry_msgs::msg::TransformStamped & transform) +{ + geometry_msgs::msg::PoseStamped pose; + pose.header = transform.header; + pose.pose = transform2pose(transform.transform); + return pose; +} + +geometry_msgs::msg::Transform pose2transform(const geometry_msgs::msg::Pose & pose) +{ + geometry_msgs::msg::Transform transform; + transform.translation.x = pose.position.x; + transform.translation.y = pose.position.y; + transform.translation.z = pose.position.z; + transform.rotation = pose.orientation; + return transform; +} + +geometry_msgs::msg::TransformStamped pose2transform( + const geometry_msgs::msg::PoseStamped & pose, const std::string & child_frame_id) +{ + geometry_msgs::msg::TransformStamped transform; + transform.header = pose.header; + transform.transform = pose2transform(pose.pose); + transform.child_frame_id = child_frame_id; + return transform; +} + +Point3d transformPoint(const Point3d & point, const geometry_msgs::msg::Transform & transform) +{ + const auto & translation = transform.translation; + const auto & rotation = transform.rotation; + + const Eigen::Translation3d T(translation.x, translation.y, translation.z); + const Eigen::Quaterniond R(rotation.w, rotation.x, rotation.y, rotation.z); + + const Eigen::Vector3d transformed(T * R * point); + + return Point3d{transformed.x(), transformed.y(), transformed.z()}; +} + +Point2d transformPoint(const Point2d & point, const geometry_msgs::msg::Transform & transform) +{ + Point3d point_3d{point.x(), point.y(), 0}; + const auto transformed = transformPoint(point_3d, transform); + return Point2d{transformed.x(), transformed.y()}; +} + +Eigen::Vector3d transformPoint(const Eigen::Vector3d & point, const geometry_msgs::msg::Pose & pose) +{ + geometry_msgs::msg::Transform transform; + transform.translation.x = pose.position.x; + transform.translation.y = pose.position.y; + transform.translation.z = pose.position.z; + transform.rotation = pose.orientation; + + Point3d p = transformPoint(Point3d(point.x(), point.y(), point.z()), transform); + return Eigen::Vector3d(p.x(), p.y(), p.z()); +} + +geometry_msgs::msg::Point transformPoint( + const geometry_msgs::msg::Point & point, const geometry_msgs::msg::Pose & pose) +{ + const Eigen::Vector3d vec = Eigen::Vector3d(point.x, point.y, point.z); + auto transformed_vec = transformPoint(vec, pose); + + geometry_msgs::msg::Point transformed_point; + transformed_point.x = transformed_vec.x(); + transformed_point.y = transformed_vec.y(); + transformed_point.z = transformed_vec.z(); + return transformed_point; +} + +geometry_msgs::msg::Point32 transformPoint( + const geometry_msgs::msg::Point32 & point32, const geometry_msgs::msg::Pose & pose) +{ + const auto point = + geometry_msgs::build().x(point32.x).y(point32.y).z(point32.z); + const auto transformed_point = tier4_autoware_utils::transformPoint(point, pose); + return geometry_msgs::build() + .x(transformed_point.x) + .y(transformed_point.y) + .z(transformed_point.z); +} + +geometry_msgs::msg::Pose transformPose( + const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::TransformStamped & transform) +{ + geometry_msgs::msg::Pose transformed_pose; + tf2::doTransform(pose, transformed_pose, transform); + + return transformed_pose; +} + +geometry_msgs::msg::Pose transformPose( + const geometry_msgs::msg::Pose & pose, geometry_msgs::msg::Transform & transform) +{ + geometry_msgs::msg::TransformStamped transform_stamped; + transform_stamped.transform = transform; + + return transformPose(pose, transform_stamped); +} + +geometry_msgs::msg::Pose transformPose( + const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Pose & pose_transform) +{ + tf2::Transform transform; + tf2::convert(pose_transform, transform); + + geometry_msgs::msg::TransformStamped transform_msg; + transform_msg.transform = tf2::toMsg(transform); + + return transformPose(pose, transform_msg); +} + +// Transform pose in world coordinates to local coordinates +/* +geometry_msgs::msg::Pose inverseTransformPose( +const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::TransformStamped & transform) +{ +tf2::Transform tf; +tf2::fromMsg(transform, tf); +geometry_msgs::msg::TransformStamped transform_stamped; +transform_stamped.transform = tf2::toMsg(tf.inverse()); + +return transformPose(pose, transform_stamped); +} +*/ + +// Transform pose in world coordinates to local coordinates +geometry_msgs::msg::Pose inverseTransformPose( + const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Transform & transform) +{ + tf2::Transform tf; + tf2::fromMsg(transform, tf); + geometry_msgs::msg::TransformStamped transform_stamped; + transform_stamped.transform = tf2::toMsg(tf.inverse()); + + return transformPose(pose, transform_stamped); +} + +// Transform pose in world coordinates to local coordinates +geometry_msgs::msg::Pose inverseTransformPose( + const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Pose & transform_pose) +{ + tf2::Transform transform; + tf2::convert(transform_pose, transform); + + return inverseTransformPose(pose, tf2::toMsg(transform)); +} + +// Transform point in world coordinates to local coordinates +Eigen::Vector3d inverseTransformPoint( + const Eigen::Vector3d & point, const geometry_msgs::msg::Pose & pose) +{ + const Eigen::Quaterniond q( + pose.orientation.w, pose.orientation.x, pose.orientation.y, pose.orientation.z); + const Eigen::Matrix3d R = q.normalized().toRotationMatrix(); + + const Eigen::Vector3d local_origin(pose.position.x, pose.position.y, pose.position.z); + Eigen::Vector3d local_point = R.transpose() * point - R.transpose() * local_origin; + + return local_point; +} + +// Transform point in world coordinates to local coordinates +geometry_msgs::msg::Point inverseTransformPoint( + const geometry_msgs::msg::Point & point, const geometry_msgs::msg::Pose & pose) +{ + const Eigen::Vector3d local_vec = + inverseTransformPoint(Eigen::Vector3d(point.x, point.y, point.z), pose); + geometry_msgs::msg::Point local_point; + local_point.x = local_vec.x(); + local_point.y = local_vec.y(); + local_point.z = local_vec.z(); + return local_point; +} + +double calcCurvature( + const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2, + const geometry_msgs::msg::Point & p3) +{ + // Calculation details are described in the following page + // https://en.wikipedia.org/wiki/Menger_curvature + const double denominator = + calcDistance2d(p1, p2) * calcDistance2d(p2, p3) * calcDistance2d(p3, p1); + if (std::fabs(denominator) < 1e-10) { + throw std::runtime_error("points are too close for curvature calculation."); + } + return 2.0 * ((p2.x - p1.x) * (p3.y - p1.y) - (p2.y - p1.y) * (p3.x - p1.x)) / denominator; +} + +/** + * @brief Calculate offset pose. The offset values are defined in the local coordinate of the input + * pose. + */ +geometry_msgs::msg::Pose calcOffsetPose( + const geometry_msgs::msg::Pose & p, const double x, const double y, const double z) +{ + geometry_msgs::msg::Pose pose; + geometry_msgs::msg::Transform transform; + transform.translation = createTranslation(x, y, z); + transform.rotation = createQuaternion(0.0, 0.0, 0.0, 1.0); + tf2::Transform tf_pose; + tf2::Transform tf_offset; + tf2::fromMsg(transform, tf_offset); + tf2::fromMsg(p, tf_pose); + tf2::toMsg(tf_pose * tf_offset, pose); + return pose; +} + +/** + * @brief Judge whether twist covariance is valid. + * + * @param twist_with_covariance source twist with covariance + * @return If all element of covariance is 0, return false. + */ +// +bool isTwistCovarianceValid(const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance) +{ + for (const auto & c : twist_with_covariance.covariance) { + if (c != 0.0) { + return true; + } + } + return false; +} + +// NOTE: much faster than boost::geometry::intersects() +std::optional intersect( + const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2, + const geometry_msgs::msg::Point & p3, const geometry_msgs::msg::Point & p4) +{ + // calculate intersection point + const double det = (p1.x - p2.x) * (p4.y - p3.y) - (p4.x - p3.x) * (p1.y - p2.y); + if (det == 0.0) { + return std::nullopt; + } + + const double t = ((p4.y - p3.y) * (p4.x - p2.x) + (p3.x - p4.x) * (p4.y - p2.y)) / det; + const double s = ((p2.y - p1.y) * (p4.x - p2.x) + (p1.x - p2.x) * (p4.y - p2.y)) / det; + if (t < 0 || 1 < t || s < 0 || 1 < s) { + return std::nullopt; + } + + geometry_msgs::msg::Point intersect_point; + intersect_point.x = t * p1.x + (1.0 - t) * p2.x; + intersect_point.y = t * p1.y + (1.0 - t) * p2.y; + return intersect_point; +} + +} // namespace tier4_autoware_utils diff --git a/common/tier4_autoware_utils/src/geometry/pose_deviation.cpp b/common/tier4_autoware_utils/src/geometry/pose_deviation.cpp new file mode 100644 index 0000000000000..dda8afb974d26 --- /dev/null +++ b/common/tier4_autoware_utils/src/geometry/pose_deviation.cpp @@ -0,0 +1,85 @@ +// Copyright 2023 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 "tier4_autoware_utils/geometry/pose_deviation.hpp" + +#include "tier4_autoware_utils/math/normalization.hpp" + +#include + +#define EIGEN_MPL2_ONLY +#include +#include + +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif + +namespace tier4_autoware_utils +{ + +double calcLateralDeviation( + const geometry_msgs::msg::Pose & base_pose, const geometry_msgs::msg::Point & target_point) +{ + const auto & base_point = base_pose.position; + + const auto yaw = tf2::getYaw(base_pose.orientation); + const Eigen::Vector3d base_unit_vec{std::cos(yaw), std::sin(yaw), 0}; + + const auto dx = target_point.x - base_point.x; + const auto dy = target_point.y - base_point.y; + const Eigen::Vector3d diff_vec{dx, dy, 0}; + + const Eigen::Vector3d cross_vec = base_unit_vec.cross(diff_vec); + + return cross_vec.z(); +} + +double calcLongitudinalDeviation( + const geometry_msgs::msg::Pose & base_pose, const geometry_msgs::msg::Point & target_point) +{ + const auto & base_point = base_pose.position; + + const auto yaw = tf2::getYaw(base_pose.orientation); + const Eigen::Vector3d base_unit_vec{std::cos(yaw), std::sin(yaw), 0}; + + const auto dx = target_point.x - base_point.x; + const auto dy = target_point.y - base_point.y; + const Eigen::Vector3d diff_vec{dx, dy, 0}; + + return base_unit_vec.dot(diff_vec); +} + +double calcYawDeviation( + const geometry_msgs::msg::Pose & base_pose, const geometry_msgs::msg::Pose & target_pose) +{ + const auto base_yaw = tf2::getYaw(base_pose.orientation); + const auto target_yaw = tf2::getYaw(target_pose.orientation); + return normalizeRadian(target_yaw - base_yaw); +} + +PoseDeviation calcPoseDeviation( + const geometry_msgs::msg::Pose & base_pose, const geometry_msgs::msg::Pose & target_pose) +{ + PoseDeviation deviation{}; + + deviation.lateral = calcLateralDeviation(base_pose, target_pose.position); + deviation.longitudinal = calcLongitudinalDeviation(base_pose, target_pose.position); + deviation.yaw = calcYawDeviation(base_pose, target_pose); + + return deviation; +} +} // namespace tier4_autoware_utils diff --git a/common/tier4_autoware_utils/src/ros/logger_level_configure.cpp b/common/tier4_autoware_utils/src/ros/logger_level_configure.cpp new file mode 100644 index 0000000000000..d764299290d05 --- /dev/null +++ b/common/tier4_autoware_utils/src/ros/logger_level_configure.cpp @@ -0,0 +1,61 @@ +// Copyright 2023 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 "tier4_autoware_utils/ros/logger_level_configure.hpp" + +#include + +namespace tier4_autoware_utils +{ +LoggerLevelConfigure::LoggerLevelConfigure(rclcpp::Node * node) : ros_logger_(node->get_logger()) +{ + using std::placeholders::_1; + using std::placeholders::_2; + + srv_config_logger_ = node->create_service( + "~/config_logger", std::bind(&LoggerLevelConfigure::onLoggerConfigService, this, _1, _2)); +} + +void LoggerLevelConfigure::onLoggerConfigService( + const ConfigLogger::Request::SharedPtr request, const ConfigLogger::Response::SharedPtr response) +{ + int logging_severity; + const auto ret_level = rcutils_logging_severity_level_from_string( + request->level.c_str(), rcl_get_default_allocator(), &logging_severity); + + if (ret_level != RCUTILS_RET_OK) { + response->success = false; + RCLCPP_WARN_STREAM( + ros_logger_, "Failed to change logger level for " + << request->logger_name + << " due to an invalid logging severity: " << request->level); + return; + } + + const auto ret_set = + rcutils_logging_set_logger_level(request->logger_name.c_str(), logging_severity); + + if (ret_set != RCUTILS_RET_OK) { + response->success = false; + RCLCPP_WARN_STREAM(ros_logger_, "Failed to set logger level for " << request->logger_name); + return; + } + + response->success = true; + RCLCPP_INFO_STREAM( + ros_logger_, "Logger level [" << request->level << "] is set for " << request->logger_name); + return; +} + +} // namespace tier4_autoware_utils diff --git a/common/tier4_autoware_utils/src/ros/marker_helper.cpp b/common/tier4_autoware_utils/src/ros/marker_helper.cpp new file mode 100644 index 0000000000000..fe3a579e41d19 --- /dev/null +++ b/common/tier4_autoware_utils/src/ros/marker_helper.cpp @@ -0,0 +1,71 @@ +// Copyright 2023 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 "tier4_autoware_utils/ros/marker_helper.hpp" + +namespace tier4_autoware_utils +{ + +visualization_msgs::msg::Marker createDefaultMarker( + const std::string & frame_id, const rclcpp::Time & now, const std::string & ns, const int32_t id, + const int32_t type, const geometry_msgs::msg::Vector3 & scale, + const std_msgs::msg::ColorRGBA & color) +{ + visualization_msgs::msg::Marker marker; + + marker.header.frame_id = frame_id; + marker.header.stamp = now; + marker.ns = ns; + marker.id = id; + marker.type = type; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.lifetime = rclcpp::Duration::from_seconds(0.5); + + marker.pose.position = createMarkerPosition(0.0, 0.0, 0.0); + marker.pose.orientation = createMarkerOrientation(0.0, 0.0, 0.0, 1.0); + marker.scale = scale; + marker.color = color; + marker.frame_locked = true; + + return marker; +} + +visualization_msgs::msg::Marker createDeletedDefaultMarker( + const rclcpp::Time & now, const std::string & ns, const int32_t id) +{ + visualization_msgs::msg::Marker marker; + + marker.header.stamp = now; + marker.ns = ns; + marker.id = id; + marker.action = visualization_msgs::msg::Marker::DELETE; + + return marker; +} + +void appendMarkerArray( + const visualization_msgs::msg::MarkerArray & additional_marker_array, + visualization_msgs::msg::MarkerArray * marker_array, + const boost::optional & current_time) +{ + for (const auto & marker : additional_marker_array.markers) { + marker_array->markers.push_back(marker); + + if (current_time) { + marker_array->markers.back().header.stamp = current_time.get(); + } + } +} + +} // namespace tier4_autoware_utils diff --git a/common/tier4_autoware_utils/src/system/backtrace.cpp b/common/tier4_autoware_utils/src/system/backtrace.cpp new file mode 100644 index 0000000000000..343f200296cad --- /dev/null +++ b/common/tier4_autoware_utils/src/system/backtrace.cpp @@ -0,0 +1,52 @@ +// Copyright 2023 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 "tier4_autoware_utils/system/backtrace.hpp" + +#include "rclcpp/rclcpp.hpp" + +#include + +#include +#include +#include +#include + +namespace tier4_autoware_utils +{ + +void print_backtrace() +{ + constexpr size_t max_frames = 100; + void * addrlist[max_frames + 1]; + + int addrlen = backtrace(addrlist, sizeof(addrlist) / sizeof(void *)); + + if (addrlen == 0) { + return; + } + + char ** symbol_list = backtrace_symbols(addrlist, addrlen); + + std::stringstream ss; + ss << "\n @ ********** back trace **********" << std::endl; + for (int i = 1; i < addrlen; i++) { + ss << " @ " << symbol_list[i] << std::endl; + } + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("tier4_autoware_utils"), ss.str()); + + free(symbol_list); +} + +} // namespace tier4_autoware_utils diff --git a/common/tier4_autoware_utils/test/src/geometry/test_path_with_lane_id_geometry.cpp b/common/tier4_autoware_utils/test/src/geometry/test_path_with_lane_id_geometry.cpp index d6e35ea5a863f..a8179f3b3cd60 100644 --- a/common/tier4_autoware_utils/test/src/geometry/test_path_with_lane_id_geometry.cpp +++ b/common/tier4_autoware_utils/test/src/geometry/test_path_with_lane_id_geometry.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "tier4_autoware_utils/geometry/path_with_lane_id_geometry.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" #include diff --git a/common/tier4_autoware_utils/test/src/geometry/test_pose_deviation.cpp b/common/tier4_autoware_utils/test/src/geometry/test_pose_deviation.cpp index 383c35921c994..76e742edde59c 100644 --- a/common/tier4_autoware_utils/test/src/geometry/test_pose_deviation.cpp +++ b/common/tier4_autoware_utils/test/src/geometry/test_pose_deviation.cpp @@ -12,6 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "tier4_autoware_utils/geometry/geometry.hpp" #include "tier4_autoware_utils/geometry/pose_deviation.hpp" #include "tier4_autoware_utils/math/unit_conversion.hpp" diff --git a/common/tier4_autoware_utils/test/src/test_autoware_utils.cpp b/common/tier4_autoware_utils/test/src/test_autoware_utils.cpp index 34018ed730005..4ecfcb6bee091 100644 --- a/common/tier4_autoware_utils/test/src/test_autoware_utils.cpp +++ b/common/tier4_autoware_utils/test/src/test_autoware_utils.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" +#include #include diff --git a/common/tier4_logging_level_configure_rviz_plugin/CMakeLists.txt b/common/tier4_logging_level_configure_rviz_plugin/CMakeLists.txt new file mode 100644 index 0000000000000..cc7da7e322d19 --- /dev/null +++ b/common/tier4_logging_level_configure_rviz_plugin/CMakeLists.txt @@ -0,0 +1,28 @@ +cmake_minimum_required(VERSION 3.14) +project(tier4_logging_level_configure_rviz_plugin) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +find_package(Qt5 REQUIRED Core Widgets) +set(QT_LIBRARIES Qt5::Widgets) +set(CMAKE_AUTOMOC ON) +add_definitions(-DQT_NO_KEYWORDS) + +ament_auto_add_library(tier4_logging_level_configure_rviz_plugin SHARED + include/tier4_logging_level_configure_rviz_plugin/logging_level_configure.hpp + src/logging_level_configure.cpp +) + +target_link_libraries(tier4_logging_level_configure_rviz_plugin + ${QT_LIBRARIES} +) + +# Export the plugin to be imported by rviz2 +pluginlib_export_plugin_description_file(rviz_common plugins/plugin_description.xml) + +ament_auto_package( + INSTALL_TO_SHARE + plugins + config +) diff --git a/common/tier4_logging_level_configure_rviz_plugin/README.md b/common/tier4_logging_level_configure_rviz_plugin/README.md new file mode 100644 index 0000000000000..c3523ed802205 --- /dev/null +++ b/common/tier4_logging_level_configure_rviz_plugin/README.md @@ -0,0 +1,5 @@ +# tier4_logging_level_configure_rviz_plugin + +This package provides an rviz_plugin that can easily change the logger level of each node + +![tier4_logging_level_configure_rviz_plugin](tier4_logging_level_configure_rviz_plugin.png) diff --git a/common/tier4_logging_level_configure_rviz_plugin/config/logger_config.yaml b/common/tier4_logging_level_configure_rviz_plugin/config/logger_config.yaml new file mode 100644 index 0000000000000..97c3104242026 --- /dev/null +++ b/common/tier4_logging_level_configure_rviz_plugin/config/logger_config.yaml @@ -0,0 +1,63 @@ +# logger_config.yaml + +# ============================================================ +# planning +# ============================================================ + +behavior_path_planner: + - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner + logger_name: planning.scenario_planning.lane_driving.behavior_planning.behavior_path_planner + - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner + logger_name: tier4_autoware_utils + +behavior_path_planner_avoidance: + - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner + logger_name: planning.scenario_planning.lane_driving.behavior_planning.behavior_path_planner.avoidance + +behavior_path_planner_lane_change: + - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner + logger_name: lane_change + +behavior_velocity_planner: + - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner + logger_name: planning.scenario_planning.lane_driving.behavior_planning.behavior_velocity_planner + - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner + logger_name: tier4_autoware_utils + +behavior_velocity_planner_intersection: + - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner + logger_name: planning.scenario_planning.lane_driving.behavior_planning.behavior_velocity_planner.intersection + +motion_obstacle_avoidance: + - node_name: /planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner + logger_name: planning.scenario_planning.lane_driving.motion_planning.obstacle_avoidance_planner + - node_name: /planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner + logger_name: tier4_autoware_utils + +motion_velocity_smoother: + - node_name: /planning/scenario_planning/motion_velocity_smoother + logger_name: planning.scenario_planning.motion_velocity_smoother + - node_name: /planning/scenario_planning/motion_velocity_smoother + logger_name: tier4_autoware_utils + +# ============================================================ +# control +# ============================================================ + +lateral_controller: + - node_name: /control/trajectory_follower/controller_node_exe + logger_name: control.trajectory_follower.controller_node_exe.lateral_controller + - node_name: /control/trajectory_follower/controller_node_exe + logger_name: tier4_autoware_utils + +longitudinal_controller: + - node_name: /control/trajectory_follower/controller_node_exe + logger_name: control.trajectory_follower.controller_node_exe.longitudinal_controller + - node_name: /control/trajectory_follower/controller_node_exe + logger_name: tier4_autoware_utils + +vehicle_cmd_gate: + - node_name: /control/vehicle_cmd_gate + logger_name: control.vehicle_cmd_gate + - node_name: /control/vehicle_cmd_gate + logger_name: tier4_autoware_utils diff --git a/common/tier4_logging_level_configure_rviz_plugin/include/tier4_logging_level_configure_rviz_plugin/logging_level_configure.hpp b/common/tier4_logging_level_configure_rviz_plugin/include/tier4_logging_level_configure_rviz_plugin/logging_level_configure.hpp new file mode 100644 index 0000000000000..4d9b81ffb86bf --- /dev/null +++ b/common/tier4_logging_level_configure_rviz_plugin/include/tier4_logging_level_configure_rviz_plugin/logging_level_configure.hpp @@ -0,0 +1,74 @@ +// Copyright 2023 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 TIER4_LOGGING_LEVEL_CONFIGURE_RVIZ_PLUGIN__LOGGING_LEVEL_CONFIGURE_HPP_ +#define TIER4_LOGGING_LEVEL_CONFIGURE_RVIZ_PLUGIN__LOGGING_LEVEL_CONFIGURE_HPP_ + +#include "logging_demo/srv/config_logger.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace rviz_plugin +{ + +class LoggingLevelConfigureRvizPlugin : public rviz_common::Panel +{ + Q_OBJECT // This macro is needed for Qt to handle slots and signals + + public : LoggingLevelConfigureRvizPlugin(QWidget * parent = nullptr); + void onInitialize() override; + void save(rviz_common::Config config) const override; + void load(const rviz_common::Config & config) override; + +private: + QMap buttonGroups_; + rclcpp::Node::SharedPtr raw_node_; + + // node_logger_map_[button_name] = {node_name, logger_name} + std::map>> node_logger_map_; + + // client_map_[node_name] = service_client + std::unordered_map::SharedPtr> + client_map_; + + // button_map_[button_name][logging_level] = Q_button_pointer + std::unordered_map> button_map_; + + QStringList getNodeList(); + int getMaxModuleNameWidth(QLabel * containerLabel); + void setLoggerNodeMap(); + +private Q_SLOTS: + void onButtonClick(QPushButton * button, const QString & name, const QString & level); + void updateButtonColors( + const QString & target_module_name, QPushButton * active_button, const QString & level); + void changeLogLevel(const QString & container, const QString & level); +}; + +} // namespace rviz_plugin + +#endif // TIER4_LOGGING_LEVEL_CONFIGURE_RVIZ_PLUGIN__LOGGING_LEVEL_CONFIGURE_HPP_ diff --git a/common/tier4_logging_level_configure_rviz_plugin/package.xml b/common/tier4_logging_level_configure_rviz_plugin/package.xml new file mode 100644 index 0000000000000..7849e6049a077 --- /dev/null +++ b/common/tier4_logging_level_configure_rviz_plugin/package.xml @@ -0,0 +1,33 @@ + + + + tier4_logging_level_configure_rviz_plugin + 0.1.0 + The tier4_logging_level_configure_rviz_plugin package + Takamasa Horibe + Satoshi Ota + Kosuke Takeuchi + Apache License 2.0 + + ament_cmake + autoware_cmake + + libqt5-core + libqt5-gui + libqt5-widgets + logging_demo + qtbase5-dev + rclcpp + rviz_common + rviz_default_plugins + rviz_rendering + yaml-cpp + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + + diff --git a/common/tier4_logging_level_configure_rviz_plugin/plugins/plugin_description.xml b/common/tier4_logging_level_configure_rviz_plugin/plugins/plugin_description.xml new file mode 100644 index 0000000000000..ce5cbd13fc131 --- /dev/null +++ b/common/tier4_logging_level_configure_rviz_plugin/plugins/plugin_description.xml @@ -0,0 +1,7 @@ + + + tier4_logging_level_configure_rviz_plugin + + diff --git a/common/tier4_logging_level_configure_rviz_plugin/src/logging_level_configure.cpp b/common/tier4_logging_level_configure_rviz_plugin/src/logging_level_configure.cpp new file mode 100644 index 0000000000000..b15c0f0f735fa --- /dev/null +++ b/common/tier4_logging_level_configure_rviz_plugin/src/logging_level_configure.cpp @@ -0,0 +1,212 @@ +// Copyright 2023 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 "yaml-cpp/yaml.h" + +#include +#include +#include +#include +#include + +#include + +namespace rviz_plugin +{ + +LoggingLevelConfigureRvizPlugin::LoggingLevelConfigureRvizPlugin(QWidget * parent) +: rviz_common::Panel(parent) +{ +} + +void LoggingLevelConfigureRvizPlugin::onInitialize() +{ + raw_node_ = this->getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node(); + + setLoggerNodeMap(); + + QVBoxLayout * layout = new QVBoxLayout; + + QStringList levels = {"DEBUG", "INFO", "WARN", "ERROR", "FATAL"}; + + constexpr int height = 20; + for (const auto & item : node_logger_map_) { + const auto & target_node_name = item.first; + + QHBoxLayout * hLayout = new QHBoxLayout; + + // Create a QLabel to display the node name. + QLabel * label = new QLabel(target_node_name); + label->setFixedHeight(height); // Set fixed height for the button + label->setFixedWidth(getMaxModuleNameWidth(label)); + + hLayout->addWidget(label); // Add the QLabel to the hLayout. + + QButtonGroup * group = new QButtonGroup(this); + for (const QString & level : levels) { + QPushButton * btn = new QPushButton(level); + btn->setFixedHeight(height); // Set fixed height for the button + hLayout->addWidget(btn); // Add each QPushButton to the hLayout. + group->addButton(btn); + button_map_[target_node_name][level] = btn; + connect(btn, &QPushButton::clicked, this, [this, btn, target_node_name, level]() { + this->onButtonClick(btn, target_node_name, level); + }); + } + // Set the "INFO" button as checked by default and change its color. + updateButtonColors(target_node_name, button_map_[target_node_name]["INFO"], "INFO"); + + buttonGroups_[target_node_name] = group; + layout->addLayout(hLayout); + } + + // Create a QWidget to hold the layout. + QWidget * containerWidget = new QWidget; + containerWidget->setLayout(layout); + + // Create a QScrollArea to make the layout scrollable. + QScrollArea * scrollArea = new QScrollArea; + scrollArea->setWidget(containerWidget); + scrollArea->setWidgetResizable(true); + + // Set the QScrollArea as the layout of the main widget. + QVBoxLayout * mainLayout = new QVBoxLayout; + mainLayout->addWidget(scrollArea); + setLayout(mainLayout); + + // set up service clients + const auto & nodes = getNodeList(); + for (const QString & node : nodes) { + const auto client = raw_node_->create_client( + node.toStdString() + "/config_logger"); + client_map_[node] = client; + } +} + +// Calculate the maximum width among all target_module_name. +int LoggingLevelConfigureRvizPlugin::getMaxModuleNameWidth(QLabel * label) +{ + int max_width = 0; + QFontMetrics metrics(label->font()); + for (const auto & item : node_logger_map_) { + const auto & target_module_name = item.first; + max_width = std::max(metrics.horizontalAdvance(target_module_name), max_width); + } + return max_width; +} + +// create node list in node_logger_map_ without +QStringList LoggingLevelConfigureRvizPlugin::getNodeList() +{ + QStringList nodes; + for (const auto & item : node_logger_map_) { + const auto & node_logger_vec = item.second; + for (const auto & node_logger_pair : node_logger_vec) { + if (!nodes.contains(node_logger_pair.first)) { + nodes.append(node_logger_pair.first); + } + } + } + return nodes; +} + +// Modify the signature of the onButtonClick function: +void LoggingLevelConfigureRvizPlugin::onButtonClick( + QPushButton * button, const QString & target_module_name, const QString & level) +{ + if (button) { + const auto callback = + [&](rclcpp::Client::SharedFuture future) { + std::cerr << "change logging level: " + << std::string(future.get()->success ? "success!" : "failed...") << std::endl; + }; + + for (const auto & node_logger_map : node_logger_map_[target_module_name]) { + const auto node_name = node_logger_map.first; + const auto logger_name = node_logger_map.second; + const auto req = std::make_shared(); + + req->logger_name = logger_name.toStdString(); + req->level = level.toStdString(); + std::cerr << "logger level of " << req->logger_name << " is set to " << req->level + << std::endl; + client_map_[node_name]->async_send_request(req, callback); + } + + updateButtonColors( + target_module_name, button, level); // Modify updateButtonColors to accept QPushButton only. + } +} + +void LoggingLevelConfigureRvizPlugin::updateButtonColors( + const QString & target_module_name, QPushButton * active_button, const QString & level) +{ + std::unordered_map colorMap = { + {"DEBUG", "rgb(181, 255, 20)"}, /* green */ + {"INFO", "rgb(200, 255, 255)"}, /* light blue */ + {"WARN", "rgb(255, 255, 0)"}, /* yellow */ + {"ERROR", "rgb(255, 0, 0)"}, /* red */ + {"FATAL", "rgb(139, 0, 0)"}, /* dark red */ + {"OFF", "rgb(211, 211, 211)"} /* gray */ + }; + + const QString LIGHT_GRAY_TEXT = "rgb(180, 180, 180)"; + + const QString color = colorMap.count(level) ? colorMap[level] : colorMap["OFF"]; + + for (const auto & button : button_map_[target_module_name]) { + if (button.second == active_button) { + button.second->setStyleSheet("background-color: " + color + "; color: black"); + } else { + button.second->setStyleSheet( + "background-color: " + colorMap["OFF"] + "; color: " + LIGHT_GRAY_TEXT); + } + } +} +void LoggingLevelConfigureRvizPlugin::save(rviz_common::Config config) const +{ + Panel::save(config); +} + +void LoggingLevelConfigureRvizPlugin::load(const rviz_common::Config & config) +{ + Panel::load(config); +} + +void LoggingLevelConfigureRvizPlugin::setLoggerNodeMap() +{ + const std::string package_share_directory = + ament_index_cpp::get_package_share_directory("tier4_logging_level_configure_rviz_plugin"); + const std::string default_config_path = package_share_directory + "/config/logger_config.yaml"; + + const auto filename = + raw_node_->declare_parameter("config_filename", default_config_path); + RCLCPP_INFO(raw_node_->get_logger(), "load config file: %s", filename.c_str()); + + YAML::Node config = YAML::LoadFile(filename); + + for (YAML::const_iterator it = config.begin(); it != config.end(); ++it) { + const auto key = QString::fromStdString(it->first.as()); + const YAML::Node values = it->second; + for (size_t i = 0; i < values.size(); i++) { + const auto node_name = QString::fromStdString(values[i]["node_name"].as()); + const auto logger_name = QString::fromStdString(values[i]["logger_name"].as()); + node_logger_map_[key].push_back({node_name, logger_name}); + } + } +} + +} // namespace rviz_plugin +#include +PLUGINLIB_EXPORT_CLASS(rviz_plugin::LoggingLevelConfigureRvizPlugin, rviz_common::Panel) diff --git a/common/tier4_logging_level_configure_rviz_plugin/tier4_logging_level_configure_rviz_plugin.png b/common/tier4_logging_level_configure_rviz_plugin/tier4_logging_level_configure_rviz_plugin.png new file mode 100644 index 0000000000000..a93aff724bb19 Binary files /dev/null and b/common/tier4_logging_level_configure_rviz_plugin/tier4_logging_level_configure_rviz_plugin.png differ diff --git a/common/tier4_planning_rviz_plugin/include/path/display_base.hpp b/common/tier4_planning_rviz_plugin/include/path/display_base.hpp index 55c7a9c53d72d..74c2a60df3f32 100644 --- a/common/tier4_planning_rviz_plugin/include/path/display_base.hpp +++ b/common/tier4_planning_rviz_plugin/include/path/display_base.hpp @@ -133,7 +133,10 @@ class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay property_point_alpha_{"Alpha", 1.0, "", &property_point_view_}, property_point_color_{"Color", QColor(0, 60, 255), "", &property_point_view_}, property_point_radius_{"Radius", 0.1, "", &property_point_view_}, - property_point_offset_{"Offset", 0.0, "", &property_point_view_} + property_point_offset_{"Offset", 0.0, "", &property_point_view_}, + // slope + property_slope_text_view_{"View Text Slope", false, "", this}, + property_slope_text_scale_{"Scale", 0.3, "", &property_slope_text_view_} { // path property_path_width_.setMin(0.0); @@ -171,6 +174,12 @@ class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay node->detachAllObjects(); this->scene_manager_->destroySceneNode(node); } + for (size_t i = 0; i < slope_text_nodes_.size(); i++) { + Ogre::SceneNode * node = slope_text_nodes_.at(i); + node->removeAndDestroyAllChildren(); + node->detachAllObjects(); + this->scene_manager_->destroySceneNode(node); + } this->scene_manager_->destroyManualObject(footprint_manual_object_); this->scene_manager_->destroyManualObject(point_manual_object_); } @@ -198,6 +207,7 @@ class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay rviz_common::MessageFilterDisplay::MFDClass::reset(); path_manual_object_->clear(); velocity_manual_object_->clear(); + for (size_t i = 0; i < velocity_texts_.size(); i++) { Ogre::SceneNode * node = velocity_text_nodes_.at(i); node->detachAllObjects(); @@ -206,6 +216,16 @@ class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay } velocity_text_nodes_.clear(); velocity_texts_.clear(); + + for (size_t i = 0; i < slope_texts_.size(); i++) { + Ogre::SceneNode * node = slope_text_nodes_.at(i); + node->detachAllObjects(); + node->removeAndDestroyAllChildren(); + this->scene_manager_->destroySceneNode(node); + } + slope_text_nodes_.clear(); + slope_texts_.clear(); + footprint_manual_object_->clear(); point_manual_object_->clear(); } @@ -300,6 +320,29 @@ class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay velocity_text_nodes_.resize(msg_ptr->points.size()); } + if (msg_ptr->points.size() > slope_texts_.size()) { + for (size_t i = slope_texts_.size(); i < msg_ptr->points.size(); i++) { + Ogre::SceneNode * node = this->scene_node_->createChildSceneNode(); + rviz_rendering::MovableText * text = + new rviz_rendering::MovableText("not initialized", "Liberation Sans", 0.1); + text->setVisible(false); + text->setTextAlignment( + rviz_rendering::MovableText::H_CENTER, rviz_rendering::MovableText::V_ABOVE); + node->attachObject(text); + slope_texts_.push_back(text); + slope_text_nodes_.push_back(node); + } + } else if (msg_ptr->points.size() < slope_texts_.size()) { + for (size_t i = slope_texts_.size() - 1; i >= msg_ptr->points.size(); i--) { + Ogre::SceneNode * node = slope_text_nodes_.at(i); + node->detachAllObjects(); + node->removeAndDestroyAllChildren(); + this->scene_manager_->destroySceneNode(node); + } + slope_texts_.resize(msg_ptr->points.size()); + slope_text_nodes_.resize(msg_ptr->points.size()); + } + const auto info = vehicle_footprint_info_; const float left = property_path_width_view_.getBool() ? -property_path_width_.getFloat() / 2.0 : -info->width / 2.0; @@ -395,6 +438,38 @@ class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay rviz_rendering::MovableText * text = velocity_texts_.at(point_idx); text->setVisible(false); } + + // slope text + if (property_slope_text_view_.getBool() && 1 < msg_ptr->points.size()) { + const size_t prev_idx = + (point_idx != msg_ptr->points.size() - 1) ? point_idx : point_idx - 1; + const size_t next_idx = + (point_idx != msg_ptr->points.size() - 1) ? point_idx + 1 : point_idx; + + const auto & prev_path_pos = + tier4_autoware_utils::getPose(msg_ptr->points.at(prev_idx)).position; + const auto & next_path_pos = + tier4_autoware_utils::getPose(msg_ptr->points.at(next_idx)).position; + + Ogre::Vector3 position; + position.x = pose.position.x; + position.y = pose.position.y; + position.z = pose.position.z; + Ogre::SceneNode * node = slope_text_nodes_.at(point_idx); + node->setPosition(position); + + rviz_rendering::MovableText * text = slope_texts_.at(point_idx); + const double slope = tier4_autoware_utils::calcElevationAngle(prev_path_pos, next_path_pos); + + std::stringstream ss; + ss << std::fixed << std::setprecision(2) << slope; + text->setCaption(ss.str()); + text->setCharacterHeight(property_slope_text_scale_.getFloat()); + text->setVisible(true); + } else { + rviz_rendering::MovableText * text = slope_texts_.at(point_idx); + text->setVisible(false); + } } path_manual_object_->end(); @@ -506,6 +581,10 @@ class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay vehicle_footprint_info_ = std::make_shared( vehicle_info_->vehicle_length_m, vehicle_info_->vehicle_width_m, vehicle_info_->rear_overhang_m); + + property_vehicle_length_.setValue(vehicle_info_->vehicle_length_m); + property_vehicle_width_.setValue(vehicle_info_->vehicle_width_m); + property_rear_overhang_.setValue(vehicle_info_->rear_overhang_m); } else { const float length{property_vehicle_length_.getFloat()}; const float width{property_vehicle_width_.getFloat()}; @@ -522,6 +601,8 @@ class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay Ogre::ManualObject * point_manual_object_{nullptr}; std::vector velocity_texts_; std::vector velocity_text_nodes_; + std::vector slope_texts_; + std::vector slope_text_nodes_; rviz_common::properties::BoolProperty property_path_view_; rviz_common::properties::BoolProperty property_path_width_view_; @@ -552,6 +633,9 @@ class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay rviz_common::properties::FloatProperty property_point_radius_; rviz_common::properties::FloatProperty property_point_offset_; + rviz_common::properties::BoolProperty property_slope_text_view_; + rviz_common::properties::FloatProperty property_slope_text_scale_; + std::shared_ptr vehicle_info_; private: diff --git a/common/tier4_planning_rviz_plugin/include/utils.hpp b/common/tier4_planning_rviz_plugin/include/utils.hpp index 8a16059cbf275..76f9da0685908 100644 --- a/common/tier4_planning_rviz_plugin/include/utils.hpp +++ b/common/tier4_planning_rviz_plugin/include/utils.hpp @@ -16,7 +16,6 @@ #define UTILS_HPP_ #include -#include #include #include diff --git a/common/tier4_system_rviz_plugin/CMakeLists.txt b/common/tier4_system_rviz_plugin/CMakeLists.txt new file mode 100644 index 0000000000000..a65379c553131 --- /dev/null +++ b/common/tier4_system_rviz_plugin/CMakeLists.txt @@ -0,0 +1,36 @@ +cmake_minimum_required(VERSION 3.14) +project(tier4_system_rviz_plugin) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +find_package(Qt5 REQUIRED Core Widgets) +set(QT_LIBRARIES Qt5::Widgets) + +set(CMAKE_AUTOMOC ON) +set(CMAKE_INCLUDE_CURRENT_DIR ON) +add_definitions(-DQT_NO_KEYWORDS) + +set(HEADERS + src/mrm_summary_overlay_display.hpp + src/jsk_overlay_utils.hpp +) + +## Declare a C++ library +ament_auto_add_library(tier4_system_rviz_plugin SHARED + src/mrm_summary_overlay_display.cpp + src/jsk_overlay_utils.cpp + ${HEADERS} +) + +target_link_libraries(tier4_system_rviz_plugin + ${QT_LIBRARIES} +) + +# Export the plugin to be imported by rviz2 +pluginlib_export_plugin_description_file(rviz_common plugins/plugin_description.xml) + +ament_auto_package( + INSTALL_TO_SHARE + plugins +) diff --git a/common/tier4_system_rviz_plugin/README.md b/common/tier4_system_rviz_plugin/README.md new file mode 100644 index 0000000000000..61260ecfda723 --- /dev/null +++ b/common/tier4_system_rviz_plugin/README.md @@ -0,0 +1,11 @@ +# tier4_system_rviz_plugin + +## Purpose + +This plugin display the Hazard information from Autoware; and output notices when emergencies are from initial localization and route setting. + +## Input + +| Name | Type | Description | +| --------------------------------- | ----------------------------------------------------- | ------------------------------------------------------------ | +| `/system/emergency/hazard_status` | `autoware_auto_system_msgs::msg::HazardStatusStamped` | The topic represents the emergency information from Autoware | diff --git a/common/tier4_system_rviz_plugin/package.xml b/common/tier4_system_rviz_plugin/package.xml new file mode 100644 index 0000000000000..adae997ea07aa --- /dev/null +++ b/common/tier4_system_rviz_plugin/package.xml @@ -0,0 +1,30 @@ + + + + tier4_system_rviz_plugin + 0.1.0 + The tier4_vehicle_rviz_plugin package + Koji Minoda + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + + autoware_auto_system_msgs + diagnostic_msgs + libqt5-core + libqt5-gui + libqt5-widgets + qtbase5-dev + rviz_common + rviz_default_plugins + rviz_ogre_vendor + tier4_autoware_utils + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/common/tier4_system_rviz_plugin/plugins/plugin_description.xml b/common/tier4_system_rviz_plugin/plugins/plugin_description.xml new file mode 100644 index 0000000000000..a3ac26d1a304d --- /dev/null +++ b/common/tier4_system_rviz_plugin/plugins/plugin_description.xml @@ -0,0 +1,6 @@ + + + + diff --git a/common/tier4_system_rviz_plugin/src/jsk_overlay_utils.cpp b/common/tier4_system_rviz_plugin/src/jsk_overlay_utils.cpp new file mode 100644 index 0000000000000..36c5d1ce82693 --- /dev/null +++ b/common/tier4_system_rviz_plugin/src/jsk_overlay_utils.cpp @@ -0,0 +1,222 @@ +// 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. + +// Copyright (c) 2014, JSK Lab +// All rights reserved. +// +// Software License Agreement (BSD License) +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions +// are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above +// copyright notice, this list of conditions and the following +// disclaimer in the documentation and/or other materials provided +// with the distribution. +// * Neither the name of {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived +// from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE.S SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#include "jsk_overlay_utils.hpp" + +#include + +namespace jsk_rviz_plugins +{ +ScopedPixelBuffer::ScopedPixelBuffer(Ogre::HardwarePixelBufferSharedPtr pixel_buffer) +: pixel_buffer_(pixel_buffer) +{ + pixel_buffer_->lock(Ogre::HardwareBuffer::HBL_NORMAL); +} + +ScopedPixelBuffer::~ScopedPixelBuffer() +{ + pixel_buffer_->unlock(); +} + +Ogre::HardwarePixelBufferSharedPtr ScopedPixelBuffer::getPixelBuffer() +{ + return pixel_buffer_; +} + +QImage ScopedPixelBuffer::getQImage(unsigned int width, unsigned int height) +{ + const Ogre::PixelBox & pixelBox = pixel_buffer_->getCurrentLock(); + Ogre::uint8 * pDest = static_cast(pixelBox.data); + memset(pDest, 0, width * height); + return QImage(pDest, width, height, QImage::Format_ARGB32); +} + +QImage ScopedPixelBuffer::getQImage(unsigned int width, unsigned int height, QColor & bg_color) +{ + QImage Hud = getQImage(width, height); + for (unsigned int i = 0; i < width; i++) { + for (unsigned int j = 0; j < height; j++) { + Hud.setPixel(i, j, bg_color.rgba()); + } + } + return Hud; +} + +QImage ScopedPixelBuffer::getQImage(OverlayObject & overlay) +{ + return getQImage(overlay.getTextureWidth(), overlay.getTextureHeight()); +} + +QImage ScopedPixelBuffer::getQImage(OverlayObject & overlay, QColor & bg_color) +{ + return getQImage(overlay.getTextureWidth(), overlay.getTextureHeight(), bg_color); +} + +OverlayObject::OverlayObject(const std::string & name) : name_(name) +{ + std::string material_name = name_ + "Material"; + Ogre::OverlayManager * mOverlayMgr = Ogre::OverlayManager::getSingletonPtr(); + overlay_ = mOverlayMgr->create(name_); + panel_ = static_cast( + mOverlayMgr->createOverlayElement("Panel", name_ + "Panel")); + panel_->setMetricsMode(Ogre::GMM_PIXELS); + + panel_material_ = Ogre::MaterialManager::getSingleton().create( + material_name, Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME); + panel_->setMaterialName(panel_material_->getName()); + overlay_->add2D(panel_); +} + +OverlayObject::~OverlayObject() +{ + hide(); + panel_material_->unload(); + Ogre::MaterialManager::getSingleton().remove(panel_material_->getName()); + // Ogre::OverlayManager* mOverlayMgr = Ogre::OverlayManager::getSingletonPtr(); + // mOverlayMgr->destroyOverlayElement(panel_); + // delete panel_; + // delete overlay_; +} + +std::string OverlayObject::getName() +{ + return name_; +} + +void OverlayObject::hide() +{ + if (overlay_->isVisible()) { + overlay_->hide(); + } +} + +void OverlayObject::show() +{ + if (!overlay_->isVisible()) { + overlay_->show(); + } +} + +bool OverlayObject::isTextureReady() +{ + return static_cast(texture_); +} + +void OverlayObject::updateTextureSize(unsigned int width, unsigned int height) +{ + const std::string texture_name = name_ + "Texture"; + if (width == 0) { + RCLCPP_WARN(rclcpp::get_logger("OverlayObject"), "width=0 is specified as texture size"); + width = 1; + } + if (height == 0) { + RCLCPP_WARN(rclcpp::get_logger("OverlayObject"), "height=0 is specified as texture size"); + height = 1; + } + if (!isTextureReady() || ((width != texture_->getWidth()) || (height != texture_->getHeight()))) { + if (isTextureReady()) { + Ogre::TextureManager::getSingleton().remove(texture_name); + panel_material_->getTechnique(0)->getPass(0)->removeAllTextureUnitStates(); + } + texture_ = Ogre::TextureManager::getSingleton().createManual( + texture_name, // name + Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, + Ogre::TEX_TYPE_2D, // type + width, height, // width & height of the render window + 0, // number of mipmaps + Ogre::PF_A8R8G8B8, // pixel format chosen to match a format Qt can use + Ogre::TU_DEFAULT // usage + ); + panel_material_->getTechnique(0)->getPass(0)->createTextureUnitState(texture_name); + + panel_material_->getTechnique(0)->getPass(0)->setSceneBlending(Ogre::SBT_TRANSPARENT_ALPHA); + } +} + +ScopedPixelBuffer OverlayObject::getBuffer() +{ + if (isTextureReady()) { + return ScopedPixelBuffer(texture_->getBuffer()); + } else { + return ScopedPixelBuffer(Ogre::HardwarePixelBufferSharedPtr()); + } +} + +void OverlayObject::setPosition(double left, double top) +{ + panel_->setPosition(left, top); +} + +void OverlayObject::setDimensions(double width, double height) +{ + panel_->setDimensions(width, height); +} + +bool OverlayObject::isVisible() +{ + return overlay_->isVisible(); +} + +unsigned int OverlayObject::getTextureWidth() +{ + if (isTextureReady()) { + return texture_->getWidth(); + } else { + return 0; + } +} + +unsigned int OverlayObject::getTextureHeight() +{ + if (isTextureReady()) { + return texture_->getHeight(); + } else { + return 0; + } +} + +} // namespace jsk_rviz_plugins diff --git a/common/tier4_system_rviz_plugin/src/jsk_overlay_utils.hpp b/common/tier4_system_rviz_plugin/src/jsk_overlay_utils.hpp new file mode 100644 index 0000000000000..568111f241a28 --- /dev/null +++ b/common/tier4_system_rviz_plugin/src/jsk_overlay_utils.hpp @@ -0,0 +1,138 @@ +// 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. + +// Copyright (c) 2014, JSK Lab +// All rights reserved. +// +// Software License Agreement (BSD License) +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions +// are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above +// copyright notice, this list of conditions and the following +// disclaimer in the documentation and/or other materials provided +// with the distribution. +// * Neither the name of {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived +// from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE.S SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#ifndef JSK_OVERLAY_UTILS_HPP_ +#define JSK_OVERLAY_UTILS_HPP_ + +#include +#include +#include +#include +#include + +#include +#include +// see OGRE/OgrePrerequisites.h +// #define OGRE_VERSION +// ((OGRE_VERSION_MAJOR << 16) | (OGRE_VERSION_MINOR << 8) | OGRE_VERSION_PATCH) +#if OGRE_VERSION < ((1 << 16) | (9 << 8) | 0) +#include +#include +#include +#include +#else +#include +#include +#include +#include +#include +#endif + +#include +#include +#include + +namespace jsk_rviz_plugins +{ +class OverlayObject; + +class ScopedPixelBuffer +{ +public: + explicit ScopedPixelBuffer(Ogre::HardwarePixelBufferSharedPtr pixel_buffer); + virtual ~ScopedPixelBuffer(); + virtual Ogre::HardwarePixelBufferSharedPtr getPixelBuffer(); + virtual QImage getQImage(unsigned int width, unsigned int height); + virtual QImage getQImage(OverlayObject & overlay); + virtual QImage getQImage(unsigned int width, unsigned int height, QColor & bg_color); + virtual QImage getQImage(OverlayObject & overlay, QColor & bg_color); + +protected: + Ogre::HardwarePixelBufferSharedPtr pixel_buffer_; + +private: +}; + +// this is a class for put overlay object on rviz 3D panel. +// This class suppose to be instantiated in onInitialize method +// of rviz::Display class. +class OverlayObject +{ +public: + typedef std::shared_ptr Ptr; + + explicit OverlayObject(const std::string & name); + virtual ~OverlayObject(); + + virtual std::string getName(); + virtual void hide(); + virtual void show(); + virtual bool isTextureReady(); + virtual void updateTextureSize(unsigned int width, unsigned int height); + virtual ScopedPixelBuffer getBuffer(); + virtual void setPosition(double left, double top); + virtual void setDimensions(double width, double height); + virtual bool isVisible(); + virtual unsigned int getTextureWidth(); + virtual unsigned int getTextureHeight(); + +protected: + const std::string name_; + Ogre::Overlay * overlay_; + Ogre::PanelOverlayElement * panel_; + Ogre::MaterialPtr panel_material_; + Ogre::TexturePtr texture_; + +private: +}; + +// Ogre::Overlay* createOverlay(std::string name); +// Ogre::PanelOverlayElement* createOverlayPanel(Ogre::Overlay* overlay); +// Ogre::MaterialPtr createOverlayMaterial(Ogre::Overlay* overlay); +} // namespace jsk_rviz_plugins + +#endif // JSK_OVERLAY_UTILS_HPP_ diff --git a/common/tier4_system_rviz_plugin/src/mrm_summary_overlay_display.cpp b/common/tier4_system_rviz_plugin/src/mrm_summary_overlay_display.cpp new file mode 100644 index 0000000000000..61c2bd378dab1 --- /dev/null +++ b/common/tier4_system_rviz_plugin/src/mrm_summary_overlay_display.cpp @@ -0,0 +1,271 @@ +// Copyright 2023 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. + +// Copyright (c) 2014, JSK Lab +// All rights reserved. +// +// Software License Agreement (BSD License) +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions +// are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above +// copyright notice, this list of conditions and the following +// disclaimer in the documentation and/or other materials provided +// with the distribution. +// * Neither the name of {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived +// from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE.S SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#include "mrm_summary_overlay_display.hpp" + +#include +#include + +#include +#include + +#include + +#include +#include +#include +#include + +namespace rviz_plugins +{ +void insertNewlines(std::string & str, const size_t max_line_length) +{ + size_t index = max_line_length; + + while (index < str.size()) { + str.insert(index, "\n"); + index = index + max_line_length + 1; + } +} + +std::optional generateMrmMessage(diagnostic_msgs::msg::DiagnosticStatus diag_status) +{ + if (diag_status.hardware_id == "" || diag_status.hardware_id == "system_error_monitor") { + return std::nullopt; + } else if (diag_status.level == diagnostic_msgs::msg::DiagnosticStatus::ERROR) { + std::string msg = "- ERROR: " + diag_status.name + " (" + diag_status.message + ")"; + insertNewlines(msg, 100); + return msg; + } else if (diag_status.level == diagnostic_msgs::msg::DiagnosticStatus::WARN) { + std::string msg = "- WARN: " + diag_status.name + " (" + diag_status.message + ")"; + insertNewlines(msg, 100); + return msg; + } + return std::nullopt; +} + +MrmSummaryOverlayDisplay::MrmSummaryOverlayDisplay() +{ + const Screen * screen_info = DefaultScreenOfDisplay(XOpenDisplay(NULL)); + + constexpr float hight_4k = 2160.0; + const float scale = static_cast(screen_info->height) / hight_4k; + const auto left = static_cast(std::round(1024 * scale)); + const auto top = static_cast(std::round(128 * scale)); + + property_text_color_ = new rviz_common::properties::ColorProperty( + "Text Color", QColor(25, 255, 240), "text color", this, SLOT(updateVisualization()), this); + property_left_ = new rviz_common::properties::IntProperty( + "Left", left, "Left of the plotter window", this, SLOT(updateVisualization()), this); + property_left_->setMin(0); + property_top_ = new rviz_common::properties::IntProperty( + "Top", top, "Top of the plotter window", this, SLOT(updateVisualization())); + property_top_->setMin(0); + + property_value_height_offset_ = new rviz_common::properties::IntProperty( + "Value height offset", 0, "Height offset of the plotter window", this, + SLOT(updateVisualization())); + property_font_size_ = new rviz_common::properties::IntProperty( + "Font Size", 10, "Font Size", this, SLOT(updateVisualization()), this); + property_font_size_->setMin(1); + property_max_letter_num_ = new rviz_common::properties::IntProperty( + "Max Letter Num", 100, "Max Letter Num", this, SLOT(updateVisualization()), this); + property_max_letter_num_->setMin(10); +} + +MrmSummaryOverlayDisplay::~MrmSummaryOverlayDisplay() +{ + if (initialized()) { + overlay_->hide(); + } +} + +void MrmSummaryOverlayDisplay::onInitialize() +{ + RTDClass::onInitialize(); + + static int count = 0; + rviz_common::UniformStringStream ss; + ss << "MrmSummaryOverlayDisplayObject" << count++; + overlay_.reset(new jsk_rviz_plugins::OverlayObject(ss.str())); + + overlay_->show(); + + const int texture_size = property_font_size_->getInt() * property_max_letter_num_->getInt(); + overlay_->updateTextureSize(texture_size, texture_size); + overlay_->setPosition(property_left_->getInt(), property_top_->getInt()); + overlay_->setDimensions(overlay_->getTextureWidth(), overlay_->getTextureHeight()); +} + +void MrmSummaryOverlayDisplay::onEnable() +{ + subscribe(); + overlay_->show(); +} + +void MrmSummaryOverlayDisplay::onDisable() +{ + unsubscribe(); + reset(); + overlay_->hide(); +} + +void MrmSummaryOverlayDisplay::update(float wall_dt, float ros_dt) +{ + (void)wall_dt; + (void)ros_dt; + + // MRM summary + std::vector mrm_comfortable_stop_summary_list = {}; + std::vector mrm_emergency_stop_summary_list = {}; + int hazard_level = autoware_auto_system_msgs::msg::HazardStatus::NO_FAULT; + { + std::lock_guard message_lock(mutex_); + if (last_msg_ptr_) { + hazard_level = last_msg_ptr_->status.level; + for (const auto & diag_status : last_msg_ptr_->status.diag_latent_fault) { + const std::optional msg = generateMrmMessage(diag_status); + if (msg != std::nullopt) { + mrm_comfortable_stop_summary_list.push_back(msg.value()); + } + } + for (const auto & diag_status : last_msg_ptr_->status.diag_single_point_fault) { + const std::optional msg = generateMrmMessage(diag_status); + if (msg != std::nullopt) { + mrm_emergency_stop_summary_list.push_back(msg.value()); + } + } + } + } + + // Display + QColor background_color; + background_color.setAlpha(0); + jsk_rviz_plugins::ScopedPixelBuffer buffer = overlay_->getBuffer(); + QImage hud = buffer.getQImage(*overlay_); + hud.fill(background_color); + + QPainter painter(&hud); + painter.setRenderHint(QPainter::Antialiasing, true); + + const int w = overlay_->getTextureWidth() - line_width_; + const int h = overlay_->getTextureHeight() - line_width_; + + // text + QColor text_color(property_text_color_->getColor()); + text_color.setAlpha(255); + painter.setPen(QPen(text_color, static_cast(2), Qt::SolidLine)); + QFont font = painter.font(); + font.setPixelSize(property_font_size_->getInt()); + font.setBold(true); + painter.setFont(font); + + std::ostringstream output_text; + + // Display the MRM Summary only when there is a fault + if (hazard_level != autoware_auto_system_msgs::msg::HazardStatus::NO_FAULT) { + // Broadcasting the Basic Error Infos + int number_of_comfortable_stop_messages = + static_cast(mrm_comfortable_stop_summary_list.size()); + if (number_of_comfortable_stop_messages > 0) // Only Display when there are errors + { + output_text << std::fixed + << "Comfortable Stop MRM Summary: " << number_of_comfortable_stop_messages + << std::endl; + for (const auto & mrm_element : mrm_comfortable_stop_summary_list) { + output_text << mrm_element << std::endl; + } + } + + int number_of_emergency_stop_messages = + static_cast(mrm_emergency_stop_summary_list.size()); + if (number_of_emergency_stop_messages > 0) // Only Display when there are some errors + { + output_text << "Emergency Stop MRM Summary: " << int(mrm_emergency_stop_summary_list.size()) + << std::endl; + for (const auto & mrm_element : mrm_emergency_stop_summary_list) { + output_text << mrm_element << std::endl; + } + } + } + + // same as above, but align on right side + painter.drawText( + 0, std::min(property_value_height_offset_->getInt(), h - 1), w, + std::max(h - property_value_height_offset_->getInt(), 1), Qt::AlignLeft | Qt::AlignTop, + output_text.str().c_str()); + painter.end(); + updateVisualization(); +} + +void MrmSummaryOverlayDisplay::processMessage( + const autoware_auto_system_msgs::msg::HazardStatusStamped::ConstSharedPtr msg_ptr) +{ + if (!isEnabled()) { + return; + } + + { + std::lock_guard message_lock(mutex_); + last_msg_ptr_ = msg_ptr; + } + + queueRender(); +} + +void MrmSummaryOverlayDisplay::updateVisualization() +{ + const int texture_size = property_font_size_->getInt() * property_max_letter_num_->getInt(); + overlay_->updateTextureSize(texture_size, texture_size); + overlay_->setPosition(property_left_->getInt(), property_top_->getInt()); + overlay_->setDimensions(overlay_->getTextureWidth(), overlay_->getTextureHeight()); +} + +} // namespace rviz_plugins + +#include +PLUGINLIB_EXPORT_CLASS(rviz_plugins::MrmSummaryOverlayDisplay, rviz_common::Display) diff --git a/common/tier4_system_rviz_plugin/src/mrm_summary_overlay_display.hpp b/common/tier4_system_rviz_plugin/src/mrm_summary_overlay_display.hpp new file mode 100644 index 0000000000000..d0b0e32c3788f --- /dev/null +++ b/common/tier4_system_rviz_plugin/src/mrm_summary_overlay_display.hpp @@ -0,0 +1,108 @@ +// Copyright 2023 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. + +// Copyright (c) 2014, JSK Lab +// All rights reserved. +// +// Software License Agreement (BSD License) +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions +// are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above +// copyright notice, this list of conditions and the following +// disclaimer in the documentation and/or other materials provided +// with the distribution. +// * Neither the name of {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived +// from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE.S SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#ifndef MRM_SUMMARY_OVERLAY_DISPLAY_HPP_ +#define MRM_SUMMARY_OVERLAY_DISPLAY_HPP_ + +#include +#include + +#ifndef Q_MOC_RUN +#include "jsk_overlay_utils.hpp" + +#include +#include +#include +#include + +#include + +#endif + +namespace rviz_plugins +{ +class MrmSummaryOverlayDisplay +: public rviz_common::RosTopicDisplay + +{ + Q_OBJECT + +public: + MrmSummaryOverlayDisplay(); + ~MrmSummaryOverlayDisplay() override; + + void onInitialize() override; + void onEnable() override; + void onDisable() override; + +private Q_SLOTS: + void updateVisualization(); + +protected: + void update(float wall_dt, float ros_dt) override; + void processMessage( + const autoware_auto_system_msgs::msg::HazardStatusStamped::ConstSharedPtr msg_ptr) override; + jsk_rviz_plugins::OverlayObject::Ptr overlay_; + rviz_common::properties::ColorProperty * property_text_color_; + rviz_common::properties::IntProperty * property_left_; + rviz_common::properties::IntProperty * property_top_; + rviz_common::properties::IntProperty * property_value_height_offset_; + rviz_common::properties::FloatProperty * property_value_scale_; + rviz_common::properties::IntProperty * property_font_size_; + rviz_common::properties::IntProperty * property_max_letter_num_; + // QImage hud_; + +private: + static constexpr int line_width_ = 2; + static constexpr int hand_width_ = 4; + + std::mutex mutex_; + autoware_auto_system_msgs::msg::HazardStatusStamped::ConstSharedPtr last_msg_ptr_; +}; +} // namespace rviz_plugins + +#endif // MRM_SUMMARY_OVERLAY_DISPLAY_HPP_ diff --git a/common/tier4_target_object_type_rviz_plugin/CMakeLists.txt b/common/tier4_target_object_type_rviz_plugin/CMakeLists.txt new file mode 100644 index 0000000000000..ed458cf924e33 --- /dev/null +++ b/common/tier4_target_object_type_rviz_plugin/CMakeLists.txt @@ -0,0 +1,25 @@ +cmake_minimum_required(VERSION 3.14) +project(tier4_target_object_type_rviz_plugin) + +find_package(autoware_cmake REQUIRED) +autoware_package() +find_package(OpenCV REQUIRED) +find_package(Qt5 REQUIRED Core Widgets) +set(QT_LIBRARIES Qt5::Widgets) +set(CMAKE_AUTOMOC ON) +set(CMAKE_INCLUDE_CURRENT_DIR ON) + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/target_object_type_panel.hpp + src/target_object_type_panel.cpp +) +target_link_libraries(${PROJECT_NAME} + ${QT_LIBRARIES} + ${OpenCV_LIBRARIES} +) +pluginlib_export_plugin_description_file(rviz_common plugins/plugin_description.xml) + +ament_auto_package( + INSTALL_TO_SHARE + plugins +) diff --git a/common/tier4_target_object_type_rviz_plugin/README.md b/common/tier4_target_object_type_rviz_plugin/README.md new file mode 100644 index 0000000000000..1296bd3442ab3 --- /dev/null +++ b/common/tier4_target_object_type_rviz_plugin/README.md @@ -0,0 +1,9 @@ +# tier4_target_object_type_rviz_plugin + +This plugin allows you to check which types of the dynamic object is being used by each planner. + +![window](./image/window.png) + +## Limitations + +Currently, which parameters of which module to check are hardcoded. In the future, this will be parameterized using YAML. diff --git a/common/tier4_target_object_type_rviz_plugin/image/window.png b/common/tier4_target_object_type_rviz_plugin/image/window.png new file mode 100644 index 0000000000000..33aa9a1e5a26e Binary files /dev/null and b/common/tier4_target_object_type_rviz_plugin/image/window.png differ diff --git a/common/tier4_target_object_type_rviz_plugin/package.xml b/common/tier4_target_object_type_rviz_plugin/package.xml new file mode 100644 index 0000000000000..04d2f28d9ba3e --- /dev/null +++ b/common/tier4_target_object_type_rviz_plugin/package.xml @@ -0,0 +1,28 @@ + + + + tier4_target_object_type_rviz_plugin + 0.0.1 + The tier4_target_object_type_rviz_plugin package + Takamasa Horibe + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + + cv_bridge + libqt5-core + libqt5-gui + libqt5-widgets + qtbase5-dev + rclcpp + rviz_common + rviz_rendering + ament_lint_auto + autoware_lint_common + + + ament_cmake + + + diff --git a/common/tier4_target_object_type_rviz_plugin/plugins/plugin_description.xml b/common/tier4_target_object_type_rviz_plugin/plugins/plugin_description.xml new file mode 100644 index 0000000000000..eb3350e4333bd --- /dev/null +++ b/common/tier4_target_object_type_rviz_plugin/plugins/plugin_description.xml @@ -0,0 +1,9 @@ + + + + TargetObjectTypePanel + + + diff --git a/common/tier4_target_object_type_rviz_plugin/src/target_object_type_panel.cpp b/common/tier4_target_object_type_rviz_plugin/src/target_object_type_panel.cpp new file mode 100644 index 0000000000000..cf5960cac281f --- /dev/null +++ b/common/tier4_target_object_type_rviz_plugin/src/target_object_type_panel.cpp @@ -0,0 +1,248 @@ +// Copyright 2023 TIER IV, Inc. All rights reserved. +// +// 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 "target_object_type_panel.hpp" + +#include +#include +#include +#include +#include + +TargetObjectTypePanel::TargetObjectTypePanel(QWidget * parent) : rviz_common::Panel(parent) +{ + node_ = std::make_shared("matrix_display_node"); + + setParameters(); + + matrix_widget_ = new QTableWidget(modules_.size(), targets_.size(), this); + for (size_t i = 0; i < modules_.size(); i++) { + matrix_widget_->setVerticalHeaderItem( + i, new QTableWidgetItem(QString::fromStdString(modules_[i]))); + } + for (size_t j = 0; j < targets_.size(); j++) { + matrix_widget_->setHorizontalHeaderItem( + j, new QTableWidgetItem(QString::fromStdString(targets_[j]))); + } + + updateMatrix(); + + reload_button_ = new QPushButton("Reload", this); + connect( + reload_button_, &QPushButton::clicked, this, &TargetObjectTypePanel::onReloadButtonClicked); + + QVBoxLayout * layout = new QVBoxLayout; + layout->addWidget(matrix_widget_); + layout->addWidget(reload_button_); + setLayout(layout); +} + +void TargetObjectTypePanel::onReloadButtonClicked() +{ + RCLCPP_INFO(node_->get_logger(), "Reload button clicked. Update parameter data."); + updateMatrix(); +} + +void TargetObjectTypePanel::setParameters() +{ + // Parameter will be investigated for these modules: + modules_ = { + "avoidance", + "avoidance_by_lane_change", + "lane_change", + "obstacle_cruise (inside)", + "obstacle_cruise (outside)", + "obstacle_stop", + "obstacle_slowdown"}; + + // Parameter will be investigated for targets in each module + targets_ = {"car", "truck", "bus", "trailer", "unknown", "bicycle", "motorcycle", "pedestrian"}; + + // TODO(Horibe): If the param naming strategy is aligned, this should be done automatically based + // on the modules_ and targets_. + + // default + ParamNameEnableObject default_param_name; + default_param_name.name.emplace("car", "car"); + default_param_name.name.emplace("truck", "truck"); + default_param_name.name.emplace("bus", "bus"); + default_param_name.name.emplace("trailer", "trailer"); + default_param_name.name.emplace("unknown", "unknown"); + default_param_name.name.emplace("bicycle", "bicycle"); + default_param_name.name.emplace("motorcycle", "motorcycle"); + default_param_name.name.emplace("pedestrian", "pedestrian"); + + // avoidance + { + const auto module = "avoidance"; + ParamNameEnableObject param_name; + param_name.node = + "/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner"; + param_name.ns = "avoidance.target_object"; + param_name.name.emplace("car", "car.is_target"); + param_name.name.emplace("truck", "truck.is_target"); + param_name.name.emplace("bus", "bus.is_target"); + param_name.name.emplace("trailer", "trailer.is_target"); + param_name.name.emplace("unknown", "unknown.is_target"); + param_name.name.emplace("bicycle", "bicycle.is_target"); + param_name.name.emplace("motorcycle", "motorcycle.is_target"); + param_name.name.emplace("pedestrian", "pedestrian.is_target"); + param_names_.emplace(module, param_name); + } + + // avoidance_by_lane_change + { + const auto module = "avoidance_by_lane_change"; + ParamNameEnableObject param_name; + param_name.node = + "/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner"; + param_name.ns = "avoidance_by_lane_change.target_object"; + param_name.name.emplace("car", "car.is_target"); + param_name.name.emplace("truck", "truck.is_target"); + param_name.name.emplace("bus", "bus.is_target"); + param_name.name.emplace("trailer", "trailer.is_target"); + param_name.name.emplace("unknown", "unknown.is_target"); + param_name.name.emplace("bicycle", "bicycle.is_target"); + param_name.name.emplace("motorcycle", "motorcycle.is_target"); + param_name.name.emplace("pedestrian", "pedestrian.is_target"); + param_names_.emplace(module, param_name); + } + + // lane_change + { + const auto module = "lane_change"; + ParamNameEnableObject param_name; + param_name.node = + "/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner"; + param_name.ns = "lane_change.target_object"; + param_name.name = default_param_name.name; + param_names_.emplace(module, param_name); + } + + // obstacle cruise (inside) + { + const auto module = "obstacle_cruise (inside)"; + ParamNameEnableObject param_name; + param_name.node = + "/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner"; + param_name.ns = "common.cruise_obstacle_type.inside"; + param_name.name = default_param_name.name; + param_names_.emplace(module, param_name); + } + + // obstacle cruise (outside) + { + const auto module = "obstacle_cruise (outside)"; + ParamNameEnableObject param_name; + param_name.node = + "/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner"; + param_name.ns = "common.cruise_obstacle_type.outside"; + param_name.name = default_param_name.name; + param_names_.emplace(module, param_name); + } + + // obstacle stop + { + const auto module = "obstacle_stop"; + ParamNameEnableObject param_name; + param_name.node = + "/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner"; + param_name.ns = "common.stop_obstacle_type"; + param_name.name = default_param_name.name; + param_names_.emplace(module, param_name); + } + + // obstacle slowdown + { + const auto module = "obstacle_slowdown"; + ParamNameEnableObject param_name; + param_name.node = + "/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner"; + param_name.ns = "common.slow_down_obstacle_type"; + param_name.name = default_param_name.name; + param_names_.emplace(module, param_name); + } +} + +void TargetObjectTypePanel::updateMatrix() +{ + // blue base + // const QColor color_in_use("#6eb6cc"); + // const QColor color_no_use("#1d3e48"); + // const QColor color_undefined("#9e9e9e"); + + // green base + const QColor color_in_use("#afff70"); + const QColor color_no_use("#44642b"); + const QColor color_undefined("#9e9e9e"); + + const auto set_undefined = [&](const auto i, const auto j) { + QTableWidgetItem * item = new QTableWidgetItem("N/A"); + item->setForeground(QBrush(Qt::black)); // set the text color to black + item->setBackground(color_undefined); + matrix_widget_->setItem(i, j, item); + }; + + for (size_t i = 0; i < modules_.size(); i++) { + const auto & module = modules_[i]; + + // Check if module exists in param_names_ + if (param_names_.find(module) == param_names_.end()) { + RCLCPP_WARN_STREAM(node_->get_logger(), module << " is not in the param names"); + continue; + } + + const auto & module_params = param_names_.at(module); + auto parameters_client = + std::make_shared(node_, module_params.node); + if (!parameters_client->wait_for_service(std::chrono::microseconds(500))) { + RCLCPP_WARN_STREAM( + node_->get_logger(), "Failed to find parameter service for node: " << module_params.node); + for (size_t j = 0; j < targets_.size(); j++) { + set_undefined(i, j); + } + continue; + } + + for (size_t j = 0; j < targets_.size(); j++) { + const auto & target = targets_[j]; + + // Check if target exists in module's name map + if (module_params.name.find(target) == module_params.name.end()) { + RCLCPP_WARN_STREAM( + node_->get_logger(), target << " parameter is not set in the " << module); + continue; + } + + std::string param_name = module_params.ns + "." + module_params.name.at(target); + auto parameter_result = parameters_client->get_parameters({param_name}); + + if (!parameter_result.empty()) { + bool value = parameter_result[0].as_bool(); + QTableWidgetItem * item = new QTableWidgetItem(value ? "O" : "X"); + item->setForeground(QBrush(value ? Qt::black : Qt::black)); // set the text color to black + item->setBackground(QBrush(value ? color_in_use : color_no_use)); + matrix_widget_->setItem(i, j, item); + } else { + RCLCPP_WARN_STREAM( + node_->get_logger(), + "Failed to get parameter " << module_params.node << " " << param_name); + + set_undefined(i, j); + } + } + } +} + +PLUGINLIB_EXPORT_CLASS(TargetObjectTypePanel, rviz_common::Panel) diff --git a/common/tier4_target_object_type_rviz_plugin/src/target_object_type_panel.hpp b/common/tier4_target_object_type_rviz_plugin/src/target_object_type_panel.hpp new file mode 100644 index 0000000000000..1f3934369bfba --- /dev/null +++ b/common/tier4_target_object_type_rviz_plugin/src/target_object_type_panel.hpp @@ -0,0 +1,60 @@ +// Copyright 2023 TIER IV, Inc. All rights reserved. +// +// 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 TARGET_OBJECT_TYPE_PANEL_HPP_ +#define TARGET_OBJECT_TYPE_PANEL_HPP_ + +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +class TargetObjectTypePanel : public rviz_common::Panel +{ + Q_OBJECT + +public: + explicit TargetObjectTypePanel(QWidget * parent = 0); + +protected: + QTableWidget * matrix_widget_; + std::shared_ptr node_; + std::vector modules_; + std::vector targets_; + + struct ParamNameEnableObject + { + std::string node; + std::string ns; + std::unordered_map name; + }; + std::unordered_map param_names_; + +private slots: + void onReloadButtonClicked(); + +private: + QPushButton * reload_button_; + + void updateMatrix(); + void setParameters(); +}; + +#endif // TARGET_OBJECT_TYPE_PANEL_HPP_ diff --git a/common/tier4_traffic_light_rviz_plugin/README.md b/common/tier4_traffic_light_rviz_plugin/README.md index 953412a917432..91fb5bc124cb7 100644 --- a/common/tier4_traffic_light_rviz_plugin/README.md +++ b/common/tier4_traffic_light_rviz_plugin/README.md @@ -8,9 +8,9 @@ This plugin panel publishes dummy traffic light signals. ### Output -| Name | Type | Description | -| ------------------------------------------------------- | -------------------------------------------------------- | ----------------------------- | -| `/perception/traffic_light_recognition/traffic_signals` | `autoware_auto_perception_msgs::msg::TrafficSignalArray` | Publish traffic light signals | +| Name | Type | Description | +| ------------------------------------------------------- | --------------------------------------------------- | ----------------------------- | +| `/perception/traffic_light_recognition/traffic_signals` | `autoware_perception_msgs::msg::TrafficSignalArray` | Publish traffic light signals | ## HowToUse diff --git a/common/tier4_traffic_light_rviz_plugin/package.xml b/common/tier4_traffic_light_rviz_plugin/package.xml index 21d8bae8f6cff..2b118b429f1af 100644 --- a/common/tier4_traffic_light_rviz_plugin/package.xml +++ b/common/tier4_traffic_light_rviz_plugin/package.xml @@ -11,7 +11,7 @@ autoware_cmake autoware_auto_mapping_msgs - autoware_auto_perception_msgs + autoware_perception_msgs lanelet2_extension libqt5-core libqt5-gui diff --git a/common/tier4_traffic_light_rviz_plugin/src/traffic_light_publish_panel.cpp b/common/tier4_traffic_light_rviz_plugin/src/traffic_light_publish_panel.cpp index e4fb0095b8d0a..35c5a88a2f8a6 100644 --- a/common/tier4_traffic_light_rviz_plugin/src/traffic_light_publish_panel.cpp +++ b/common/tier4_traffic_light_rviz_plugin/src/traffic_light_publish_panel.cpp @@ -34,6 +34,7 @@ #include #include +#undef signals namespace rviz_plugins { TrafficLightPublishPanel::TrafficLightPublishPanel(QWidget * parent) : rviz_common::Panel(parent) @@ -138,55 +139,55 @@ void TrafficLightPublishPanel::onSetTrafficLightState() const auto shape = light_shape_combo_->currentText(); const auto status = light_status_combo_->currentText(); - TrafficLight traffic_light; + TrafficSignalElement traffic_light; traffic_light.confidence = traffic_light_confidence_input_->value(); if (color == "RED") { - traffic_light.color = TrafficLight::RED; + traffic_light.color = TrafficSignalElement::RED; } else if (color == "AMBER") { - traffic_light.color = TrafficLight::AMBER; + traffic_light.color = TrafficSignalElement::AMBER; } else if (color == "GREEN") { - traffic_light.color = TrafficLight::GREEN; + traffic_light.color = TrafficSignalElement::GREEN; } else if (color == "WHITE") { - traffic_light.color = TrafficLight::WHITE; + traffic_light.color = TrafficSignalElement::WHITE; } else if (color == "UNKNOWN") { - traffic_light.color = TrafficLight::UNKNOWN; + traffic_light.color = TrafficSignalElement::UNKNOWN; } if (shape == "CIRCLE") { - traffic_light.shape = TrafficLight::CIRCLE; + traffic_light.shape = TrafficSignalElement::CIRCLE; } else if (shape == "LEFT_ARROW") { - traffic_light.shape = TrafficLight::LEFT_ARROW; + traffic_light.shape = TrafficSignalElement::LEFT_ARROW; } else if (shape == "RIGHT_ARROW") { - traffic_light.shape = TrafficLight::RIGHT_ARROW; + traffic_light.shape = TrafficSignalElement::RIGHT_ARROW; } else if (shape == "UP_ARROW") { - traffic_light.shape = TrafficLight::UP_ARROW; + traffic_light.shape = TrafficSignalElement::UP_ARROW; } else if (shape == "DOWN_ARROW") { - traffic_light.shape = TrafficLight::DOWN_ARROW; + traffic_light.shape = TrafficSignalElement::DOWN_ARROW; } else if (shape == "DOWN_LEFT_ARROW") { - traffic_light.shape = TrafficLight::DOWN_LEFT_ARROW; + traffic_light.shape = TrafficSignalElement::DOWN_LEFT_ARROW; } else if (shape == "DOWN_RIGHT_ARROW") { - traffic_light.shape = TrafficLight::DOWN_RIGHT_ARROW; + traffic_light.shape = TrafficSignalElement::DOWN_RIGHT_ARROW; } else if (shape == "UNKNOWN") { - traffic_light.shape = TrafficLight::UNKNOWN; + traffic_light.shape = TrafficSignalElement::UNKNOWN; } if (status == "SOLID_OFF") { - traffic_light.status = TrafficLight::SOLID_OFF; + traffic_light.status = TrafficSignalElement::SOLID_OFF; } else if (status == "SOLID_ON") { - traffic_light.status = TrafficLight::SOLID_ON; + traffic_light.status = TrafficSignalElement::SOLID_ON; } else if (status == "FLASHING") { - traffic_light.status = TrafficLight::FLASHING; + traffic_light.status = TrafficSignalElement::FLASHING; } else if (status == "UNKNOWN") { - traffic_light.status = TrafficLight::UNKNOWN; + traffic_light.status = TrafficSignalElement::UNKNOWN; } TrafficSignal traffic_signal; - traffic_signal.lights.push_back(traffic_light); - traffic_signal.map_primitive_id = traffic_light_id; + traffic_signal.elements.push_back(traffic_light); + traffic_signal.traffic_signal_id = traffic_light_id; for (auto & signal : extra_traffic_signals_.signals) { - if (signal.map_primitive_id == traffic_light_id) { + if (signal.traffic_signal_id == traffic_light_id) { signal = traffic_signal; return; } @@ -247,7 +248,7 @@ void TrafficLightPublishPanel::createWallTimer() void TrafficLightPublishPanel::onTimer() { if (enable_publish_) { - extra_traffic_signals_.header.stamp = rclcpp::Clock().now(); + extra_traffic_signals_.stamp = rclcpp::Clock().now(); pub_traffic_signals_->publish(extra_traffic_signals_); } @@ -260,35 +261,35 @@ void TrafficLightPublishPanel::onTimer() for (size_t i = 0; i < extra_traffic_signals_.signals.size(); ++i) { const auto & signal = extra_traffic_signals_.signals.at(i); - if (signal.lights.empty()) { + if (signal.elements.empty()) { continue; } - auto id_label = new QLabel(QString::number(signal.map_primitive_id)); + auto id_label = new QLabel(QString::number(signal.traffic_signal_id)); id_label->setAlignment(Qt::AlignCenter); auto color_label = new QLabel(); color_label->setAlignment(Qt::AlignCenter); - const auto & light = signal.lights.front(); + const auto & light = signal.elements.front(); switch (light.color) { - case TrafficLight::RED: + case TrafficSignalElement::RED: color_label->setText("RED"); color_label->setStyleSheet("background-color: #FF0000;"); break; - case TrafficLight::AMBER: + case TrafficSignalElement::AMBER: color_label->setText("AMBER"); color_label->setStyleSheet("background-color: #FFBF00;"); break; - case TrafficLight::GREEN: + case TrafficSignalElement::GREEN: color_label->setText("GREEN"); color_label->setStyleSheet("background-color: #7CFC00;"); break; - case TrafficLight::WHITE: + case TrafficSignalElement::WHITE: color_label->setText("WHITE"); color_label->setStyleSheet("background-color: #FFFFFF;"); break; - case TrafficLight::UNKNOWN: + case TrafficSignalElement::UNKNOWN: color_label->setText("UNKNOWN"); color_label->setStyleSheet("background-color: #808080;"); break; @@ -300,31 +301,28 @@ void TrafficLightPublishPanel::onTimer() shape_label->setAlignment(Qt::AlignCenter); switch (light.shape) { - case TrafficLight::CIRCLE: + case TrafficSignalElement::CIRCLE: shape_label->setText("CIRCLE"); break; - case TrafficLight::LEFT_ARROW: + case TrafficSignalElement::LEFT_ARROW: shape_label->setText("LEFT_ARROW"); break; - case TrafficLight::RIGHT_ARROW: + case TrafficSignalElement::RIGHT_ARROW: shape_label->setText("RIGHT_ARROW"); break; - case TrafficLight::UP_ARROW: + case TrafficSignalElement::UP_ARROW: shape_label->setText("UP_ARROW"); break; - case TrafficLight::DOWN_ARROW: + case TrafficSignalElement::DOWN_ARROW: shape_label->setText("DOWN_ARROW"); break; - case TrafficLight::DOWN_LEFT_ARROW: + case TrafficSignalElement::DOWN_LEFT_ARROW: shape_label->setText("DOWN_LEFT_ARROW"); break; - case TrafficLight::DOWN_RIGHT_ARROW: + case TrafficSignalElement::DOWN_RIGHT_ARROW: shape_label->setText("DOWN_RIGHT_ARROW"); break; - case TrafficLight::FLASHING: - shape_label->setText("FLASHING"); - break; - case TrafficLight::UNKNOWN: + case TrafficSignalElement::UNKNOWN: shape_label->setText("UNKNOWN"); break; default: @@ -335,16 +333,16 @@ void TrafficLightPublishPanel::onTimer() status_label->setAlignment(Qt::AlignCenter); switch (light.status) { - case TrafficLight::SOLID_OFF: + case TrafficSignalElement::SOLID_OFF: status_label->setText("SOLID_OFF"); break; - case TrafficLight::SOLID_ON: + case TrafficSignalElement::SOLID_ON: status_label->setText("SOLID_ON"); break; - case TrafficLight::FLASHING: + case TrafficSignalElement::FLASHING: status_label->setText("FLASHING"); break; - case TrafficLight::UNKNOWN: + case TrafficSignalElement::UNKNOWN: status_label->setText("UNKNOWN"); break; default: @@ -375,11 +373,9 @@ void TrafficLightPublishPanel::onVectorMap(const HADMapBin::ConstSharedPtr msg) std::string info = "Fetching traffic lights :"; std::string delim = " "; for (auto && tl_reg_elem_ptr : tl_reg_elems) { - for (auto && traffic_light : tl_reg_elem_ptr->trafficLights()) { - auto id = static_cast(traffic_light.id()); - info += (std::exchange(delim, ", ") + std::to_string(id)); - traffic_light_ids_.insert(id); - } + auto id = static_cast(tl_reg_elem_ptr->id()); + info += (std::exchange(delim, ", ") + std::to_string(id)); + traffic_light_ids_.insert(id); } RCLCPP_INFO_STREAM(raw_node_->get_logger(), info); received_vector_map_ = true; diff --git a/common/tier4_traffic_light_rviz_plugin/src/traffic_light_publish_panel.hpp b/common/tier4_traffic_light_rviz_plugin/src/traffic_light_publish_panel.hpp index 75e6405417084..e54b6a301873b 100644 --- a/common/tier4_traffic_light_rviz_plugin/src/traffic_light_publish_panel.hpp +++ b/common/tier4_traffic_light_rviz_plugin/src/traffic_light_publish_panel.hpp @@ -27,7 +27,7 @@ #include #include -#include +#include #endif #include @@ -36,10 +36,9 @@ namespace rviz_plugins { using autoware_auto_mapping_msgs::msg::HADMapBin; -using autoware_auto_perception_msgs::msg::TrafficLight; -using autoware_auto_perception_msgs::msg::TrafficSignal; -using autoware_auto_perception_msgs::msg::TrafficSignalArray; - +using autoware_perception_msgs::msg::TrafficSignal; +using autoware_perception_msgs::msg::TrafficSignalArray; +using autoware_perception_msgs::msg::TrafficSignalElement; class TrafficLightPublishPanel : public rviz_common::Panel { Q_OBJECT diff --git a/common/tier4_vehicle_rviz_plugin/src/tools/acceleration_meter.hpp b/common/tier4_vehicle_rviz_plugin/src/tools/acceleration_meter.hpp index 371480eccdbce..8a962cea575e8 100644 --- a/common/tier4_vehicle_rviz_plugin/src/tools/acceleration_meter.hpp +++ b/common/tier4_vehicle_rviz_plugin/src/tools/acceleration_meter.hpp @@ -25,7 +25,7 @@ #include #include #include -#include +#include #include #endif diff --git a/common/tier4_vehicle_rviz_plugin/src/tools/console_meter.hpp b/common/tier4_vehicle_rviz_plugin/src/tools/console_meter.hpp index e71003ea7f61c..cba0fa16b50fe 100644 --- a/common/tier4_vehicle_rviz_plugin/src/tools/console_meter.hpp +++ b/common/tier4_vehicle_rviz_plugin/src/tools/console_meter.hpp @@ -25,7 +25,8 @@ #include #include #include -#include +#include +#include #include #endif diff --git a/control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp b/control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp index 8859367c25e9b..a8f260795d485 100644 --- a/control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp +++ b/control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp @@ -28,6 +28,7 @@ #include #include #include +#include #include diff --git a/control/autonomous_emergency_braking/package.xml b/control/autonomous_emergency_braking/package.xml index 4f2ad6c50a9b3..5dc65cb243bfc 100644 --- a/control/autonomous_emergency_braking/package.xml +++ b/control/autonomous_emergency_braking/package.xml @@ -6,6 +6,8 @@ Autonomous Emergency Braking package as a ROS 2 node Yutaka Shimizu Takamasa Horibe + Tomoya Kimura + Apache License 2.0 ament_cmake diff --git a/control/autonomous_emergency_braking/src/node.cpp b/control/autonomous_emergency_braking/src/node.cpp index 7e10ca9864f11..0ccc9bc4a3dae 100644 --- a/control/autonomous_emergency_braking/src/node.cpp +++ b/control/autonomous_emergency_braking/src/node.cpp @@ -15,7 +15,11 @@ #include "autonomous_emergency_braking/node.hpp" #include -#include +#include +#include +#include + +#include #include #include @@ -29,6 +33,8 @@ #include #endif +#include +#include namespace autoware::motion::control::autonomous_emergency_braking { using diagnostic_msgs::msg::DiagnosticStatus; @@ -426,6 +432,10 @@ void AEB::generateEgoPath( const Trajectory & predicted_traj, Path & path, std::vector & polygons) { + if (predicted_traj.points.empty()) { + return; + } + geometry_msgs::msg::TransformStamped transform_stamped{}; try { transform_stamped = tf_buffer_.lookupTransform( diff --git a/control/control_performance_analysis/launch/controller_performance_analysis.launch.xml b/control/control_performance_analysis/launch/control_performance_analysis.launch.xml similarity index 100% rename from control/control_performance_analysis/launch/controller_performance_analysis.launch.xml rename to control/control_performance_analysis/launch/control_performance_analysis.launch.xml diff --git a/control/control_performance_analysis/schema/control_performance_analysis.schema.json b/control/control_performance_analysis/schema/control_performance_analysis.schema.json new file mode 100644 index 0000000000000..f7acf3778bf46 --- /dev/null +++ b/control/control_performance_analysis/schema/control_performance_analysis.schema.json @@ -0,0 +1,62 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Control Performance Analysis Node", + "type": "object", + "definitions": { + "control_performance_analysis": { + "type": "object", + "properties": { + "curvature_interval_length": { + "type": "number", + "default": 5.0, + "description": "Used for estimating current curvature." + }, + "prevent_zero_division_value": { + "type": "number", + "default": 0.001, + "description": "Value to avoid zero division.." + }, + "odom_interval": { + "type": "integer", + "default": 0, + "description": "Interval between odom messages, increase it for smoother curve." + }, + "acceptable_max_distance_to_waypoint": { + "type": "number", + "default": 2.0, + "description": "Maximum distance between trajectory point and vehicle [m]." + }, + "acceptable_max_yaw_difference_rad": { + "type": "number", + "default": 0.95, + "description": "Maximum yaw difference between trajectory point and vehicle [rad]." + }, + "low_pass_filter_gain": { + "type": "number", + "default": 1.0472, + "description": "Low pass filter gain." + } + }, + "required": [ + "curvature_interval_length", + "prevent_zero_division_value", + "odom_interval", + "acceptable_max_distance_to_waypoint", + "acceptable_max_yaw_difference_rad", + "low_pass_filter_gain" + ] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/control_performance_analysis" + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] +} diff --git a/control/control_performance_analysis/src/control_performance_analysis_node.cpp b/control/control_performance_analysis/src/control_performance_analysis_node.cpp index ad845c86fc19a..768aed321b4f0 100644 --- a/control/control_performance_analysis/src/control_performance_analysis_node.cpp +++ b/control/control_performance_analysis/src/control_performance_analysis_node.cpp @@ -47,7 +47,7 @@ ControlPerformanceAnalysisNode::ControlPerformanceAnalysisNode( // Node Parameters. param_.curvature_interval_length_ = declare_parameter("curvature_interval_length"); param_.prevent_zero_division_value_ = declare_parameter("prevent_zero_division_value"); - param_.odom_interval_ = declare_parameter("odom_interval"); + param_.odom_interval_ = declare_parameter("odom_interval"); param_.acceptable_max_distance_to_waypoint_ = declare_parameter("acceptable_max_distance_to_waypoint"); param_.acceptable_max_yaw_difference_rad_ = diff --git a/control/control_validator/src/control_validator.cpp b/control/control_validator/src/control_validator.cpp index 59b798a786722..ffce38f009a41 100644 --- a/control/control_validator/src/control_validator.cpp +++ b/control/control_validator/src/control_validator.cpp @@ -16,9 +16,6 @@ #include "control_validator/utils.hpp" -#include -#include - #include #include #include @@ -163,7 +160,9 @@ void ControlValidator::publishDebugInfo() void ControlValidator::validate(const Trajectory & predicted_trajectory) { if (predicted_trajectory.points.size() < 2) { - RCLCPP_ERROR(get_logger(), "predicted_trajectory size is less than 2. Cannot validate."); + RCLCPP_ERROR_THROTTLE( + get_logger(), *get_clock(), 1000, + "predicted_trajectory size is less than 2. Cannot validate."); return; } @@ -204,7 +203,9 @@ void ControlValidator::displayStatus() const auto & s = validation_status_; - warn(s.is_valid_max_distance_deviation, "planning trajectory is too far from ego!!"); + warn( + s.is_valid_max_distance_deviation, + "predicted trajectory is too far from planning trajectory!!"); } } // namespace control_validator diff --git a/control/control_validator/src/debug_marker.cpp b/control/control_validator/src/debug_marker.cpp index c94c22c807648..1b92aee0bb416 100644 --- a/control/control_validator/src/debug_marker.cpp +++ b/control/control_validator/src/debug_marker.cpp @@ -14,8 +14,8 @@ #include "control_validator/debug_marker.hpp" -#include -#include +#include +#include #include #include diff --git a/control/control_validator/src/utils.cpp b/control/control_validator/src/utils.cpp index 88b8431b07af0..fa8261e9c3f95 100644 --- a/control/control_validator/src/utils.cpp +++ b/control/control_validator/src/utils.cpp @@ -16,7 +16,7 @@ #include #include -#include +#include #include #include @@ -44,7 +44,8 @@ void insertPointInPredictedTrajectory( TrajectoryPoints reverseTrajectoryPoints(const TrajectoryPoints & trajectory_points) { - TrajectoryPoints reversed_trajectory_points(trajectory_points.size()); + TrajectoryPoints reversed_trajectory_points; + reversed_trajectory_points.reserve(trajectory_points.size()); std::reverse_copy( trajectory_points.begin(), trajectory_points.end(), std::back_inserter(reversed_trajectory_points)); @@ -106,7 +107,7 @@ Trajectory alignTrajectoryWithReferenceTrajectory( // predicted_trajectory: 　　　　p1-----p2-----p3----//------pN // trajectory: t1--------//------tN // ↓ - // predicted_trajectory: 　　　　 tNew--p3----//------pN + // predicted_trajectory: 　　　　 pNew--p3----//------pN // trajectory: t1--------//------tN auto predicted_trajectory_point_removed = removeFrontTrajectoryPoint( trajectory_points, modified_trajectory_points, predicted_trajectory_points); @@ -152,8 +153,8 @@ double calcMaxLateralDistance( // find nearest segment const size_t nearest_segment_idx = motion_utils::findNearestSegmentIndex(reference_trajectory.points, p0); - double temp_dist = - motion_utils::calcLateralOffset(reference_trajectory.points, p0, nearest_segment_idx); + const double temp_dist = std::abs( + motion_utils::calcLateralOffset(reference_trajectory.points, p0, nearest_segment_idx)); if (temp_dist > max_dist) { max_dist = temp_dist; } diff --git a/control/lane_departure_checker/README.md b/control/lane_departure_checker/README.md index 5148d6a998f50..67b770aefb3d2 100644 --- a/control/lane_departure_checker/README.md +++ b/control/lane_departure_checker/README.md @@ -63,15 +63,30 @@ This package includes the following features: ### Node Parameters -| Name | Type | Description | Default value | -| :---------------------------- | :----- | :------------------------------------------------------------ | :------------ | -| update_rate | double | Frequency for publishing [Hz] | 10.0 | -| visualize_lanelet | bool | Flag for visualizing lanelet | False | -| include_right_lanes | bool | Flag for including right lanelet in borders | False | -| include_left_lanes | bool | Flag for including left lanelet in borders | False | -| include_opposite_lanes | bool | Flag for including opposite lanelet in borders | False | -| include_conflicting_lanes | bool | Flag for including conflicting lanelet in borders | False | -| road_border_departure_checker | bool | Flag for checking if the vehicle will departs the road border | False | +#### General Parameters + +| Name | Type | Description | Default value | +| :------------------------- | :----- | :---------------------------------------------------------------------------------------------------------- | :------------ | +| will_out_of_lane_checker | bool | Enable checker whether ego vehicle footprint will depart from lane | True | +| out_of_lane_checker | bool | Enable checker whether ego vehicle footprint is out of lane | True | +| boundary_departure_checker | bool | Enable checker whether ego vehicle footprint wil depart from boundary specified by boundary_types_to_detect | False | +| update_rate | double | Frequency for publishing [Hz] | 10.0 | +| visualize_lanelet | bool | Flag for visualizing lanelet | False | + +#### Parameters For Lane Departure + +| Name | Type | Description | Default value | +| :------------------------ | :--- | :------------------------------------------------ | :------------ | +| include_right_lanes | bool | Flag for including right lanelet in borders | False | +| include_left_lanes | bool | Flag for including left lanelet in borders | False | +| include_opposite_lanes | bool | Flag for including opposite lanelet in borders | False | +| include_conflicting_lanes | bool | Flag for including conflicting lanelet in borders | False | + +#### Parameters For Road Border Departure + +| Name | Type | Description | Default value | +| :----------------------- | :------------------------- | :---------------------------------------------------------- | :------------ | +| boundary_types_to_detect | std::vector\ | line_string types to detect with boundary_departure_checker | [road_border] | ### Core Parameters diff --git a/control/lane_departure_checker/config/lane_departure_checker.param.yaml b/control/lane_departure_checker/config/lane_departure_checker.param.yaml index 008832b1cab21..f0a8df21c425b 100644 --- a/control/lane_departure_checker/config/lane_departure_checker.param.yaml +++ b/control/lane_departure_checker/config/lane_departure_checker.param.yaml @@ -1,5 +1,10 @@ /**: ros__parameters: + # Enable feature + will_out_of_lane_checker: true + out_of_lane_checker: true + boundary_departure_checker: false + # Node update_rate: 10.0 visualize_lanelet: false @@ -7,7 +12,8 @@ include_left_lanes: false include_opposite_lanes: false include_conflicting_lanes: false - road_border_departure_checker: false + boundary_types_to_detect: [road_border] + # Core footprint_margin_scale: 1.0 diff --git a/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp b/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp index 9822fd820dc3c..c77503106e485 100644 --- a/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp +++ b/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp @@ -18,7 +18,6 @@ #include #include #include -#include #include #include @@ -73,6 +72,7 @@ struct Input lanelet::ConstLanelets shoulder_lanelets{}; Trajectory::ConstSharedPtr reference_trajectory{}; Trajectory::ConstSharedPtr predicted_trajectory{}; + std::vector boundary_types_to_detect{}; }; struct Output @@ -80,7 +80,7 @@ struct Output std::map processing_time_map{}; bool will_leave_lane{}; bool is_out_of_lane{}; - bool will_cross_road_border{}; + bool will_cross_boundary{}; PoseDeviation trajectory_deviation{}; lanelet::ConstLanelets candidate_lanelets{}; TrajectoryPoints resampled_trajectory{}; @@ -137,9 +137,10 @@ class LaneDepartureChecker const lanelet::ConstLanelets & candidate_lanelets, const std::vector & vehicle_footprints); - static bool willCrossRoadBorder( + static bool willCrossBoundary( const lanelet::ConstLanelets & candidate_lanelets, - const std::vector & vehicle_footprints); + const std::vector & vehicle_footprints, + const std::vector & boundary_types_to_detects); static bool isCrossingRoadBorder( const lanelet::BasicLineString2d & road_border, const std::vector & footprints); diff --git a/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker_node.hpp b/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker_node.hpp index f7f7bcfda7d51..dd05ab226f6b7 100644 --- a/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker_node.hpp +++ b/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker_node.hpp @@ -18,8 +18,6 @@ #include "lane_departure_checker/lane_departure_checker.hpp" #include -#include -#include #include #include #include @@ -34,9 +32,12 @@ #include #include +#include +#include #include #include +#include #include namespace lane_departure_checker @@ -45,13 +46,17 @@ using autoware_auto_mapping_msgs::msg::HADMapBin; struct NodeParam { + bool will_out_of_lane_checker; + bool out_of_lane_checker; + bool boundary_departure_checker; + double update_rate; bool visualize_lanelet; bool include_right_lanes; bool include_left_lanes; bool include_opposite_lanes; bool include_conflicting_lanes; - bool road_border_departure_checker; + std::vector boundary_types_to_detect; }; class LaneDepartureCheckerNode : public rclcpp::Node diff --git a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp b/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp index f1d8d75452df1..9d413b2bf0a70 100644 --- a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp +++ b/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp @@ -67,19 +67,19 @@ bool isInAnyLane(const lanelet::ConstLanelets & candidate_lanelets, const Point2 return false; } -bool isCrossingWithRoadBorder( - const lanelet::BasicLineString2d & road_border, const std::vector & footprints) +bool isCrossingWithBoundary( + const lanelet::BasicLineString2d & boundary, const std::vector & footprints) { for (auto & footprint : footprints) { for (size_t i = 0; i < footprint.size() - 1; ++i) { auto footprint1 = footprint.at(i).to_3d(); auto footprint2 = footprint.at(i + 1).to_3d(); - for (size_t i = 0; i < road_border.size() - 1; ++i) { - auto road_border1 = road_border.at(i); - auto road_border2 = road_border.at(i + 1); + for (size_t i = 0; i < boundary.size() - 1; ++i) { + auto boundary1 = boundary.at(i); + auto boundary2 = boundary.at(i + 1); if (tier4_autoware_utils::intersect( tier4_autoware_utils::toMsg(footprint1), tier4_autoware_utils::toMsg(footprint2), - fromVector2dToMsg(road_border1), fromVector2dToMsg(road_border2))) { + fromVector2dToMsg(boundary1), fromVector2dToMsg(boundary2))) { return true; } } @@ -172,9 +172,9 @@ Output LaneDepartureChecker::update(const Input & input) output.is_out_of_lane = isOutOfLane(output.candidate_lanelets, output.vehicle_footprints.front()); output.processing_time_map["isOutOfLane"] = stop_watch.toc(true); - output.will_cross_road_border = - willCrossRoadBorder(output.candidate_lanelets, output.vehicle_footprints); - output.processing_time_map["willCrossRoadBorder"] = stop_watch.toc(true); + output.will_cross_boundary = willCrossBoundary( + output.candidate_lanelets, output.vehicle_footprints, input.boundary_types_to_detect); + output.processing_time_map["willCrossBoundary"] = stop_watch.toc(true); return output; } @@ -334,15 +334,18 @@ bool LaneDepartureChecker::isOutOfLane( return false; } -bool LaneDepartureChecker::willCrossRoadBorder( +bool LaneDepartureChecker::willCrossBoundary( const lanelet::ConstLanelets & candidate_lanelets, - const std::vector & vehicle_footprints) + const std::vector & vehicle_footprints, + const std::vector & boundary_types_to_detect) { for (const auto & candidate_lanelet : candidate_lanelets) { const std::string r_type = candidate_lanelet.rightBound().attributeOr(lanelet::AttributeName::Type, "none"); - if (r_type == "road_border") { - if (isCrossingWithRoadBorder( + if ( + std::find(boundary_types_to_detect.begin(), boundary_types_to_detect.end(), r_type) != + boundary_types_to_detect.end()) { + if (isCrossingWithBoundary( candidate_lanelet.rightBound2d().basicLineString(), vehicle_footprints)) { // std::cerr << "The crossed road_border's line string id: " // << candidate_lanelet.rightBound().id() << std::endl; @@ -351,8 +354,10 @@ bool LaneDepartureChecker::willCrossRoadBorder( } const std::string l_type = candidate_lanelet.leftBound().attributeOr(lanelet::AttributeName::Type, "none"); - if (l_type == "road_border") { - if (isCrossingWithRoadBorder( + if ( + std::find(boundary_types_to_detect.begin(), boundary_types_to_detect.end(), l_type) != + boundary_types_to_detect.end()) { + if (isCrossingWithBoundary( candidate_lanelet.leftBound2d().basicLineString(), vehicle_footprints)) { // std::cerr << "The crossed road_border's line string id: " // << candidate_lanelet.leftBound().id() << std::endl; diff --git a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp b/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp index 20d535a82bfa1..b40a0d0473135 100644 --- a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp +++ b/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp @@ -14,8 +14,10 @@ #include "lane_departure_checker/lane_departure_checker_node.hpp" +#include #include #include +#include #include #include #include @@ -125,6 +127,11 @@ LaneDepartureCheckerNode::LaneDepartureCheckerNode(const rclcpp::NodeOptions & o { using std::placeholders::_1; + // Enable feature + node_param_.will_out_of_lane_checker = declare_parameter("will_out_of_lane_checker"); + node_param_.out_of_lane_checker = declare_parameter("out_of_lane_checker"); + node_param_.boundary_departure_checker = declare_parameter("boundary_departure_checker"); + // Node Parameter node_param_.update_rate = declare_parameter("update_rate"); node_param_.visualize_lanelet = declare_parameter("visualize_lanelet"); @@ -132,8 +139,10 @@ LaneDepartureCheckerNode::LaneDepartureCheckerNode(const rclcpp::NodeOptions & o node_param_.include_left_lanes = declare_parameter("include_left_lanes"); node_param_.include_opposite_lanes = declare_parameter("include_opposite_lanes"); node_param_.include_conflicting_lanes = declare_parameter("include_conflicting_lanes"); - node_param_.road_border_departure_checker = - declare_parameter("road_border_departure_checker"); + + // Boundary_departure_checker + node_param_.boundary_types_to_detect = + declare_parameter>("boundary_types_to_detect"); // Vehicle Info const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); @@ -338,6 +347,7 @@ void LaneDepartureCheckerNode::onTimer() input_.shoulder_lanelets = shoulder_lanelets_; input_.reference_trajectory = reference_trajectory_; input_.predicted_trajectory = predicted_trajectory_; + input_.boundary_types_to_detect = node_param_.boundary_types_to_detect; processing_time_map["Node: setInputData"] = stop_watch.toc(true); output_ = lane_departure_checker_->update(input_); @@ -377,12 +387,19 @@ rcl_interfaces::msg::SetParametersResult LaneDepartureCheckerNode::onParameter( result.reason = "success"; try { + // Enable feature + update_param(parameters, "will_out_of_lane_checker", node_param_.will_out_of_lane_checker); + update_param(parameters, "out_of_lane_checker", node_param_.out_of_lane_checker); + update_param(parameters, "boundary_departure_checker", node_param_.boundary_departure_checker); + // Node update_param(parameters, "visualize_lanelet", node_param_.visualize_lanelet); update_param(parameters, "include_right_lanes", node_param_.include_right_lanes); update_param(parameters, "include_left_lanes", node_param_.include_left_lanes); update_param(parameters, "include_opposite_lanes", node_param_.include_opposite_lanes); update_param(parameters, "include_conflicting_lanes", node_param_.include_conflicting_lanes); + update_param(parameters, "boundary_departure_checker", node_param_.boundary_departure_checker); + update_param(parameters, "boundary_types_to_detect", node_param_.boundary_types_to_detect); // Core update_param(parameters, "footprint_margin_scale", param_.footprint_margin_scale); @@ -409,19 +426,19 @@ void LaneDepartureCheckerNode::checkLaneDeparture( int8_t level = DiagStatus::OK; std::string msg = "OK"; - if (output_.will_leave_lane) { + if (output_.will_leave_lane && node_param_.will_out_of_lane_checker) { level = DiagStatus::WARN; msg = "vehicle will leave lane"; } - if (output_.is_out_of_lane) { + if (output_.is_out_of_lane && node_param_.out_of_lane_checker) { level = DiagStatus::ERROR; msg = "vehicle is out of lane"; } - if (output_.will_cross_road_border && node_param_.road_border_departure_checker) { + if (output_.will_cross_boundary && node_param_.boundary_departure_checker) { level = DiagStatus::ERROR; - msg = "vehicle will cross road border"; + msg = "vehicle will cross boundary"; } stat.summary(level, msg); diff --git a/control/mpc_lateral_controller/README.md b/control/mpc_lateral_controller/README.md index 7579e3da265c4..50e9d44a0a7a4 100644 --- a/control/mpc_lateral_controller/README.md +++ b/control/mpc_lateral_controller/README.md @@ -67,7 +67,7 @@ Set the following from the [controller_node](../trajectory_follower_node/README. - `autoware_auto_planning_msgs/Trajectory` : reference trajectory to follow. - `nav_msgs/Odometry`: current odometry -- `autoware_auto_vehicle_msgs/SteeringReport` current steering +- `autoware_auto_vehicle_msgs/SteeringReport`: current steering ### Outputs @@ -89,83 +89,108 @@ can be calculated by providing the current steer, velocity, and pose to function The default parameters defined in `param/lateral_controller_defaults.param.yaml` are adjusted to the AutonomouStuff Lexus RX 450h for under 40 km/h driving. -| Name | Type | Description | Default value | -| :------------------------------------------- | :----- | :--------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | -| show_debug_info | bool | display debug info | false | -| traj_resample_dist | double | distance of waypoints in resampling [m] | 0.1 | -| enable_path_smoothing | bool | path smoothing flag. This should be true when uses path resampling to reduce resampling noise. | true | -| path_filter_moving_ave_num | int | number of data points moving average filter for path smoothing | 35 | -| path_smoothing_times | int | number of times of applying path smoothing filter | 1 | -| curvature_smoothing_num_ref_steer | double | index distance of points used in curvature calculation for reference steering command: p(i-num), p(i), p(i+num). larger num makes less noisy values. | 35 | -| curvature_smoothing_num_traj | double | index distance of points used in curvature calculation for trajectory: p(i-num), p(i), p(i+num). larger num makes less noisy values. | 1 | -| extend_trajectory_for_end_yaw_control | bool | trajectory extending flag for end yaw control. | true | -| steering_lpf_cutoff_hz | double | cutoff frequency of lowpass filter for steering output command [hz] | 3.0 | -| admissible_position_error | double | stop vehicle when following position error is larger than this value [m]. | 5.0 | -| admissible_yaw_error_rad | double | stop vehicle when following yaw angle error is larger than this value [rad]. | 1.57 | -| stop_state_entry_ego_speed \*1 | double | threshold value of the ego vehicle speed used to the stop state entry condition | 0.0 | -| stop_state_entry_target_speed \*1 | double | threshold value of the target speed used to the stop state entry condition | 0.0 | -| converged_steer_rad | double | threshold value of the steer convergence | 0.1 | -| keep_steer_control_until_converged | bool | keep steer control until steer is converged | true | -| new_traj_duration_time | double | threshold value of the time to be considered as new trajectory | 1.0 | -| new_traj_end_dist | double | threshold value of the distance between trajectory ends to be considered as new trajectory | 0.3 | -| mpc_converged_threshold_rps | double | threshold value to be sure output of the optimization is converged, it is used in stopped state | 0.3 | - -(\*1) To prevent unnecessary steering movement, the steering command is fixed to the previous value in the stop state. - -#### MPC algorithm - -| Name | Type | Description | Default value | -| :-------------------------------------------------- | :----- | :------------------------------------------------------------------------------------------------------------------------------------------------- | :---------------- | -| qp_solver_type | string | QP solver option. described below in detail. | unconstraint_fast | -| mpc_vehicle_model_type | string | vehicle model option. described below in detail. | kinematics | -| mpc_prediction_horizon | int | total prediction step for MPC | 70 | -| mpc_prediction_sampling_time | double | prediction period for one step [s] | 0.1 | -| mpc_weight_lat_error | double | weight for lateral error | 0.1 | -| mpc_weight_heading_error | double | weight for heading error | 0.0 | -| mpc_weight_heading_error_squared_vel_coeff | double | weight for heading error \* velocity | 5.0 | -| mpc_weight_steering_input | double | weight for steering error (steer command - reference steer) | 1.0 | -| mpc_weight_steering_input_squared_vel_coeff | double | weight for steering error (steer command - reference steer) \* velocity | 0.25 | -| mpc_weight_lat_jerk | double | weight for lateral jerk (steer(i) - steer(i-1)) \* velocity | 0.0 | -| mpc_weight_terminal_lat_error | double | terminal cost weight for lateral error | 1.0 | -| mpc_weight_steer_rate | double | weight for steering rate [rad/s] | 0.0 | -| mpc_weight_steer_acc | double | weight for derivatives of the steering rate [rad/ss] | 0.0 | -| mpc_weight_terminal_heading_error | double | terminal cost weight for heading error | 0.1 | -| mpc_low_curvature_thresh_curvature | double | trajectory curvature threshold to change the weight. If the curvature is lower than this value, the `low_curvature_weight_**` values will be used. | 0.0 | -| mpc_low_curvature_weight_lat_error | double | [used in a low curvature trajectory] weight for lateral error | 0.0 | -| mpc_low_curvature_weight_heading_error | double | [used in a low curvature trajectory] weight for heading error | 0.0 | -| mpc_low_curvature_weight_heading_error_squared_vel | double | [used in a low curvature trajectory] weight for heading error \* velocity | 0.0 | -| mpc_low_curvature_weight_steering_input | double | [used in a low curvature trajectory] weight for steering error (steer command - reference steer) | 0.0 | -| mpc_low_curvature_weight_steering_input_squared_vel | double | [used in a low curvature trajectory] weight for steering error (steer command - reference steer) \* velocity | 0.0 | -| mpc_low_curvature_weight_lat_jerk | double | [used in a low curvature trajectory] weight for lateral jerk (steer(i) - steer(i-1)) \* velocity | 0.0 | -| mpc_low_curvature_weight_steer_rate | double | [used in a low curvature trajectory] weight for steering rate [rad/s] | 0.0 | -| mpc_low_curvature_weight_steer_acc | double | [used in a low curvature trajectory] weight for derivatives of the steering rate [rad/ss] | 0.0 | -| mpc_zero_ff_steer_deg | double | threshold of feedforward angle [deg]. feedforward angle smaller than this value is set to zero. | 2.0 | - -#### Steering offset remover - -Defined in the `steering_offset` namespace. This logic is designed as simple as possible, with minimum design parameters. - -| Name | Type | Description | Default value | -| :---------------------------------- | :----- | :----------------------------------------------------------------------------------------------- | :------------ | -| enable_auto_steering_offset_removal | bool | Estimate the steering offset and apply compensation | true | -| update_vel_threshold | double | If the velocity is smaller than this value, the data is not used for the offset estimation. | 5.56 | -| update_steer_threshold | double | If the steering angle is larger than this value, the data is not used for the offset estimation. | 0.035 | -| average_num | double | The average of this number of data is used as a steering offset. | 1000 | -| steering_offset_limit | double | The angle limit to be applied to the offset compensation. | 0.02 | - -#### Vehicle model +#### System + +| Name | Type | Description | Default value | +| :------------------------ | :------ | :-------------------------------------------------------------------------- | :------------ | +| traj_resample_dist | double | distance of waypoints in resampling [m] | 0.1 | +| use_steer_prediction | boolean | flag for using steer prediction (do not use steer measurement) | false | +| admissible_position_error | double | stop vehicle when following position error is larger than this value [m] | 5.0 | +| admissible_yaw_error_rad | double | stop vehicle when following yaw angle error is larger than this value [rad] | 1.57 | + +#### Path Smoothing + +| Name | Type | Description | Default value | +| :-------------------------------- | :------ | :--------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | +| enable_path_smoothing | boolean | path smoothing flag. This should be true when uses path resampling to reduce resampling noise. | false | +| path_filter_moving_ave_num | int | number of data points moving average filter for path smoothing | 25 | +| curvature_smoothing_num_traj | int | index distance of points used in curvature calculation for trajectory: p(i-num), p(i), p(i+num). larger num makes less noisy values. | 15 | +| curvature_smoothing_num_ref_steer | int | index distance of points used in curvature calculation for reference steering command: p(i-num), p(i), p(i+num). larger num makes less noisy values. | 15 | + +#### Trajectory Extending + +| Name | Type | Description | Default value | +| :------------------------------------ | :------ | :-------------------------------------------- | :------------ | +| extend_trajectory_for_end_yaw_control | boolean | trajectory extending flag for end yaw control | true | + +#### MPC Optimization + +| Name | Type | Description | Default value | +| :-------------------------------------------------- | :----- | :----------------------------------------------------------------------------------------------------------- | :------------ | +| qp_solver_type | string | QP solver option. described below in detail. | "osqp" | +| mpc_prediction_horizon | int | total prediction step for MPC | 50 | +| mpc_prediction_dt | double | prediction period for one step [s] | 0.1 | +| mpc_weight_lat_error | double | weight for lateral error | 1.0 | +| mpc_weight_heading_error | double | weight for heading error | 0.0 | +| mpc_weight_heading_error_squared_vel | double | weight for heading error \* velocity | 0.3 | +| mpc_weight_steering_input | double | weight for steering error (steer command - reference steer) | 1.0 | +| mpc_weight_steering_input_squared_vel | double | weight for steering error (steer command - reference steer) \* velocity | 0.25 | +| mpc_weight_lat_jerk | double | weight for lateral jerk (steer(i) - steer(i-1)) \* velocity | 0.1 | +| mpc_weight_steer_rate | double | weight for steering rate [rad/s] | 0.0 | +| mpc_weight_steer_acc | double | weight for derivatives of the steering rate [rad/ss] | 0.000001 | +| mpc_low_curvature_weight_lat_error | double | [used in a low curvature trajectory] weight for lateral error | 0.1 | +| mpc_low_curvature_weight_heading_error | double | [used in a low curvature trajectory] weight for heading error | 0.0 | +| mpc_low_curvature_weight_heading_error_squared_vel | double | [used in a low curvature trajectory] weight for heading error \* velocity | 0.3 | +| mpc_low_curvature_weight_steering_input | double | [used in a low curvature trajectory] weight for steering error (steer command - reference steer) | 1.0 | +| mpc_low_curvature_weight_steering_input_squared_vel | double | [used in a low curvature trajectory] weight for steering error (steer command - reference steer) \* velocity | 0.25 | +| mpc_low_curvature_weight_lat_jerk | double | [used in a low curvature trajectory] weight for lateral jerk (steer(i) - steer(i-1)) \* velocity | 0.0 | +| mpc_low_curvature_weight_steer_rate | double | [used in a low curvature trajectory] weight for steering rate [rad/s] | 0.0 | +| mpc_low_curvature_weight_steer_acc | double | [used in a low curvature trajectory] weight for derivatives of the steering rate [rad/ss] | 0.000001 | +| mpc_low_curvature_thresh_curvature | double | threshold of curvature to use "low_curvature" parameter | 0.0 | +| mpc_weight_terminal_lat_error | double | terminal lateral error weight in matrix Q to improve mpc stability | 1.0 | +| mpc_weight_terminal_heading_error | double | terminal heading error weight in matrix Q to improve mpc stability | 0.1 | +| mpc_zero_ff_steer_deg | double | threshold that feed-forward angle becomes zero | 0.5 | +| mpc_acceleration_limit | double | limit on the vehicle's acceleration | 2.0 | +| mpc_velocity_time_constant | double | time constant used for velocity smoothing | 0.3 | +| mpc_min_prediction_length | double | minimum prediction length | 5.0 | + +#### Vehicle Model | Name | Type | Description | Default value | | :----------------------------------- | :------- | :--------------------------------------------------------------------------------- | :------------------- | +| vehicle_model_type | string | vehicle model type for mpc prediction | "kinematics" | | input_delay | double | steering input delay time for delay compensation | 0.24 | -| vehicle_model_steer_tau | double | steering dynamics time constant | 0.3 | -| steer_rate_lim_dps_list_by_curvature | [double] | steering angle rate limit list depending on curvature [deg/s] | [10.0, 20.0, 30.0] | +| vehicle_model_steer_tau | double | steering dynamics time constant (1d approximation) [s] | 0.3 | +| steer_rate_lim_dps_list_by_curvature | [double] | steering angle rate limit list depending on curvature [deg/s] | [40.0, 50.0, 60.0] | | curvature_list_for_steer_rate_lim | [double] | curvature list for steering angle rate limit interpolation in ascending order [/m] | [0.001, 0.002, 0.01] | -| steer_rate_lim_dps_list_by_velocity | [double] | steering angle rate limit list depending on velocity [deg/s] | [40.0, 30.0, 20.0] | +| steer_rate_lim_dps_list_by_velocity | [double] | steering angle rate limit list depending on velocity [deg/s] | [60.0, 50.0, 40.0] | | velocity_list_for_steer_rate_lim | [double] | velocity list for steering angle rate limit interpolation in ascending order [m/s] | [10.0, 15.0, 20.0] | | acceleration_limit | double | acceleration limit for trajectory velocity modification [m/ss] | 2.0 | | velocity_time_constant | double | velocity dynamics time constant for trajectory velocity modification [s] | 0.3 | +#### Lowpass Filter for Noise Reduction + +| Name | Type | Description | Default value | +| :------------------------ | :----- | :------------------------------------------------------------------ | :------------ | +| steering_lpf_cutoff_hz | double | cutoff frequency of lowpass filter for steering output command [hz] | 3.0 | +| error_deriv_lpf_cutoff_hz | double | cutoff frequency of lowpass filter for error derivative [Hz] | 5.0 | + +#### Stop State + +| Name | Type | Description | Default value | +| :------------------------------------------- | :------ | :---------------------------------------------------------------------------------------------- | :------------ | +| stop_state_entry_ego_speed \*1 | double | threshold value of the ego vehicle speed used to the stop state entry condition | 0.001 | +| stop_state_entry_target_speed \*1 | double | threshold value of the target speed used to the stop state entry condition | 0.001 | +| converged_steer_rad | double | threshold value of the steer convergence | 0.1 | +| keep_steer_control_until_converged | boolean | keep steer control until steer is converged | true | +| new_traj_duration_time | double | threshold value of the time to be considered as new trajectory | 1.0 | +| new_traj_end_dist | double | threshold value of the distance between trajectory ends to be considered as new trajectory | 0.3 | +| mpc_converged_threshold_rps | double | threshold value to be sure output of the optimization is converged, it is used in stopped state | 0.01 | + +(\*1) To prevent unnecessary steering movement, the steering command is fixed to the previous value in the stop state. + +#### Steer Offset + +Defined in the `steering_offset` namespace. This logic is designed as simple as possible, with minimum design parameters. + +| Name | Type | Description | Default value | +| :---------------------------------- | :------ | :----------------------------------------------------------------------------------------------- | :------------ | +| enable_auto_steering_offset_removal | boolean | Estimate the steering offset and apply compensation | true | +| update_vel_threshold | double | If the velocity is smaller than this value, the data is not used for the offset estimation | 5.56 | +| update_steer_threshold | double | If the steering angle is larger than this value, the data is not used for the offset estimation. | 0.035 | +| average_num | int | The average of this number of data is used as a steering offset. | 1000 | +| steering_offset_limit | double | The angle limit to be applied to the offset compensation. | 0.02 | + ##### For dynamics model (WIP) | Name | Type | Description | Default value | @@ -215,10 +240,8 @@ If you want to adjust the effect only in the high-speed range, you can use `weig - `weight_steering_input`: Reduce oscillation of tracking. - `weight_steering_input_squared_vel_coeff`: Reduce oscillation of tracking in high speed range. - `weight_lat_jerk`: Reduce lateral jerk. -- `weight_terminal_lat_error`: Preferable to set a higher value than normal lateral weight `weight_lat_error` for - stability. -- `weight_terminal_heading_error`: Preferable to set a higher value than normal heading weight `weight_heading_error` - for stability. +- `weight_terminal_lat_error`: Preferable to set a higher value than normal lateral weight `weight_lat_error` for stability. +- `weight_terminal_heading_error`: Preferable to set a higher value than normal heading weight `weight_heading_error` for stability. #### Other tips for tuning diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc.hpp index 0854340ed24a5..186481e92cdc4 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc.hpp @@ -447,7 +447,8 @@ class MPC * @param param Trajectory filtering parameters. */ void setReferenceTrajectory( - const Trajectory & trajectory_msg, const TrajectoryFilteringParam & param); + const Trajectory & trajectory_msg, const TrajectoryFilteringParam & param, + const Odometry & current_kinematics); /** * @brief Reset the previous result of MPC. diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_lateral_controller.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_lateral_controller.hpp index ee33062854ab9..441101abfc243 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_lateral_controller.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_lateral_controller.hpp @@ -52,11 +52,13 @@ using tier4_debug_msgs::msg::Float32Stamped; class MpcLateralController : public trajectory_follower::LateralControllerBase { public: + /// \param node Reference to the node used only for the component and parameter initialization. explicit MpcLateralController(rclcpp::Node & node); virtual ~MpcLateralController(); private: - rclcpp::Node * node_; + rclcpp::Clock::SharedPtr clock_; + rclcpp::Logger logger_; rclcpp::Publisher::SharedPtr m_pub_predicted_traj; rclcpp::Publisher::SharedPtr m_pub_debug_values; @@ -144,23 +146,27 @@ class MpcLateralController : public trajectory_follower::LateralControllerBase * @param wheelbase Vehicle's wheelbase. * @param steer_lim Steering command limit. * @param steer_tau Steering time constant. + * @param node Reference to the node. * @return Pointer to the created vehicle model. */ std::shared_ptr createVehicleModel( - const double wheelbase, const double steer_lim, const double steer_tau); + const double wheelbase, const double steer_lim, const double steer_tau, rclcpp::Node & node); /** * @brief Create the quadratic problem solver interface. + * @param node Reference to the node. * @return Pointer to the created QP solver interface. */ - std::shared_ptr createQPSolverInterface(); + std::shared_ptr createQPSolverInterface(rclcpp::Node & node); /** * @brief Create the steering offset estimator for offset compensation. * @param wheelbase Vehicle's wheelbase. + * @param node Reference to the node. * @return Pointer to the created steering offset estimator. */ - std::shared_ptr createSteerOffsetEstimator(const double wheelbase); + std::shared_ptr createSteerOffsetEstimator( + const double wheelbase, rclcpp::Node & node); /** * @brief Check if all necessary data is received and ready to run the control. @@ -181,7 +187,7 @@ class MpcLateralController : public trajectory_follower::LateralControllerBase * @brief Set the current trajectory using the received message. * @param msg Received trajectory message. */ - void setTrajectory(const Trajectory & msg); + void setTrajectory(const Trajectory & msg, const Odometry & current_kinematics); /** * @brief Check if the received data is valid. @@ -250,8 +256,9 @@ class MpcLateralController : public trajectory_follower::LateralControllerBase /** * @brief Declare MPC parameters as ROS parameters to allow tuning on the fly. + * @param node Reference to the node. */ - void declareMPCparameters(); + void declareMPCparameters(rclcpp::Node & node); /** * @brief Callback function called when parameters are changed outside of the node. @@ -264,13 +271,13 @@ class MpcLateralController : public trajectory_follower::LateralControllerBase template inline void info_throttle(Args &&... args) { - RCLCPP_INFO_THROTTLE(node_->get_logger(), *node_->get_clock(), 5000, args...); + RCLCPP_INFO_THROTTLE(logger_, *clock_, 5000, args...); } template inline void warn_throttle(Args &&... args) { - RCLCPP_WARN_THROTTLE(node_->get_logger(), *node_->get_clock(), 5000, args...); + RCLCPP_WARN_THROTTLE(logger_, *clock_, 5000, args...); } }; } // namespace autoware::motion::control::mpc_lateral_controller diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_utils.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_utils.hpp index 89136f294aa18..036e6a32a9b83 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_utils.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_utils.hpp @@ -99,7 +99,8 @@ void calcMPCTrajectoryArcLength(const MPCTrajectory & trajectory, std::vector resampleMPCTrajectoryByDistance( - const MPCTrajectory & input, const double resample_interval_dist); + const MPCTrajectory & input, const double resample_interval_dist, const size_t nearest_seg_idx, + const double ego_offset_to_segment); /** * @brief linearly interpolate the given trajectory assuming a base indexing and a new desired @@ -122,14 +123,14 @@ bool calcMPCTrajectoryTime(MPCTrajectory & traj); /** * @brief recalculate the velocity field (vx) of the MPCTrajectory with dynamic smoothing - * @param [in] start_idx index of the trajectory point from which to start smoothing - * @param [in] start_vel initial velocity to set at the start_idx + * @param [in] start_seg_idx segment index of the trajectory point from which to start smoothing + * @param [in] start_vel initial velocity to set at the start_seg_idx * @param [in] acc_lim limit on the acceleration * @param [in] tau constant to control the smoothing (high-value = very smooth) * @param [inout] traj MPCTrajectory for which to calculate the smoothed velocity */ void dynamicSmoothingVelocity( - const size_t start_idx, const double start_vel, const double acc_lim, const double tau, + const size_t start_seg_idx, const double start_vel, const double acc_lim, const double tau, MPCTrajectory & traj); /** diff --git a/control/mpc_lateral_controller/src/mpc.cpp b/control/mpc_lateral_controller/src/mpc.cpp index 643f8a6f91023..0d7aa8aa8b2e6 100644 --- a/control/mpc_lateral_controller/src/mpc.cpp +++ b/control/mpc_lateral_controller/src/mpc.cpp @@ -14,8 +14,10 @@ #include "mpc_lateral_controller/mpc.hpp" -#include "motion_utils/motion_utils.hpp" +#include "interpolation/linear_interpolation.hpp" +#include "motion_utils/trajectory/trajectory.hpp" #include "mpc_lateral_controller/mpc_utils.hpp" +#include "tier4_autoware_utils/math/unit_conversion.hpp" #include #include @@ -173,13 +175,20 @@ Float32MultiArrayStamped MPC::generateDiagData( } void MPC::setReferenceTrajectory( - const Trajectory & trajectory_msg, const TrajectoryFilteringParam & param) + const Trajectory & trajectory_msg, const TrajectoryFilteringParam & param, + const Odometry & current_kinematics) { + const size_t nearest_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + trajectory_msg.points, current_kinematics.pose.pose, ego_nearest_dist_threshold, + ego_nearest_yaw_threshold); + const double ego_offset_to_segment = motion_utils::calcLongitudinalOffsetToSegment( + trajectory_msg.points, nearest_seg_idx, current_kinematics.pose.pose.position); + const auto mpc_traj_raw = MPCUtils::convertToMPCTrajectory(trajectory_msg); // resampling - const auto [success_resample, mpc_traj_resampled] = - MPCUtils::resampleMPCTrajectoryByDistance(mpc_traj_raw, param.traj_resample_dist); + const auto [success_resample, mpc_traj_resampled] = MPCUtils::resampleMPCTrajectoryByDistance( + mpc_traj_raw, param.traj_resample_dist, nearest_seg_idx, ego_offset_to_segment); if (!success_resample) { warn_throttle("[setReferenceTrajectory] spline error when resampling by distance"); return; @@ -389,13 +398,13 @@ MPCTrajectory MPC::applyVelocityDynamicsFilter( return input; } - const size_t nearest_idx = motion_utils::findFirstNearestIndexWithSoftConstraints( + const size_t nearest_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( autoware_traj.points, current_kinematics.pose.pose, ego_nearest_dist_threshold, ego_nearest_yaw_threshold); MPCTrajectory output = input; MPCUtils::dynamicSmoothingVelocity( - nearest_idx, current_kinematics.twist.twist.linear.x, m_param.acceleration_limit, + nearest_seg_idx, current_kinematics.twist.twist.linear.x, m_param.acceleration_limit, m_param.velocity_time_constant, output); auto last_point = output.back(); diff --git a/control/mpc_lateral_controller/src/mpc_lateral_controller.cpp b/control/mpc_lateral_controller/src/mpc_lateral_controller.cpp index af680b5050037..3d19104112a71 100644 --- a/control/mpc_lateral_controller/src/mpc_lateral_controller.cpp +++ b/control/mpc_lateral_controller/src/mpc_lateral_controller.cpp @@ -14,7 +14,7 @@ #include "mpc_lateral_controller/mpc_lateral_controller.hpp" -#include "motion_utils/motion_utils.hpp" +#include "motion_utils/trajectory/trajectory.hpp" #include "mpc_lateral_controller/qp_solver/qp_solver_osqp.hpp" #include "mpc_lateral_controller/qp_solver/qp_solver_unconstraint_fast.hpp" #include "mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_dynamics.hpp" @@ -35,13 +35,14 @@ namespace autoware::motion::control::mpc_lateral_controller { -MpcLateralController::MpcLateralController(rclcpp::Node & node) : node_{&node} +MpcLateralController::MpcLateralController(rclcpp::Node & node) +: clock_(node.get_clock()), logger_(node.get_logger().get_child("lateral_controller")) { - const auto dp_int = [&](const std::string & s) { return node_->declare_parameter(s); }; - const auto dp_bool = [&](const std::string & s) { return node_->declare_parameter(s); }; - const auto dp_double = [&](const std::string & s) { return node_->declare_parameter(s); }; + const auto dp_int = [&](const std::string & s) { return node.declare_parameter(s); }; + const auto dp_bool = [&](const std::string & s) { return node.declare_parameter(s); }; + const auto dp_double = [&](const std::string & s) { return node.declare_parameter(s); }; - m_mpc.m_ctrl_period = node_->get_parameter("ctrl_period").as_double(); + m_mpc.m_ctrl_period = node.get_parameter("ctrl_period").as_double(); auto & p_filt = m_trajectory_filtering_param; p_filt.enable_path_smoothing = dp_bool("enable_path_smoothing"); @@ -66,16 +67,16 @@ MpcLateralController::MpcLateralController(rclcpp::Node & node) : node_{&node} m_mpc_converged_threshold_rps = dp_double("mpc_converged_threshold_rps"); // [rad/s] /* mpc parameters */ - const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(*node_).getVehicleInfo(); + const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo(); const double wheelbase = vehicle_info.wheel_base_m; constexpr double deg2rad = static_cast(M_PI) / 180.0; m_mpc.m_steer_lim = vehicle_info.max_steer_angle_rad; // steer rate limit depending on curvature const auto steer_rate_lim_dps_list_by_curvature = - node_->declare_parameter>("steer_rate_lim_dps_list_by_curvature"); + node.declare_parameter>("steer_rate_lim_dps_list_by_curvature"); const auto curvature_list_for_steer_rate_lim = - node_->declare_parameter>("curvature_list_for_steer_rate_lim"); + node.declare_parameter>("curvature_list_for_steer_rate_lim"); for (size_t i = 0; i < steer_rate_lim_dps_list_by_curvature.size(); ++i) { m_mpc.m_steer_rate_lim_map_by_curvature.emplace_back( curvature_list_for_steer_rate_lim.at(i), @@ -84,9 +85,9 @@ MpcLateralController::MpcLateralController(rclcpp::Node & node) : node_{&node} // steer rate limit depending on velocity const auto steer_rate_lim_dps_list_by_velocity = - node_->declare_parameter>("steer_rate_lim_dps_list_by_velocity"); + node.declare_parameter>("steer_rate_lim_dps_list_by_velocity"); const auto velocity_list_for_steer_rate_lim = - node_->declare_parameter>("velocity_list_for_steer_rate_lim"); + node.declare_parameter>("velocity_list_for_steer_rate_lim"); for (size_t i = 0; i < steer_rate_lim_dps_list_by_velocity.size(); ++i) { m_mpc.m_steer_rate_lim_map_by_velocity.emplace_back( velocity_list_for_steer_rate_lim.at(i), steer_rate_lim_dps_list_by_velocity.at(i) * deg2rad); @@ -94,12 +95,12 @@ MpcLateralController::MpcLateralController(rclcpp::Node & node) : node_{&node} /* vehicle model setup */ auto vehicle_model_ptr = - createVehicleModel(wheelbase, m_mpc.m_steer_lim, m_mpc.m_param.steer_tau); + createVehicleModel(wheelbase, m_mpc.m_steer_lim, m_mpc.m_param.steer_tau, node); m_mpc.setVehicleModel(vehicle_model_ptr); /* QP solver setup */ m_mpc.setVehicleModel(vehicle_model_ptr); - auto qpsolver_ptr = createQPSolverInterface(); + auto qpsolver_ptr = createQPSolverInterface(node); m_mpc.setQPSolver(qpsolver_ptr); /* delay compensation */ @@ -113,7 +114,7 @@ MpcLateralController::MpcLateralController(rclcpp::Node & node) : node_{&node} /* steering offset compensation */ enable_auto_steering_offset_removal_ = dp_bool("steering_offset.enable_auto_steering_offset_removal"); - steering_offset_ = createSteerOffsetEstimator(wheelbase); + steering_offset_ = createSteerOffsetEstimator(wheelbase, node); /* initialize low-pass filter */ { @@ -124,30 +125,29 @@ MpcLateralController::MpcLateralController(rclcpp::Node & node) : node_{&node} // ego nearest index search const auto check_and_get_param = [&](const auto & param) { - return node_->has_parameter(param) ? node_->get_parameter(param).as_double() : dp_double(param); + return node.has_parameter(param) ? node.get_parameter(param).as_double() : dp_double(param); }; m_ego_nearest_dist_threshold = check_and_get_param("ego_nearest_dist_threshold"); m_ego_nearest_yaw_threshold = check_and_get_param("ego_nearest_yaw_threshold"); m_mpc.ego_nearest_dist_threshold = m_ego_nearest_dist_threshold; m_mpc.ego_nearest_yaw_threshold = m_ego_nearest_yaw_threshold; - m_pub_predicted_traj = node_->create_publisher("~/output/predicted_trajectory", 1); + m_pub_predicted_traj = node.create_publisher("~/output/predicted_trajectory", 1); m_pub_debug_values = - node_->create_publisher("~/output/lateral_diagnostic", 1); - m_pub_steer_offset = - node_->create_publisher("~/output/estimated_steer_offset", 1); + node.create_publisher("~/output/lateral_diagnostic", 1); + m_pub_steer_offset = node.create_publisher("~/output/estimated_steer_offset", 1); - declareMPCparameters(); + declareMPCparameters(node); /* get parameter updates */ using std::placeholders::_1; - m_set_param_res = node_->add_on_set_parameters_callback( - std::bind(&MpcLateralController::paramCallback, this, _1)); + m_set_param_res = + node.add_on_set_parameters_callback(std::bind(&MpcLateralController::paramCallback, this, _1)); m_mpc.initializeSteeringPredictor(); - m_mpc.setLogger(node_->get_logger()); - m_mpc.setClock(node_->get_clock()); + m_mpc.setLogger(logger_); + m_mpc.setClock(clock_); } MpcLateralController::~MpcLateralController() @@ -155,12 +155,11 @@ MpcLateralController::~MpcLateralController() } std::shared_ptr MpcLateralController::createVehicleModel( - const double wheelbase, const double steer_lim, const double steer_tau) + const double wheelbase, const double steer_lim, const double steer_tau, rclcpp::Node & node) { std::shared_ptr vehicle_model_ptr; - const std::string vehicle_model_type = - node_->declare_parameter("vehicle_model_type"); + const std::string vehicle_model_type = node.declare_parameter("vehicle_model_type"); if (vehicle_model_type == "kinematics") { vehicle_model_ptr = std::make_shared(wheelbase, steer_lim, steer_tau); @@ -173,12 +172,12 @@ std::shared_ptr MpcLateralController::createVehicleModel( } if (vehicle_model_type == "dynamics") { - const double mass_fl = node_->declare_parameter("vehicle.mass_fl"); - const double mass_fr = node_->declare_parameter("vehicle.mass_fr"); - const double mass_rl = node_->declare_parameter("vehicle.mass_rl"); - const double mass_rr = node_->declare_parameter("vehicle.mass_rr"); - const double cf = node_->declare_parameter("vehicle.cf"); - const double cr = node_->declare_parameter("vehicle.cr"); + const double mass_fl = node.declare_parameter("vehicle.mass_fl"); + const double mass_fr = node.declare_parameter("vehicle.mass_fr"); + const double mass_rl = node.declare_parameter("vehicle.mass_rl"); + const double mass_rr = node.declare_parameter("vehicle.mass_rr"); + const double cf = node.declare_parameter("vehicle.cf"); + const double cr = node.declare_parameter("vehicle.cr"); // vehicle_model_ptr is only assigned in ctor, so parameter value have to be passed at init time vehicle_model_ptr = @@ -186,15 +185,16 @@ std::shared_ptr MpcLateralController::createVehicleModel( return vehicle_model_ptr; } - RCLCPP_ERROR(node_->get_logger(), "vehicle_model_type is undefined"); + RCLCPP_ERROR(logger_, "vehicle_model_type is undefined"); return vehicle_model_ptr; } -std::shared_ptr MpcLateralController::createQPSolverInterface() +std::shared_ptr MpcLateralController::createQPSolverInterface( + rclcpp::Node & node) { std::shared_ptr qpsolver_ptr; - const std::string qp_solver_type = node_->declare_parameter("qp_solver_type"); + const std::string qp_solver_type = node.declare_parameter("qp_solver_type"); if (qp_solver_type == "unconstraint_fast") { qpsolver_ptr = std::make_shared(); @@ -202,22 +202,22 @@ std::shared_ptr MpcLateralController::createQPSolverInterface } if (qp_solver_type == "osqp") { - qpsolver_ptr = std::make_shared(node_->get_logger()); + qpsolver_ptr = std::make_shared(logger_); return qpsolver_ptr; } - RCLCPP_ERROR(node_->get_logger(), "qp_solver_type is undefined"); + RCLCPP_ERROR(logger_, "qp_solver_type is undefined"); return qpsolver_ptr; } std::shared_ptr MpcLateralController::createSteerOffsetEstimator( - const double wheelbase) + const double wheelbase, rclcpp::Node & node) { const std::string ns = "steering_offset."; - const auto vel_thres = node_->declare_parameter(ns + "update_vel_threshold"); - const auto steer_thres = node_->declare_parameter(ns + "update_steer_threshold"); - const auto limit = node_->declare_parameter(ns + "steering_offset_limit"); - const auto num = node_->declare_parameter(ns + "average_num"); + const auto vel_thres = node.declare_parameter(ns + "update_vel_threshold"); + const auto steer_thres = node.declare_parameter(ns + "update_steer_threshold"); + const auto limit = node.declare_parameter(ns + "steering_offset_limit"); + const auto num = node.declare_parameter(ns + "average_num"); steering_offset_ = std::make_shared(wheelbase, num, vel_thres, steer_thres, limit); return steering_offset_; @@ -227,7 +227,7 @@ trajectory_follower::LateralOutput MpcLateralController::run( trajectory_follower::InputData const & input_data) { // set input data - setTrajectory(input_data.current_trajectory); + setTrajectory(input_data.current_trajectory, input_data.current_odometry); m_current_kinematic_state = input_data.current_odometry; m_current_steering = input_data.current_steering; @@ -306,7 +306,7 @@ bool MpcLateralController::isSteerConverged(const AckermannLateralCommand & cmd) // wait for a while to propagate the trajectory shape to the output command when the trajectory // shape is changed. if (!m_has_received_first_trajectory || isTrajectoryShapeChanged()) { - RCLCPP_DEBUG(node_->get_logger(), "trajectory shaped is changed"); + RCLCPP_DEBUG(logger_, "trajectory shaped is changed"); return false; } @@ -319,7 +319,7 @@ bool MpcLateralController::isSteerConverged(const AckermannLateralCommand & cmd) bool MpcLateralController::isReady(const trajectory_follower::InputData & input_data) { - setTrajectory(input_data.current_trajectory); + setTrajectory(input_data.current_trajectory, input_data.current_odometry); m_current_kinematic_state = input_data.current_odometry; m_current_steering = input_data.current_steering; @@ -339,21 +339,22 @@ bool MpcLateralController::isReady(const trajectory_follower::InputData & input_ return true; } -void MpcLateralController::setTrajectory(const Trajectory & msg) +void MpcLateralController::setTrajectory( + const Trajectory & msg, const Odometry & current_kinematics) { m_current_trajectory = msg; if (msg.points.size() < 3) { - RCLCPP_DEBUG(node_->get_logger(), "received path size is < 3, not enough."); + RCLCPP_DEBUG(logger_, "received path size is < 3, not enough."); return; } if (!isValidTrajectory(msg)) { - RCLCPP_ERROR(node_->get_logger(), "Trajectory is invalid!! stop computing."); + RCLCPP_ERROR(logger_, "Trajectory is invalid!! stop computing."); return; } - m_mpc.setReferenceTrajectory(msg, m_trajectory_filtering_param); + m_mpc.setReferenceTrajectory(msg, m_trajectory_filtering_param, current_kinematics); // update trajectory buffer to check the trajectory shape change. m_trajectory_buffer.push_back(m_current_trajectory); @@ -424,32 +425,32 @@ AckermannLateralCommand MpcLateralController::createCtrlCmdMsg( const AckermannLateralCommand & ctrl_cmd) { auto out = ctrl_cmd; - out.stamp = node_->now(); + out.stamp = clock_->now(); m_steer_cmd_prev = out.steering_tire_angle; return out; } void MpcLateralController::publishPredictedTraj(Trajectory & predicted_traj) const { - predicted_traj.header.stamp = node_->now(); + predicted_traj.header.stamp = clock_->now(); predicted_traj.header.frame_id = m_current_trajectory.header.frame_id; m_pub_predicted_traj->publish(predicted_traj); } void MpcLateralController::publishDebugValues(Float32MultiArrayStamped & debug_values) const { - debug_values.stamp = node_->now(); + debug_values.stamp = clock_->now(); m_pub_debug_values->publish(debug_values); Float32Stamped offset; - offset.stamp = node_->now(); + offset.stamp = clock_->now(); offset.data = steering_offset_->getOffset(); m_pub_steer_offset->publish(offset); } void MpcLateralController::setSteeringToHistory(const AckermannLateralCommand & steering) { - const auto time = node_->now(); + const auto time = clock_->now(); if (m_mpc_steering_history.empty()) { m_mpc_steering_history.emplace_back(steering, time); m_is_mpc_history_filled = false; @@ -501,12 +502,12 @@ bool MpcLateralController::isMpcConverged() return (max_steering_value - min_steering_value) < m_mpc_converged_threshold_rps; } -void MpcLateralController::declareMPCparameters() +void MpcLateralController::declareMPCparameters(rclcpp::Node & node) { - m_mpc.m_param.prediction_horizon = node_->declare_parameter("mpc_prediction_horizon"); - m_mpc.m_param.prediction_dt = node_->declare_parameter("mpc_prediction_dt"); + m_mpc.m_param.prediction_horizon = node.declare_parameter("mpc_prediction_horizon"); + m_mpc.m_param.prediction_dt = node.declare_parameter("mpc_prediction_dt"); - const auto dp = [&](const auto & param) { return node_->declare_parameter(param); }; + const auto dp = [&](const auto & param) { return node.declare_parameter(param); }; auto & nw = m_mpc.m_param.nominal_weight; nw.lat_error = dp("mpc_weight_lat_error"); diff --git a/control/mpc_lateral_controller/src/mpc_utils.cpp b/control/mpc_lateral_controller/src/mpc_utils.cpp index 891a48299c81d..d3fc5fba0b014 100644 --- a/control/mpc_lateral_controller/src/mpc_utils.cpp +++ b/control/mpc_lateral_controller/src/mpc_utils.cpp @@ -16,7 +16,7 @@ #include "interpolation/linear_interpolation.hpp" #include "interpolation/spline_interpolation.hpp" -#include "motion_utils/motion_utils.hpp" +#include "motion_utils/trajectory/trajectory.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" #include "tier4_autoware_utils/math/normalization.hpp" @@ -27,6 +27,19 @@ namespace autoware::motion::control::mpc_lateral_controller { +namespace +{ +double calcLongitudinalOffset( + const geometry_msgs::msg::Point & p_front, const geometry_msgs::msg::Point & p_back, + const geometry_msgs::msg::Point & p_target) +{ + const Eigen::Vector3d segment_vec{p_back.x - p_front.x, p_back.y - p_front.y, 0}; + const Eigen::Vector3d target_vec{p_target.x - p_front.x, p_target.y - p_front.y, 0}; + + return segment_vec.dot(target_vec) / segment_vec.norm(); +} +} // namespace + namespace MPCUtils { using tier4_autoware_utils::calcDistance2d; @@ -77,7 +90,8 @@ void calcMPCTrajectoryArcLength(const MPCTrajectory & trajectory, std::vector resampleMPCTrajectoryByDistance( - const MPCTrajectory & input, const double resample_interval_dist) + const MPCTrajectory & input, const double resample_interval_dist, const size_t nearest_seg_idx, + const double ego_offset_to_segment) { MPCTrajectory output; @@ -92,7 +106,18 @@ std::pair resampleMPCTrajectoryByDistance( } std::vector output_arclength; - for (double s = 0; s < input_arclength.back(); s += resample_interval_dist) { + // To accurately sample the ego point, resample separately in the forward direction and the + // backward direction from the current position. + for (double s = std::clamp( + input_arclength.at(nearest_seg_idx) + ego_offset_to_segment, 0.0, + input_arclength.back() - 1e-6); + 0 <= s; s -= resample_interval_dist) { + output_arclength.push_back(s); + } + std::reverse(output_arclength.begin(), output_arclength.end()); + for (double s = std::max(input_arclength.at(nearest_seg_idx) + ego_offset_to_segment, 0.0) + + resample_interval_dist; + s < input_arclength.back(); s += resample_interval_dist) { output_arclength.push_back(s); } @@ -274,13 +299,17 @@ bool calcMPCTrajectoryTime(MPCTrajectory & traj) } void dynamicSmoothingVelocity( - const size_t start_idx, const double start_vel, const double acc_lim, const double tau, + const size_t start_seg_idx, const double start_vel, const double acc_lim, const double tau, MPCTrajectory & traj) { double curr_v = start_vel; - traj.vx.at(start_idx) = start_vel; + // set current velocity in both start and end point of the segment + traj.vx.at(start_seg_idx) = start_vel; + if (1 < traj.vx.size()) { + traj.vx.at(start_seg_idx + 1) = start_vel; + } - for (size_t i = start_idx + 1; i < traj.size(); ++i) { + for (size_t i = start_seg_idx + 2; i < traj.size(); ++i) { const double ds = calcDistance2d(traj, i, i - 1); const double dt = ds / std::max(std::fabs(curr_v), std::numeric_limits::epsilon()); const double a = tau / std::max(tau + dt, std::numeric_limits::epsilon()); @@ -320,29 +349,40 @@ bool calcNearestPoseInterp( return true; } - auto calcSquaredDist = [](const Pose & p, const MPCTrajectory & t, const size_t idx) { - const double dx = p.position.x - t.x.at(idx); - const double dy = p.position.y - t.y.at(idx); - return dx * dx + dy * dy; - }; - /* get second nearest index = next to nearest_index */ - const size_t next = static_cast( - std::min(static_cast(*nearest_index) + 1, static_cast(traj_size) - 1)); - const size_t prev = - static_cast(std::max(static_cast(*nearest_index) - 1, static_cast(0))); - const double dist_to_next = calcSquaredDist(self_pose, traj, next); - const double dist_to_prev = calcSquaredDist(self_pose, traj, prev); - const size_t second_nearest_index = (dist_to_next < dist_to_prev) ? next : prev; - - const double a_sq = calcSquaredDist(self_pose, traj, *nearest_index); - const double b_sq = calcSquaredDist(self_pose, traj, second_nearest_index); - const double dx3 = traj.x.at(*nearest_index) - traj.x.at(second_nearest_index); - const double dy3 = traj.y.at(*nearest_index) - traj.y.at(second_nearest_index); - const double c_sq = dx3 * dx3 + dy3 * dy3; + const auto [prev, next] = [&]() -> std::pair { + if (*nearest_index == 0) { + return std::make_pair(0, 1); + } + if (*nearest_index == traj_size - 1) { + return std::make_pair(traj_size - 2, traj_size - 1); + } + geometry_msgs::msg::Point nearest_traj_point; + nearest_traj_point.x = traj.x.at(*nearest_index); + nearest_traj_point.y = traj.y.at(*nearest_index); + geometry_msgs::msg::Point next_nearest_traj_point; + next_nearest_traj_point.x = traj.x.at(*nearest_index + 1); + next_nearest_traj_point.y = traj.y.at(*nearest_index + 1); + + const double signed_length = + calcLongitudinalOffset(nearest_traj_point, next_nearest_traj_point, self_pose.position); + if (signed_length <= 0) { + return std::make_pair(*nearest_index - 1, *nearest_index); + } + return std::make_pair(*nearest_index, *nearest_index + 1); + }(); + + geometry_msgs::msg::Point next_traj_point; + next_traj_point.x = traj.x.at(next); + next_traj_point.y = traj.y.at(next); + geometry_msgs::msg::Point prev_traj_point; + prev_traj_point.x = traj.x.at(prev); + prev_traj_point.y = traj.y.at(prev); + const double traj_seg_length = + tier4_autoware_utils::calcDistance2d(prev_traj_point, next_traj_point); /* if distance between two points are too close */ - if (c_sq < 1.0E-5) { + if (traj_seg_length < 1.0E-5) { nearest_pose->position.x = traj.x.at(*nearest_index); nearest_pose->position.y = traj.y.at(*nearest_index); nearest_pose->orientation = createQuaternionFromYaw(traj.yaw.at(*nearest_index)); @@ -351,18 +391,15 @@ bool calcNearestPoseInterp( } /* linear interpolation */ - const double alpha = std::max(std::min(0.5 * (c_sq - a_sq + b_sq) / c_sq, 1.0), 0.0); - nearest_pose->position.x = - alpha * traj.x.at(*nearest_index) + (1 - alpha) * traj.x.at(second_nearest_index); - nearest_pose->position.y = - alpha * traj.y.at(*nearest_index) + (1 - alpha) * traj.y.at(second_nearest_index); - const double tmp_yaw_err = - normalizeRadian(traj.yaw.at(*nearest_index) - traj.yaw.at(second_nearest_index)); - const double nearest_yaw = - normalizeRadian(traj.yaw.at(second_nearest_index) + alpha * tmp_yaw_err); + const double ratio = std::clamp( + calcLongitudinalOffset(prev_traj_point, next_traj_point, self_pose.position) / traj_seg_length, + 0.0, 1.0); + nearest_pose->position.x = (1 - ratio) * traj.x.at(prev) + ratio * traj.x.at(next); + nearest_pose->position.y = (1 - ratio) * traj.y.at(prev) + ratio * traj.y.at(next); + const double tmp_yaw_err = normalizeRadian(traj.yaw.at(prev) - traj.yaw.at(next)); + const double nearest_yaw = normalizeRadian(traj.yaw.at(next) + (1 - ratio) * tmp_yaw_err); nearest_pose->orientation = createQuaternionFromYaw(nearest_yaw); - *nearest_time = alpha * traj.relative_time.at(*nearest_index) + - (1 - alpha) * traj.relative_time.at(second_nearest_index); + *nearest_time = (1 - ratio) * traj.relative_time.at(prev) + ratio * traj.relative_time.at(next); return true; } diff --git a/control/mpc_lateral_controller/test/test_mpc.cpp b/control/mpc_lateral_controller/test/test_mpc.cpp index 6f8a6fb598058..dade035daf26c 100644 --- a/control/mpc_lateral_controller/test/test_mpc.cpp +++ b/control/mpc_lateral_controller/test/test_mpc.cpp @@ -163,7 +163,9 @@ class MPCTest : public ::testing::Test mpc.initializeSteeringPredictor(); // Init trajectory - mpc.setReferenceTrajectory(dummy_straight_trajectory, trajectory_param); + const auto current_kinematics = + makeOdometry(dummy_straight_trajectory.points.front().pose, 0.0); + mpc.setReferenceTrajectory(dummy_straight_trajectory, trajectory_param, current_kinematics); } nav_msgs::msg::Odometry makeOdometry(const geometry_msgs::msg::Pose & pose, const double velocity) @@ -221,7 +223,9 @@ TEST_F(MPCTest, InitializeAndCalculateRightTurn) // Init parameters and reference trajectory initializeMPC(mpc); - mpc.setReferenceTrajectory(dummy_right_turn_trajectory, trajectory_param); + const auto current_kinematics = + makeOdometry(dummy_right_turn_trajectory.points.front().pose, 0.0); + mpc.setReferenceTrajectory(dummy_right_turn_trajectory, trajectory_param, current_kinematics); // Calculate MPC AckermannLateralCommand ctrl_cmd; @@ -237,7 +241,8 @@ TEST_F(MPCTest, OsqpCalculate) { MPC mpc; initializeMPC(mpc); - mpc.setReferenceTrajectory(dummy_straight_trajectory, trajectory_param); + const auto current_kinematics = makeOdometry(dummy_straight_trajectory.points.front().pose, 0.0); + mpc.setReferenceTrajectory(dummy_straight_trajectory, trajectory_param, current_kinematics); std::shared_ptr vehicle_model_ptr = std::make_shared(wheelbase, steer_limit, steer_tau); @@ -262,7 +267,9 @@ TEST_F(MPCTest, OsqpCalculateRightTurn) { MPC mpc; initializeMPC(mpc); - mpc.setReferenceTrajectory(dummy_right_turn_trajectory, trajectory_param); + const auto current_kinematics = + makeOdometry(dummy_right_turn_trajectory.points.front().pose, 0.0); + mpc.setReferenceTrajectory(dummy_right_turn_trajectory, trajectory_param, current_kinematics); std::shared_ptr vehicle_model_ptr = std::make_shared(wheelbase, steer_limit, steer_tau); @@ -300,7 +307,8 @@ TEST_F(MPCTest, KinematicsNoDelayCalculate) // Init filters mpc.initializeLowPassFilters(steering_lpf_cutoff_hz, error_deriv_lpf_cutoff_hz); // Init trajectory - mpc.setReferenceTrajectory(dummy_straight_trajectory, trajectory_param); + const auto current_kinematics = makeOdometry(dummy_straight_trajectory.points.front().pose, 0.0); + mpc.setReferenceTrajectory(dummy_straight_trajectory, trajectory_param, current_kinematics); // Calculate MPC AckermannLateralCommand ctrl_cmd; Trajectory pred_traj; @@ -315,7 +323,9 @@ TEST_F(MPCTest, KinematicsNoDelayCalculateRightTurn) { MPC mpc; initializeMPC(mpc); - mpc.setReferenceTrajectory(dummy_right_turn_trajectory, trajectory_param); + const auto current_kinematics = + makeOdometry(dummy_right_turn_trajectory.points.front().pose, 0.0); + mpc.setReferenceTrajectory(dummy_right_turn_trajectory, trajectory_param, current_kinematics); std::shared_ptr vehicle_model_ptr = std::make_shared(wheelbase, steer_limit); diff --git a/control/operation_mode_transition_manager/README.md b/control/operation_mode_transition_manager/README.md index 3bbc3a006c73a..3cb07a9bd7821 100644 --- a/control/operation_mode_transition_manager/README.md +++ b/control/operation_mode_transition_manager/README.md @@ -25,7 +25,27 @@ There is also an `In Transition` state that occurs during each mode transitions. ## Design - +A rough design of the relationship between `operation_mode_transition_manager`` and the other nodes is shown below. + +![transition_rough_structure](image/transition_rough_structure.drawio.svg) + +A more detailed structure is below. + +![transition_detailed_structure](image/transition_detailed_structure.drawio.svg) + +Here we see that `operation_mode_transition_manager` has multiple state transitions as follows + +- **AUTOWARE ENABLED <---> DISABLED** + - **ENABLED**: the vehicle is controlled by Autoware. + - **DISABLED**: the vehicle is out of Autoware control, expecting the e.g. manual driving. +- **AUTOWARE ENABLED <---> AUTO/LOCAL/REMOTE/NONE** + - **AUTO**: the vehicle is controlled by Autoware, with the autonomous control command calculated by the planning/control component. + - **LOCAL**: the vehicle is controlled by Autoware, with the locally connected operator, e.g. joystick controller. + - **REMOTE**: the vehicle is controlled by Autoware, with the remotely connected operator. + - **NONE**: the vehicle is not controlled by any operator. +- **IN TRANSITION <---> COMPLETED** + - **IN TRANSITION**: the mode listed above is in the transition process, expecting the former operator to have a responsibility to confirm the transition is completed. + - **COMPLETED**: the mode transition is completed. ## Inputs / Outputs / API @@ -63,13 +83,14 @@ For the backward compatibility (to be removed): ## Parameters -| Name | Type | Description | Default value | -| :--------------------------------- | :------- | :------------------------------------------------------------------------------------------------ | :------------ | -| `transition_timeout` | `double` | If the state transition is not completed within this time, it is considered a transition failure. | 10.0 | -| `frequency_hz` | `double` | running hz | 10.0 | -| `check_engage_condition` | `double` | If false, autonomous transition is always available | 0.1 | -| `nearest_dist_deviation_threshold` | `double` | distance threshold used to find nearest trajectory point | 3.0 | -| `nearest_yaw_deviation_threshold` | `double` | angle threshold used to find nearest trajectory point | 1.57 | +| Name | Type | Description | Default value | +| :--------------------------------- | :------- | :---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | +| `transition_timeout` | `double` | If the state transition is not completed within this time, it is considered a transition failure. | 10.0 | +| `frequency_hz` | `double` | running hz | 10.0 | +| `enable_engage_on_driving` | `bool` | Set true if you want to engage the autonomous driving mode while the vehicle is driving. If set to false, it will deny Engage in any situation where the vehicle speed is not zero. Note that if you use this feature without adjusting the parameters, it may cause issues like sudden deceleration. Before using, please ensure the engage condition and the vehicle_cmd_gate transition filter are appropriately adjusted. | 0.1 | +| `check_engage_condition` | `bool` | If false, autonomous transition is always available | 0.1 | +| `nearest_dist_deviation_threshold` | `double` | distance threshold used to find nearest trajectory point | 3.0 | +| `nearest_yaw_deviation_threshold` | `double` | angle threshold used to find nearest trajectory point | 1.57 | For `engage_acceptable_limits` related parameters: @@ -94,6 +115,21 @@ For `stable_check` related parameters: | `speed_upper_threshold` | `double` | the velocity deviation between control command and ego vehicle must be within this threshold to complete `Autonomous` transition. | 2.0 | | `speed_lower_threshold` | `double` | the velocity deviation between control command and ego vehicle must be within this threshold to complete `Autonomous` transition. | 2.0 | +## Engage check behavior on each parameter setting + +This matrix describes the scenarios in which the vehicle can be engaged based on the combinations of parameter settings: + +| `enable_engage_on_driving` | `check_engage_condition` | `allow_autonomous_in_stopped` | Scenarios where engage is permitted | +| :------------------------: | :----------------------: | :---------------------------: | :---------------------------------------------------------------- | +| x | x | x | Only when the vehicle is stationary. | +| x | x | o | Only when the vehicle is stationary. | +| x | o | x | When the vehicle is stationary and all engage conditions are met. | +| x | o | o | Only when the vehicle is stationary. | +| o | x | x | At any time (Caution: Not recommended). | +| o | x | o | At any time (Caution: Not recommended). | +| o | o | x | When all engage conditions are met, regardless of vehicle status. | +| o | o | o | When all engage conditions are met or the vehicle is stationary. | + ## Future extensions / Unimplemented parts - Need to remove backward compatibility interfaces. diff --git a/control/operation_mode_transition_manager/config/operation_mode_transition_manager.param.yaml b/control/operation_mode_transition_manager/config/operation_mode_transition_manager.param.yaml index a86443f5cabdb..67ce4b485e8c3 100644 --- a/control/operation_mode_transition_manager/config/operation_mode_transition_manager.param.yaml +++ b/control/operation_mode_transition_manager/config/operation_mode_transition_manager.param.yaml @@ -2,11 +2,16 @@ ros__parameters: transition_timeout: 10.0 frequency_hz: 10.0 + + # set true if you want to engage the autonomous driving mode while the vehicle is driving. If set to false, it will deny Engage in any situation where the vehicle speed is not zero. Note that if you use this feature without adjusting the parameters, it may cause issues like sudden deceleration. Before using, please ensure the engage condition and the vehicle_cmd_gate transition filter are appropriately adjusted. + enable_engage_on_driving: false + check_engage_condition: false # set false if you do not want to care about the engage condition. + nearest_dist_deviation_threshold: 3.0 # [m] for finding nearest index nearest_yaw_deviation_threshold: 1.57 # [rad] for finding nearest index engage_acceptable_limits: - allow_autonomous_in_stopped: true # no check if the velocity is zero, always allowed. + allow_autonomous_in_stopped: true # if true, all engage check is skipped. dist_threshold: 1.5 yaw_threshold: 0.524 speed_upper_threshold: 10.0 diff --git a/control/operation_mode_transition_manager/image/transition_detailed_structure.drawio.svg b/control/operation_mode_transition_manager/image/transition_detailed_structure.drawio.svg new file mode 100644 index 0000000000000..3492dca6dd86b --- /dev/null +++ b/control/operation_mode_transition_manager/image/transition_detailed_structure.drawio.svg @@ -0,0 +1,610 @@ + + + + + + + + + + + + +
+
+
+ Control Command +
+ (Auto) +
+
+
+ + Control Command... + + + + + + +
+
+
+ Trajectory Follower +
+
+
+ + Trajectory Follower + + + + + + + + + +
+
+
+ Control Command +
+
+
+ + Control Command + + + + + + +
+
+
+ Vehicle Cmd Gate +
+
+
+ + Vehicle Cmd Gate + + + + + + +
+
+
+ Vehicle Interface +
+
+
+ + Vehicle Interface + + + + + + + +
+
+
+ Current Mode +
+
+
+ + Current Mode + + + + + + + +
+
+
+ Auto mode +
+ Availability +
+
+
+ + Auto mode... + + + + + + + +
+
+
+ Mode Change +
+ Request +
+
+
+ + Mode Change... + + + + + + +
+
+
+ Operation Mode +
+ Transition Manager +
+
+
+ + Operation Mode... + + + + + + + +
+
+
+ Mode Change +
+ Request +
+
+
+ + Mode Change... + + + + + + +
+
+
+ API +
+
+
+ + API + + + + + + +
+
+
+ Trajectory (From Planning) +
+
+
+ + Trajectory (From Planning) + + + + + + + +
+
+
+ Mode Change +
+ Response +
+
+
+ + Mode Change... + + + + + + + + + + +
+
+
+ Ego pose, twist, acceleration (From Localization) +
+
+
+ + Ego pose, twist, accelera... + + + + + + + +
+
+
+ External Control Command +
+
+
+ + External C... + + + + + + +
+
+
+ AUTOWARE ENABLED +
+
+
+ + AUTOWARE ENABLED + + + + + + +
+
+
+ REMOTE +
+
+
+ + REMOTE + + + + + + + + + +
+
+
+ NONE +
+
+
+ + NONE + + + + + + + + + + + + +
+
+
+ AUTO +
+
+
+ + AUTO + + + + + + + + + + + + +
+
+
+ LOCAL +
+
+
+ + LOCAL + + + + + + + + + +
+
+
+ + AUTOWARE +
+ DISABLED + +
+
+
+ + AUTOWARE... + + + + + + + + + +
+
+
+ TRANSITION +
+
+
+ + TRANSITION + + + + + + +
+
+
+ IN TRANSITION +
+
+
+ + IN TRANSITION + + + + + + +
+
+
+ COMPLETED +
+
+
+ + COMPLETED + + + + + + + + + + + +
+
+
+ + Vehicle Control mode +
+ (From Vehicle) + +
+
+
+ + Vehicle Control mode... + + + + + + +
+
+
+ + Control Component + +
+
+
+ + Control Component + + + + + + +
+
+
+ External Command +
+ Selector +
+
+
+ + External Command... + + + + + + + +
+
+
+ Control Command +
+ (Local) +
+
+
+ + Control Co... + + + + + + + +
+
+
+ Control Command +
+ (Remote) +
+
+
+ + Control Command... + + + + + + + Text is not SVG - cannot display + + + diff --git a/control/operation_mode_transition_manager/image/transition_rough_structure.drawio.svg b/control/operation_mode_transition_manager/image/transition_rough_structure.drawio.svg new file mode 100644 index 0000000000000..073a2315355be --- /dev/null +++ b/control/operation_mode_transition_manager/image/transition_rough_structure.drawio.svg @@ -0,0 +1,321 @@ + + + + + + + + + + + +
+
+
+ Control Command +
+
+
+ + Control Command + + + + + + +
+
+
+ Trajectory Follower +
+
+
+ + Trajectory Follower + + + + + + + +
+
+
+ Control Command +
+
+
+ + Control Command + + + + + + +
+
+
+ Vehicle Cmd Gate +
+
+
+ + Vehicle Cmd Gate + + + + + + +
+
+
+ Vehicle Interface +
+
+
+ + Vehicle Interface + + + + + + + +
+
+
+ Current Mode +
+
+
+ + Current Mode + + + + + + + +
+
+
+ Auto mode +
+ Availability +
+
+
+ + Auto mode... + + + + + + + +
+
+
+ Mode Change +
+ Request +
+
+
+ + Mode Change... + + + + + + +
+
+
+ Operation Mode +
+ Transition Manager +
+
+
+ + Operation Mode... + + + + + + + +
+
+
+ Mode Change +
+ Request +
+
+
+ + Mode Change... + + + + + + +
+
+
+ API +
+
+
+ + API + + + + + + + + +
+
+
+ Change the control mode (e.g. Auto/Manual) for the request, follow the control command in the Auto mode +
+
+
+ + Change the control mode (e.g. Auto/Ma... + + + + + + +
+
+
+ Manage the operation mode transition (e.g. Manual/Auto). +
+ It also rejects the request based on the driving conditrion, checks the transition is complited. +
+
+
+ + Manage the operation mode transition... + + + + + + + + +
+
+
+ Manage the operation mode transition (e.g. Manual/Auto). +
+ It also rejects the request, checks the transition is complited. +
+
+
+ + Manage the operation mode transition... + + + + + + +
+
+
+ Apply filter based on the current operation mode (e.g. Manual/Auto, in the process of the transition) +
+
+
+ + Apply filter based on t... + + + + + + + + + + + Text is not SVG - cannot display + + + diff --git a/control/operation_mode_transition_manager/src/state.cpp b/control/operation_mode_transition_manager/src/state.cpp index f8434bbdb081f..d57cb8c78c740 100644 --- a/control/operation_mode_transition_manager/src/state.cpp +++ b/control/operation_mode_transition_manager/src/state.cpp @@ -16,8 +16,9 @@ #include "util.hpp" -#include -#include +#include +#include +#include #include #include @@ -37,6 +38,10 @@ AutonomousMode::AutonomousMode(rclcpp::Node * node) sub_control_cmd_ = node->create_subscription( "control_cmd", 1, [this](const AckermannControlCommand::SharedPtr msg) { control_cmd_ = *msg; }); + sub_trajectory_follower_control_cmd_ = node->create_subscription( + "trajectory_follower_control_cmd", 1, [this](const AckermannControlCommand::SharedPtr msg) { + trajectory_follower_control_cmd_ = *msg; + }); sub_kinematics_ = node->create_subscription( "kinematics", 1, [this](const Odometry::SharedPtr msg) { kinematics_ = *msg; }); @@ -45,6 +50,7 @@ AutonomousMode::AutonomousMode(rclcpp::Node * node) "trajectory", 1, [this](const Trajectory::SharedPtr msg) { trajectory_ = *msg; }); check_engage_condition_ = node->declare_parameter("check_engage_condition"); + enable_engage_on_driving_ = node->declare_parameter("enable_engage_on_driving"); nearest_dist_deviation_threshold_ = node->declare_parameter("nearest_dist_deviation_threshold"); nearest_yaw_deviation_threshold_ = @@ -121,14 +127,24 @@ bool AutonomousMode::isModeChangeCompleted() const auto closest_point = trajectory_.points.at(*closest_idx); // check for lateral deviation - const auto dist_deviation = calcDistance2d(closest_point.pose, kinematics_.pose.pose); + const auto dist_deviation = + motion_utils::calcLateralOffset(trajectory_.points, kinematics_.pose.pose.position); + if (std::isnan(dist_deviation)) { + RCLCPP_INFO(logger_, "Not stable yet: lateral offset calculation failed."); + return unstable(); + } if (dist_deviation > stable_check_param_.dist_threshold) { RCLCPP_INFO(logger_, "Not stable yet: distance deviation is too large: %f", dist_deviation); return unstable(); } // check for yaw deviation - const auto yaw_deviation = calcYawDeviation(closest_point.pose, kinematics_.pose.pose); + const auto yaw_deviation = + motion_utils::calcYawDeviation(trajectory_.points, kinematics_.pose.pose); + if (std::isnan(yaw_deviation)) { + RCLCPP_INFO(logger_, "Not stable yet: lateral offset calculation failed."); + return unstable(); + } if (yaw_deviation > stable_check_param_.yaw_threshold) { RCLCPP_INFO(logger_, "Not stable yet: yaw deviation is too large: %f", yaw_deviation); return unstable(); @@ -207,6 +223,15 @@ bool AutonomousMode::isModeChangeAvailable() const auto target_control_speed = control_cmd_.longitudinal.speed; const auto & param = engage_acceptable_param_; + if (!enable_engage_on_driving_ && std::fabs(current_speed) > 1.0e-2) { + RCLCPP_INFO( + logger_, + "Engage unavailable: enable_engage_on_driving is false, and the vehicle is not " + "stationary."); + debug_info_ = DebugInfo{}; // all false + return false; + } + if (trajectory_.points.size() < 2) { RCLCPP_WARN_SKIPFIRST_THROTTLE( logger_, *clock_, 5000, "Engage unavailable: trajectory size must be > 2"); @@ -240,7 +265,10 @@ bool AutonomousMode::isModeChangeAvailable() const bool speed_lower_deviation_ok = speed_deviation >= param.speed_lower_threshold; // No engagement if the vehicle is moving but the target speed is zero. - const bool stop_ok = !(std::abs(current_speed) > 0.1 && std::abs(target_control_speed) < 0.01); + const bool is_stop_cmd_indicated = + std::abs(target_control_speed) < 0.01 || + std::abs(trajectory_follower_control_cmd_.longitudinal.speed) < 0.01; + const bool stop_ok = !(std::abs(current_speed) > 0.1 && is_stop_cmd_indicated); // No engagement if the large acceleration is commanded. const bool large_acceleration_ok = !hasDangerAcceleration(); diff --git a/control/operation_mode_transition_manager/src/state.hpp b/control/operation_mode_transition_manager/src/state.hpp index 9d384857bbe3d..e10d64e367f8d 100644 --- a/control/operation_mode_transition_manager/src/state.hpp +++ b/control/operation_mode_transition_manager/src/state.hpp @@ -67,17 +67,20 @@ class AutonomousMode : public ModeChangeBase using Odometry = nav_msgs::msg::Odometry; using Trajectory = autoware_auto_planning_msgs::msg::Trajectory; rclcpp::Subscription::SharedPtr sub_control_cmd_; + rclcpp::Subscription::SharedPtr sub_trajectory_follower_control_cmd_; rclcpp::Subscription::SharedPtr sub_kinematics_; rclcpp::Subscription::SharedPtr sub_trajectory_; rclcpp::Logger logger_; rclcpp::Clock::SharedPtr clock_; bool check_engage_condition_ = true; // if false, the vehicle is engaged without any checks. + bool enable_engage_on_driving_ = false; // if false, engage is not permited on driving double nearest_dist_deviation_threshold_; // [m] for finding nearest index double nearest_yaw_deviation_threshold_; // [rad] for finding nearest index EngageAcceptableParam engage_acceptable_param_; StableCheckParam stable_check_param_; AckermannControlCommand control_cmd_; + AckermannControlCommand trajectory_follower_control_cmd_; Odometry kinematics_; Trajectory trajectory_; vehicle_info_util::VehicleInfo vehicle_info_; diff --git a/control/pid_longitudinal_controller/README.md b/control/pid_longitudinal_controller/README.md index 418b3f7d27220..9d25322c793e0 100644 --- a/control/pid_longitudinal_controller/README.md +++ b/control/pid_longitudinal_controller/README.md @@ -75,6 +75,10 @@ For instance, if the vehicle is attempting to start with an acceleration of `1.0 A suitable example of a vehicle system for the slope compensation function is one in which the output acceleration from the longitudinal_controller is converted into target accel/brake pedal input without any feedbacks. In this case, the output acceleration is just used as a feedforward term to calculate the target pedal, and hence the issue mentioned above does not arise. +Note: The angle of the slope is defined as positive for an uphill slope, while the pitch angle of the ego pose is defined as negative when facing upward. They have an opposite definition. + +![slope_definition](./media/slope_definition.drawio.svg) + #### PID control For deviations that cannot be handled by FeedForward control, such as model errors, PID control is used to construct a feedback system. diff --git a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/longitudinal_controller_utils.hpp b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/longitudinal_controller_utils.hpp index 8685d6264993b..9c01f7bc26c4b 100644 --- a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/longitudinal_controller_utils.hpp +++ b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/longitudinal_controller_utils.hpp @@ -66,11 +66,6 @@ double getPitchByPose(const Quaternion & quaternion); double getPitchByTraj( const Trajectory & trajectory, const size_t closest_idx, const double wheel_base); -/** - * @brief calculate elevation angle - */ -double calcElevationAngle(const TrajectoryPoint & p_from, const TrajectoryPoint & p_to); - /** * @brief calculate vehicle pose after time delay by moving the vehicle at current velocity and * acceleration for delayed time diff --git a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid_longitudinal_controller.hpp b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid_longitudinal_controller.hpp index c2dbc67f011dc..921cd3995f6a9 100644 --- a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid_longitudinal_controller.hpp +++ b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid_longitudinal_controller.hpp @@ -56,6 +56,7 @@ namespace trajectory_follower = ::autoware::motion::control::trajectory_follower class PidLongitudinalController : public trajectory_follower::LongitudinalControllerBase { public: + /// \param node Reference to the node used only for the component and parameter initialization. explicit PidLongitudinalController(rclcpp::Node & node); private: @@ -77,7 +78,9 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro double slope_angle{0.0}; double dt{0.0}; }; - rclcpp::Node * node_; + rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters_; + rclcpp::Clock::SharedPtr clock_; + rclcpp::Logger logger_; // ros variables rclcpp::Publisher::SharedPtr m_pub_slope; rclcpp::Publisher::SharedPtr m_pub_debug; @@ -202,7 +205,7 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro // debug values DebugValues m_debug_values; - std::shared_ptr m_last_running_time{std::make_shared(node_->now())}; + std::shared_ptr m_last_running_time{std::make_shared(clock_->now())}; // Diagnostic @@ -362,7 +365,7 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro * @param [in] current_vel current velocity of the vehicle */ double applyVelocityFeedback( - const Motion target_motion, const double dt, const double current_vel); + const Motion target_motion, const double dt, const double current_vel, const Shift & shift); /** * @brief update variables for debugging about pitch diff --git a/control/pid_longitudinal_controller/media/slope_definition.drawio.svg b/control/pid_longitudinal_controller/media/slope_definition.drawio.svg new file mode 100644 index 0000000000000..7f0aa677629a3 --- /dev/null +++ b/control/pid_longitudinal_controller/media/slope_definition.drawio.svg @@ -0,0 +1,165 @@ + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ Slope is defined as positive for an uphill +
+
+
+ + Slope is defined as positive for an uphi... + + + + + + + + + + + + + + + +
+
+
+ (Pitch of ego is defined as negative when facing upward) +
+
+
+ + (Pitch of ego is defined as negative when facing upwar... + + + + + + +
+
+
+ x +
+
+
+ + x + + + + + + +
+
+
+ y +
+
+
+ + z + + + + + + +
+
+
+ z +
+
+
+ + y + + + + + + +
+
+
+ d_z +
+
+
+ + d_z + + + + + + +
+
+
+ d_xy +
+
+
+ + d_xy + + + + + + + Text is not SVG - cannot display + + + diff --git a/control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp b/control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp index 70cdbcaf17bd2..9791a1f0b5e3e 100644 --- a/control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp +++ b/control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp @@ -91,41 +91,22 @@ double getPitchByTraj( return 0.0; } - for (size_t i = nearest_idx + 1; i < trajectory.points.size(); ++i) { - const double dist = tier4_autoware_utils::calcDistance2d( - trajectory.points.at(nearest_idx), trajectory.points.at(i)); - if (dist > wheel_base) { - // calculate pitch from trajectory between rear wheel (nearest) and front center (i) - return calcElevationAngle(trajectory.points.at(nearest_idx), trajectory.points.at(i)); + const auto [prev_idx, next_idx] = [&]() { + for (size_t i = nearest_idx + 1; i < trajectory.points.size(); ++i) { + const double dist = tier4_autoware_utils::calcDistance2d( + trajectory.points.at(nearest_idx), trajectory.points.at(i)); + if (dist > wheel_base) { + // calculate pitch from trajectory between rear wheel (nearest) and front center (i) + return std::make_pair(nearest_idx, i); + } } - } - - // close to goal - for (size_t i = trajectory.points.size() - 1; i > 0; --i) { - const double dist = - tier4_autoware_utils::calcDistance2d(trajectory.points.back(), trajectory.points.at(i)); - - if (dist > wheel_base) { - // calculate pitch from trajectory - // between wheelbase behind the end of trajectory (i) and the end of trajectory (back) - return calcElevationAngle(trajectory.points.at(i), trajectory.points.back()); - } - } + // NOTE: The ego pose is close to the goal. + return std::make_pair( + std::min(nearest_idx, trajectory.points.size() - 2), trajectory.points.size() - 1); + }(); - // calculate pitch from trajectory between the beginning and end of trajectory - return calcElevationAngle(trajectory.points.at(0), trajectory.points.back()); -} - -double calcElevationAngle(const TrajectoryPoint & p_from, const TrajectoryPoint & p_to) -{ - const double dx = p_from.pose.position.x - p_to.pose.position.x; - const double dy = p_from.pose.position.y - p_to.pose.position.y; - const double dz = p_from.pose.position.z - p_to.pose.position.z; - - const double dxy = std::max(std::hypot(dx, dy), std::numeric_limits::epsilon()); - const double pitch = std::atan2(dz, dxy); - - return pitch; + return tier4_autoware_utils::calcElevationAngle( + trajectory.points.at(prev_idx).pose.position, trajectory.points.at(next_idx).pose.position); } Pose calcPoseAfterTimeDelay( diff --git a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp index 337a63bc7dc76..a134fd775155b 100644 --- a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp +++ b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp @@ -14,8 +14,9 @@ #include "pid_longitudinal_controller/pid_longitudinal_controller.hpp" -#include "motion_utils/motion_utils.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" +#include "motion_utils/trajectory/trajectory.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" +#include "tier4_autoware_utils/math/normalization.hpp" #include #include @@ -27,110 +28,110 @@ namespace autoware::motion::control::pid_longitudinal_controller { PidLongitudinalController::PidLongitudinalController(rclcpp::Node & node) -: node_{&node}, diagnostic_updater_(&node) +: node_parameters_(node.get_node_parameters_interface()), + clock_(node.get_clock()), + logger_(node.get_logger().get_child("longitudinal_controller")), + diagnostic_updater_(&node) { using std::placeholders::_1; // parameters timer - m_longitudinal_ctrl_period = node_->get_parameter("ctrl_period").as_double(); + m_longitudinal_ctrl_period = node.get_parameter("ctrl_period").as_double(); - m_wheel_base = vehicle_info_util::VehicleInfoUtil(*node_).getVehicleInfo().wheel_base_m; + m_wheel_base = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo().wheel_base_m; // parameters for delay compensation - m_delay_compensation_time = node_->declare_parameter("delay_compensation_time"); // [s] + m_delay_compensation_time = node.declare_parameter("delay_compensation_time"); // [s] // parameters to enable functions - m_enable_smooth_stop = node_->declare_parameter("enable_smooth_stop"); - m_enable_overshoot_emergency = node_->declare_parameter("enable_overshoot_emergency"); + m_enable_smooth_stop = node.declare_parameter("enable_smooth_stop"); + m_enable_overshoot_emergency = node.declare_parameter("enable_overshoot_emergency"); m_enable_large_tracking_error_emergency = - node_->declare_parameter("enable_large_tracking_error_emergency"); - m_enable_slope_compensation = node_->declare_parameter("enable_slope_compensation"); + node.declare_parameter("enable_large_tracking_error_emergency"); + m_enable_slope_compensation = node.declare_parameter("enable_slope_compensation"); m_enable_keep_stopped_until_steer_convergence = - node_->declare_parameter("enable_keep_stopped_until_steer_convergence"); + node.declare_parameter("enable_keep_stopped_until_steer_convergence"); // parameters for state transition { auto & p = m_state_transition_params; // drive - p.drive_state_stop_dist = node_->declare_parameter("drive_state_stop_dist"); // [m] + p.drive_state_stop_dist = node.declare_parameter("drive_state_stop_dist"); // [m] p.drive_state_offset_stop_dist = - node_->declare_parameter("drive_state_offset_stop_dist"); // [m] + node.declare_parameter("drive_state_offset_stop_dist"); // [m] // stopping - p.stopping_state_stop_dist = - node_->declare_parameter("stopping_state_stop_dist"); // [m] + p.stopping_state_stop_dist = node.declare_parameter("stopping_state_stop_dist"); // [m] p.stopped_state_entry_duration_time = - node_->declare_parameter("stopped_state_entry_duration_time"); // [s] + node.declare_parameter("stopped_state_entry_duration_time"); // [s] // stop - p.stopped_state_entry_vel = - node_->declare_parameter("stopped_state_entry_vel"); // [m/s] + p.stopped_state_entry_vel = node.declare_parameter("stopped_state_entry_vel"); // [m/s] p.stopped_state_entry_acc = - node_->declare_parameter("stopped_state_entry_acc"); // [m/s²] + node.declare_parameter("stopped_state_entry_acc"); // [m/s²] // emergency p.emergency_state_overshoot_stop_dist = - node_->declare_parameter("emergency_state_overshoot_stop_dist"); // [m] + node.declare_parameter("emergency_state_overshoot_stop_dist"); // [m] p.emergency_state_traj_trans_dev = - node_->declare_parameter("emergency_state_traj_trans_dev"); // [m] + node.declare_parameter("emergency_state_traj_trans_dev"); // [m] p.emergency_state_traj_rot_dev = - node_->declare_parameter("emergency_state_traj_rot_dev"); // [m] + node.declare_parameter("emergency_state_traj_rot_dev"); // [m] } // parameters for drive state { // initialize PID gain - const double kp{node_->declare_parameter("kp")}; - const double ki{node_->declare_parameter("ki")}; - const double kd{node_->declare_parameter("kd")}; + const double kp{node.declare_parameter("kp")}; + const double ki{node.declare_parameter("ki")}; + const double kd{node.declare_parameter("kd")}; m_pid_vel.setGains(kp, ki, kd); // initialize PID limits - const double max_pid{node_->declare_parameter("max_out")}; // [m/s^2] - const double min_pid{node_->declare_parameter("min_out")}; // [m/s^2] - const double max_p{node_->declare_parameter("max_p_effort")}; // [m/s^2] - const double min_p{node_->declare_parameter("min_p_effort")}; // [m/s^2] - const double max_i{node_->declare_parameter("max_i_effort")}; // [m/s^2] - const double min_i{node_->declare_parameter("min_i_effort")}; // [m/s^2] - const double max_d{node_->declare_parameter("max_d_effort")}; // [m/s^2] - const double min_d{node_->declare_parameter("min_d_effort")}; // [m/s^2] + const double max_pid{node.declare_parameter("max_out")}; // [m/s^2] + const double min_pid{node.declare_parameter("min_out")}; // [m/s^2] + const double max_p{node.declare_parameter("max_p_effort")}; // [m/s^2] + const double min_p{node.declare_parameter("min_p_effort")}; // [m/s^2] + const double max_i{node.declare_parameter("max_i_effort")}; // [m/s^2] + const double min_i{node.declare_parameter("min_i_effort")}; // [m/s^2] + const double max_d{node.declare_parameter("max_d_effort")}; // [m/s^2] + const double min_d{node.declare_parameter("min_d_effort")}; // [m/s^2] m_pid_vel.setLimits(max_pid, min_pid, max_p, min_p, max_i, min_i, max_d, min_d); // set lowpass filter for vel error and pitch - const double lpf_vel_error_gain{node_->declare_parameter("lpf_vel_error_gain")}; + const double lpf_vel_error_gain{node.declare_parameter("lpf_vel_error_gain")}; m_lpf_vel_error = std::make_shared(0.0, lpf_vel_error_gain); m_current_vel_threshold_pid_integrate = - node_->declare_parameter("current_vel_threshold_pid_integration"); // [m/s] + node.declare_parameter("current_vel_threshold_pid_integration"); // [m/s] m_enable_brake_keeping_before_stop = - node_->declare_parameter("enable_brake_keeping_before_stop"); // [-] - m_brake_keeping_acc = node_->declare_parameter("brake_keeping_acc"); // [m/s^2] + node.declare_parameter("enable_brake_keeping_before_stop"); // [-] + m_brake_keeping_acc = node.declare_parameter("brake_keeping_acc"); // [m/s^2] } // parameters for smooth stop state { const double max_strong_acc{ - node_->declare_parameter("smooth_stop_max_strong_acc")}; // [m/s^2] + node.declare_parameter("smooth_stop_max_strong_acc")}; // [m/s^2] const double min_strong_acc{ - node_->declare_parameter("smooth_stop_min_strong_acc")}; // [m/s^2] - const double weak_acc{node_->declare_parameter("smooth_stop_weak_acc")}; // [m/s^2] + node.declare_parameter("smooth_stop_min_strong_acc")}; // [m/s^2] + const double weak_acc{node.declare_parameter("smooth_stop_weak_acc")}; // [m/s^2] const double weak_stop_acc{ - node_->declare_parameter("smooth_stop_weak_stop_acc")}; // [m/s^2] + node.declare_parameter("smooth_stop_weak_stop_acc")}; // [m/s^2] const double strong_stop_acc{ - node_->declare_parameter("smooth_stop_strong_stop_acc")}; // [m/s^2] + node.declare_parameter("smooth_stop_strong_stop_acc")}; // [m/s^2] - const double max_fast_vel{ - node_->declare_parameter("smooth_stop_max_fast_vel")}; // [m/s] + const double max_fast_vel{node.declare_parameter("smooth_stop_max_fast_vel")}; // [m/s] const double min_running_vel{ - node_->declare_parameter("smooth_stop_min_running_vel")}; // [m/s] + node.declare_parameter("smooth_stop_min_running_vel")}; // [m/s] const double min_running_acc{ - node_->declare_parameter("smooth_stop_min_running_acc")}; // [m/s^2] + node.declare_parameter("smooth_stop_min_running_acc")}; // [m/s^2] const double weak_stop_time{ - node_->declare_parameter("smooth_stop_weak_stop_time")}; // [s] + node.declare_parameter("smooth_stop_weak_stop_time")}; // [s] const double weak_stop_dist{ - node_->declare_parameter("smooth_stop_weak_stop_dist")}; // [m] + node.declare_parameter("smooth_stop_weak_stop_dist")}; // [m] const double strong_stop_dist{ - node_->declare_parameter("smooth_stop_strong_stop_dist")}; // [m] + node.declare_parameter("smooth_stop_strong_stop_dist")}; // [m] m_smooth_stop.setParams( max_strong_acc, min_strong_acc, weak_acc, weak_stop_acc, strong_stop_acc, max_fast_vel, @@ -140,52 +141,52 @@ PidLongitudinalController::PidLongitudinalController(rclcpp::Node & node) // parameters for stop state { auto & p = m_stopped_state_params; - p.vel = node_->declare_parameter("stopped_vel"); // [m/s] - p.acc = node_->declare_parameter("stopped_acc"); // [m/s^2] - p.jerk = node_->declare_parameter("stopped_jerk"); // [m/s^3] + p.vel = node.declare_parameter("stopped_vel"); // [m/s] + p.acc = node.declare_parameter("stopped_acc"); // [m/s^2] + p.jerk = node.declare_parameter("stopped_jerk"); // [m/s^3] } // parameters for emergency state { auto & p = m_emergency_state_params; - p.vel = node_->declare_parameter("emergency_vel"); // [m/s] - p.acc = node_->declare_parameter("emergency_acc"); // [m/s^2] - p.jerk = node_->declare_parameter("emergency_jerk"); // [m/s^3] + p.vel = node.declare_parameter("emergency_vel"); // [m/s] + p.acc = node.declare_parameter("emergency_acc"); // [m/s^2] + p.jerk = node.declare_parameter("emergency_jerk"); // [m/s^3] } // parameters for acceleration limit - m_max_acc = node_->declare_parameter("max_acc"); // [m/s^2] - m_min_acc = node_->declare_parameter("min_acc"); // [m/s^2] + m_max_acc = node.declare_parameter("max_acc"); // [m/s^2] + m_min_acc = node.declare_parameter("min_acc"); // [m/s^2] // parameters for jerk limit - m_max_jerk = node_->declare_parameter("max_jerk"); // [m/s^3] - m_min_jerk = node_->declare_parameter("min_jerk"); // [m/s^3] + m_max_jerk = node.declare_parameter("max_jerk"); // [m/s^3] + m_min_jerk = node.declare_parameter("min_jerk"); // [m/s^3] // parameters for slope compensation - m_use_traj_for_pitch = node_->declare_parameter("use_trajectory_for_pitch_calculation"); - const double lpf_pitch_gain{node_->declare_parameter("lpf_pitch_gain")}; + m_use_traj_for_pitch = node.declare_parameter("use_trajectory_for_pitch_calculation"); + const double lpf_pitch_gain{node.declare_parameter("lpf_pitch_gain")}; m_lpf_pitch = std::make_shared(0.0, lpf_pitch_gain); - m_max_pitch_rad = node_->declare_parameter("max_pitch_rad"); // [rad] - m_min_pitch_rad = node_->declare_parameter("min_pitch_rad"); // [rad] + m_max_pitch_rad = node.declare_parameter("max_pitch_rad"); // [rad] + m_min_pitch_rad = node.declare_parameter("min_pitch_rad"); // [rad] // ego nearest index search m_ego_nearest_dist_threshold = - node_->has_parameter("ego_nearest_dist_threshold") - ? node_->get_parameter("ego_nearest_dist_threshold").as_double() - : node_->declare_parameter("ego_nearest_dist_threshold"); // [m] + node.has_parameter("ego_nearest_dist_threshold") + ? node.get_parameter("ego_nearest_dist_threshold").as_double() + : node.declare_parameter("ego_nearest_dist_threshold"); // [m] m_ego_nearest_yaw_threshold = - node_->has_parameter("ego_nearest_yaw_threshold") - ? node_->get_parameter("ego_nearest_yaw_threshold").as_double() - : node_->declare_parameter("ego_nearest_yaw_threshold"); // [rad] + node.has_parameter("ego_nearest_yaw_threshold") + ? node.get_parameter("ego_nearest_yaw_threshold").as_double() + : node.declare_parameter("ego_nearest_yaw_threshold"); // [rad] // subscriber, publisher - m_pub_slope = node_->create_publisher( + m_pub_slope = node.create_publisher( "~/output/slope_angle", rclcpp::QoS{1}); - m_pub_debug = node_->create_publisher( + m_pub_debug = node.create_publisher( "~/output/longitudinal_diagnostic", rclcpp::QoS{1}); // set parameter callback - m_set_param_res = node_->add_on_set_parameters_callback( + m_set_param_res = node.add_on_set_parameters_callback( std::bind(&PidLongitudinalController::paramCallback, this, _1)); // diagnostic @@ -212,14 +213,12 @@ void PidLongitudinalController::setTrajectory( const autoware_auto_planning_msgs::msg::Trajectory & msg) { if (!longitudinal_utils::isValidTrajectory(msg)) { - RCLCPP_ERROR_THROTTLE( - node_->get_logger(), *node_->get_clock(), 3000, "received invalid trajectory. ignore."); + RCLCPP_ERROR_THROTTLE(logger_, *clock_, 3000, "received invalid trajectory. ignore."); return; } if (msg.points.size() < 2) { - RCLCPP_WARN_THROTTLE( - node_->get_logger(), *node_->get_clock(), 3000, "Unexpected trajectory size < 2. Ignored."); + RCLCPP_WARN_THROTTLE(logger_, *clock_, 3000, "Unexpected trajectory size < 2. Ignored."); return; } @@ -258,22 +257,22 @@ rcl_interfaces::msg::SetParametersResult PidLongitudinalController::paramCallbac // drive state { - double kp{node_->get_parameter("kp").as_double()}; - double ki{node_->get_parameter("ki").as_double()}; - double kd{node_->get_parameter("kd").as_double()}; + double kp{node_parameters_->get_parameter("kp").as_double()}; + double ki{node_parameters_->get_parameter("ki").as_double()}; + double kd{node_parameters_->get_parameter("kd").as_double()}; update_param("kp", kp); update_param("ki", ki); update_param("kd", kd); m_pid_vel.setGains(kp, ki, kd); - double max_pid{node_->get_parameter("max_out").as_double()}; - double min_pid{node_->get_parameter("min_out").as_double()}; - double max_p{node_->get_parameter("max_p_effort").as_double()}; - double min_p{node_->get_parameter("min_p_effort").as_double()}; - double max_i{node_->get_parameter("max_i_effort").as_double()}; - double min_i{node_->get_parameter("min_i_effort").as_double()}; - double max_d{node_->get_parameter("max_d_effort").as_double()}; - double min_d{node_->get_parameter("min_d_effort").as_double()}; + double max_pid{node_parameters_->get_parameter("max_out").as_double()}; + double min_pid{node_parameters_->get_parameter("min_out").as_double()}; + double max_p{node_parameters_->get_parameter("max_p_effort").as_double()}; + double min_p{node_parameters_->get_parameter("min_p_effort").as_double()}; + double max_i{node_parameters_->get_parameter("max_i_effort").as_double()}; + double min_i{node_parameters_->get_parameter("min_i_effort").as_double()}; + double max_d{node_parameters_->get_parameter("max_d_effort").as_double()}; + double min_d{node_parameters_->get_parameter("min_d_effort").as_double()}; update_param("max_out", max_pid); update_param("min_out", min_pid); update_param("max_p_effort", max_p); @@ -289,17 +288,25 @@ rcl_interfaces::msg::SetParametersResult PidLongitudinalController::paramCallbac // stopping state { - double max_strong_acc{node_->get_parameter("smooth_stop_max_strong_acc").as_double()}; - double min_strong_acc{node_->get_parameter("smooth_stop_min_strong_acc").as_double()}; - double weak_acc{node_->get_parameter("smooth_stop_weak_acc").as_double()}; - double weak_stop_acc{node_->get_parameter("smooth_stop_weak_stop_acc").as_double()}; - double strong_stop_acc{node_->get_parameter("smooth_stop_strong_stop_acc").as_double()}; - double max_fast_vel{node_->get_parameter("smooth_stop_max_fast_vel").as_double()}; - double min_running_vel{node_->get_parameter("smooth_stop_min_running_vel").as_double()}; - double min_running_acc{node_->get_parameter("smooth_stop_min_running_acc").as_double()}; - double weak_stop_time{node_->get_parameter("smooth_stop_weak_stop_time").as_double()}; - double weak_stop_dist{node_->get_parameter("smooth_stop_weak_stop_dist").as_double()}; - double strong_stop_dist{node_->get_parameter("smooth_stop_strong_stop_dist").as_double()}; + double max_strong_acc{ + node_parameters_->get_parameter("smooth_stop_max_strong_acc").as_double()}; + double min_strong_acc{ + node_parameters_->get_parameter("smooth_stop_min_strong_acc").as_double()}; + double weak_acc{node_parameters_->get_parameter("smooth_stop_weak_acc").as_double()}; + double weak_stop_acc{node_parameters_->get_parameter("smooth_stop_weak_stop_acc").as_double()}; + double strong_stop_acc{ + node_parameters_->get_parameter("smooth_stop_strong_stop_acc").as_double()}; + double max_fast_vel{node_parameters_->get_parameter("smooth_stop_max_fast_vel").as_double()}; + double min_running_vel{ + node_parameters_->get_parameter("smooth_stop_min_running_vel").as_double()}; + double min_running_acc{ + node_parameters_->get_parameter("smooth_stop_min_running_acc").as_double()}; + double weak_stop_time{ + node_parameters_->get_parameter("smooth_stop_weak_stop_time").as_double()}; + double weak_stop_dist{ + node_parameters_->get_parameter("smooth_stop_weak_stop_dist").as_double()}; + double strong_stop_dist{ + node_parameters_->get_parameter("smooth_stop_strong_stop_dist").as_double()}; update_param("smooth_stop_max_strong_acc", max_strong_acc); update_param("smooth_stop_min_strong_acc", min_strong_acc); update_param("smooth_stop_weak_acc", weak_acc); @@ -450,7 +457,9 @@ PidLongitudinalController::ControlData PidLongitudinalController::getControlData current_pose, m_trajectory, m_ego_nearest_dist_threshold, m_ego_nearest_yaw_threshold); // pitch - const double raw_pitch = longitudinal_utils::getPitchByPose(current_pose.orientation); + // NOTE: getPitchByTraj() calculates the pitch angle as defined in + // ../media/slope_definition.drawio.svg while getPitchByPose() is not, so `raw_pitch` is reversed + const double raw_pitch = (-1.0) * longitudinal_utils::getPitchByPose(current_pose.orientation); const double traj_pitch = longitudinal_utils::getPitchByTraj(m_trajectory, control_data.nearest_idx, m_wheel_base); control_data.slope_angle = m_use_traj_for_pitch ? traj_pitch : m_lpf_pitch->filter(raw_pitch); @@ -470,8 +479,7 @@ PidLongitudinalController::Motion PidLongitudinalController::calcEmergencyCtrlCm longitudinal_utils::applyDiffLimitFilter(p.acc, m_prev_raw_ctrl_cmd.acc, dt, p.jerk); RCLCPP_ERROR_THROTTLE( - node_->get_logger(), *node_->get_clock(), 3000, "[Emergency stop] vel: %3.3f, acc: %3.3f", vel, - acc); + logger_, *clock_, 3000, "[Emergency stop] vel: %3.3f, acc: %3.3f", vel, acc); return Motion{vel, acc}; } @@ -493,14 +501,31 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d m_enable_keep_stopped_until_steer_convergence && !lateral_sync_data_.is_steer_converged; const bool stopping_condition = stop_dist < p.stopping_state_stop_dist; - if ( - std::fabs(current_vel) > p.stopped_state_entry_vel || - std::fabs(current_acc) > p.stopped_state_entry_acc) { - m_last_running_time = std::make_shared(node_->now()); + + const bool is_stopped = std::abs(current_vel) < p.stopped_state_entry_vel && + std::abs(current_acc) < p.stopped_state_entry_acc; + // Case where the ego slips in the opposite direction of the gear due to e.g. a slope is also + // considered as a stop + const bool is_not_running = [&]() { + if (control_data.shift == Shift::Forward) { + if (is_stopped || current_vel < 0.0) { + // NOTE: Stopped or moving backward + return true; + } + } else { + if (is_stopped || 0.0 < current_vel) { + // NOTE: Stopped or moving forward + return true; + } + } + return false; + }(); + if (!is_not_running) { + m_last_running_time = std::make_shared(clock_->now()); } const bool stopped_condition = m_last_running_time - ? (node_->now() - *m_last_running_time).seconds() > p.stopped_state_entry_duration_time + ? (clock_->now() - *m_last_running_time).seconds() > p.stopped_state_entry_duration_time : false; static constexpr double vel_epsilon = @@ -515,21 +540,18 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d const auto changeState = [this](const auto s) { if (s != m_control_state) { RCLCPP_DEBUG_STREAM( - node_->get_logger(), - "controller state changed: " << toStr(m_control_state) << " -> " << toStr(s)); + logger_, "controller state changed: " << toStr(m_control_state) << " -> " << toStr(s)); } m_control_state = s; return; }; const auto info_throttle = [this](const auto & s) { - RCLCPP_INFO_SKIPFIRST_THROTTLE(node_->get_logger(), *node_->get_clock(), 5000, "%s", s); + RCLCPP_INFO_SKIPFIRST_THROTTLE(logger_, *clock_, 5000, "%s", s); }; - // if current operation mode is not autonomous mode, then change state to stopped - if (m_current_operation_mode.mode != OperationModeState::AUTONOMOUS) { - return changeState(ControlState::STOPPED); - } + const bool is_under_control = m_current_operation_mode.is_autoware_control_enabled && + m_current_operation_mode.mode == OperationModeState::AUTONOMOUS; // transit state // in DRIVE state @@ -538,6 +560,12 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d return changeState(ControlState::EMERGENCY); } + if (!is_under_control && stopped_condition && keep_stopped_condition) { + // NOTE: When the ego is stopped on manual driving, since the driving state may transit to + // autonomous, keep_stopped_condition should be checked. + return changeState(ControlState::STOPPED); + } + if (m_enable_smooth_stop) { if (stopping_condition) { // predictions after input time delay @@ -604,13 +632,19 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d // in EMERGENCY state if (m_control_state == ControlState::EMERGENCY) { - if (stopped_condition && !emergency_condition) { - return changeState(ControlState::STOPPED); + if (!emergency_condition) { + if (stopped_condition) { + return changeState(ControlState::STOPPED); + } + if (!is_under_control) { + // NOTE: On manual driving, no need stopping to exit the emergency. + return changeState(ControlState::DRIVE); + } } return; } - RCLCPP_FATAL(node_->get_logger(), "invalid state found."); + RCLCPP_FATAL(logger_, "invalid state found."); return; } @@ -639,9 +673,10 @@ PidLongitudinalController::Motion PidLongitudinalController::calcCtrlCmd( m_debug_values.setValues(DebugValues::TYPE::PREDICTED_VEL, pred_vel_in_target); raw_ctrl_cmd.vel = target_motion.vel; - raw_ctrl_cmd.acc = applyVelocityFeedback(target_motion, control_data.dt, pred_vel_in_target); + raw_ctrl_cmd.acc = + applyVelocityFeedback(target_motion, control_data.dt, pred_vel_in_target, control_data.shift); RCLCPP_DEBUG( - node_->get_logger(), + logger_, "[feedback control] vel: %3.3f, acc: %3.3f, dt: %3.3f, v_curr: %3.3f, v_ref: %3.3f " "feedback_ctrl_cmd.ac: %3.3f", raw_ctrl_cmd.vel, raw_ctrl_cmd.acc, control_data.dt, current_vel, target_motion.vel, @@ -652,8 +687,8 @@ PidLongitudinalController::Motion PidLongitudinalController::calcCtrlCmd( raw_ctrl_cmd.vel = m_stopped_state_params.vel; RCLCPP_DEBUG( - node_->get_logger(), "[smooth stop]: Smooth stopping. vel: %3.3f, acc: %3.3f", - raw_ctrl_cmd.vel, raw_ctrl_cmd.acc); + logger_, "[smooth stop]: Smooth stopping. vel: %3.3f, acc: %3.3f", raw_ctrl_cmd.vel, + raw_ctrl_cmd.acc); } else if (m_control_state == ControlState::STOPPED) { // This acceleration is without slope compensation const auto & p = m_stopped_state_params; @@ -661,8 +696,7 @@ PidLongitudinalController::Motion PidLongitudinalController::calcCtrlCmd( raw_ctrl_cmd.acc = longitudinal_utils::applyDiffLimitFilter( p.acc, m_prev_raw_ctrl_cmd.acc, control_data.dt, p.jerk); - RCLCPP_DEBUG( - node_->get_logger(), "[Stopped]. vel: %3.3f, acc: %3.3f", raw_ctrl_cmd.vel, raw_ctrl_cmd.acc); + RCLCPP_DEBUG(logger_, "[Stopped]. vel: %3.3f, acc: %3.3f", raw_ctrl_cmd.vel, raw_ctrl_cmd.acc); } else if (m_control_state == ControlState::EMERGENCY) { raw_ctrl_cmd = calcEmergencyCtrlCmd(control_data.dt); } @@ -686,12 +720,12 @@ autoware_auto_control_msgs::msg::LongitudinalCommand PidLongitudinalController:: { // publish control command autoware_auto_control_msgs::msg::LongitudinalCommand cmd{}; - cmd.stamp = node_->now(); + cmd.stamp = clock_->now(); cmd.speed = static_cast(ctrl_cmd.vel); cmd.acceleration = static_cast(ctrl_cmd.acc); // store current velocity history - m_vel_hist.push_back({node_->now(), current_vel}); + m_vel_hist.push_back({clock_->now(), current_vel}); while (m_vel_hist.size() > static_cast(0.5 / m_longitudinal_ctrl_period)) { m_vel_hist.erase(m_vel_hist.begin()); } @@ -714,7 +748,7 @@ void PidLongitudinalController::publishDebugData( // publish debug values tier4_debug_msgs::msg::Float32MultiArrayStamped debug_msg{}; - debug_msg.stamp = node_->now(); + debug_msg.stamp = clock_->now(); for (const auto & v : m_debug_values.getValues()) { debug_msg.data.push_back(static_cast(v)); } @@ -722,7 +756,7 @@ void PidLongitudinalController::publishDebugData( // slope angle tier4_debug_msgs::msg::Float32MultiArrayStamped slope_msg{}; - slope_msg.stamp = node_->now(); + slope_msg.stamp = clock_->now(); slope_msg.data.push_back( static_cast(control_data.slope_angle)); m_pub_slope->publish(slope_msg); @@ -733,10 +767,10 @@ double PidLongitudinalController::getDt() double dt; if (!m_prev_control_time) { dt = m_longitudinal_ctrl_period; - m_prev_control_time = std::make_shared(node_->now()); + m_prev_control_time = std::make_shared(clock_->now()); } else { - dt = (node_->now() - *m_prev_control_time).seconds(); - *m_prev_control_time = node_->now(); + dt = (clock_->now() - *m_prev_control_time).seconds(); + *m_prev_control_time = clock_->now(); } const double max_dt = m_longitudinal_ctrl_period * 2.0; const double min_dt = m_longitudinal_ctrl_period * 0.5; @@ -785,7 +819,7 @@ void PidLongitudinalController::storeAccelCmd(const double accel) if (m_control_state == ControlState::DRIVE) { // convert format autoware_auto_control_msgs::msg::LongitudinalCommand cmd; - cmd.stamp = node_->now(); + cmd.stamp = clock_->now(); cmd.acceleration = static_cast(accel); // store published ctrl cmd @@ -799,7 +833,7 @@ void PidLongitudinalController::storeAccelCmd(const double accel) if (m_ctrl_cmd_vec.size() <= 2) { return; } - if ((node_->now() - m_ctrl_cmd_vec.at(1).stamp).seconds() > m_delay_compensation_time) { + if ((clock_->now() - m_ctrl_cmd_vec.at(1).stamp).seconds() > m_delay_compensation_time) { m_ctrl_cmd_vec.erase(m_ctrl_cmd_vec.begin()); } } @@ -813,7 +847,7 @@ double PidLongitudinalController::applySlopeCompensation( const double pitch_limited = std::min(std::max(pitch, m_min_pitch_rad), m_max_pitch_rad); // Acceleration command is always positive independent of direction (= shift) when car is running - double sign = (shift == Shift::Forward) ? -1 : (shift == Shift::Reverse ? 1 : 0); + double sign = (shift == Shift::Forward) ? 1.0 : (shift == Shift::Reverse ? -1.0 : 0); double compensated_acc = input_acc + sign * 9.81 * std::sin(pitch_limited); return compensated_acc; } @@ -886,9 +920,9 @@ double PidLongitudinalController::predictedVelocityInTargetPoint( double pred_vel = current_vel_abs; const auto past_delay_time = - node_->now() - rclcpp::Duration::from_seconds(delay_compensation_time); + clock_->now() - rclcpp::Duration::from_seconds(delay_compensation_time); for (std::size_t i = 0; i < m_ctrl_cmd_vec.size(); ++i) { - if ((node_->now() - m_ctrl_cmd_vec.at(i).stamp).seconds() < m_delay_compensation_time) { + if ((clock_->now() - m_ctrl_cmd_vec.at(i).stamp).seconds() < m_delay_compensation_time) { if (i == 0) { // size of m_ctrl_cmd_vec is less than m_delay_compensation_time pred_vel = current_vel_abs + @@ -907,7 +941,7 @@ double PidLongitudinalController::predictedVelocityInTargetPoint( const double last_acc = m_ctrl_cmd_vec.at(m_ctrl_cmd_vec.size() - 1).acceleration; const double time_to_current = - (node_->now() - m_ctrl_cmd_vec.at(m_ctrl_cmd_vec.size() - 1).stamp).seconds(); + (clock_->now() - m_ctrl_cmd_vec.at(m_ctrl_cmd_vec.size() - 1).stamp).seconds(); pred_vel += last_acc * time_to_current; // avoid to change sign of current_vel and pred_vel @@ -915,14 +949,16 @@ double PidLongitudinalController::predictedVelocityInTargetPoint( } double PidLongitudinalController::applyVelocityFeedback( - const Motion target_motion, const double dt, const double current_vel) + const Motion target_motion, const double dt, const double current_vel, const Shift & shift) { - const double current_vel_abs = std::fabs(current_vel); - const double target_vel_abs = std::fabs(target_motion.vel); - const bool is_under_control = m_current_operation_mode.mode == OperationModeState::AUTONOMOUS; + // NOTE: Acceleration command is always positive even if the ego drives backward. + const double vel_sign = (shift == Shift::Forward) ? 1.0 : (shift == Shift::Reverse ? -1.0 : 0.0); + const double diff_vel = (target_motion.vel - current_vel) * vel_sign; + const bool is_under_control = m_current_operation_mode.is_autoware_control_enabled && + m_current_operation_mode.mode == OperationModeState::AUTONOMOUS; const bool enable_integration = - (current_vel_abs > m_current_vel_threshold_pid_integrate) && is_under_control; - const double error_vel_filtered = m_lpf_vel_error->filter(target_vel_abs - current_vel_abs); + (std::abs(current_vel) > m_current_vel_threshold_pid_integrate) && is_under_control; + const double error_vel_filtered = m_lpf_vel_error->filter(diff_vel); std::vector pid_contributions(3); const double pid_acc = @@ -935,8 +971,8 @@ double PidLongitudinalController::applyVelocityFeedback( // deviation will be bigger. constexpr double ff_scale_max = 2.0; // for safety constexpr double ff_scale_min = 0.5; // for safety - const double ff_scale = - std::clamp(current_vel_abs / std::max(target_vel_abs, 0.1), ff_scale_min, ff_scale_max); + const double ff_scale = std::clamp( + std::abs(current_vel) / std::max(std::abs(target_motion.vel), 0.1), ff_scale_min, ff_scale_max); const double ff_acc = target_motion.acc * ff_scale; const double feedback_acc = ff_acc + pid_acc; diff --git a/control/pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp b/control/pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp index d9fc404ce6abe..5c7698180f82b 100644 --- a/control/pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp +++ b/control/pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp @@ -158,33 +158,6 @@ TEST(TestLongitudinalControllerUtils, getPitchByTraj) std::atan2(0.5, 1)); } -TEST(TestLongitudinalControllerUtils, calcElevationAngle) -{ - using autoware_auto_planning_msgs::msg::TrajectoryPoint; - TrajectoryPoint p_from; - p_from.pose.position.x = 0.0; - p_from.pose.position.y = 0.0; - TrajectoryPoint p_to; - p_to.pose.position.x = 1.0; - p_to.pose.position.y = 0.0; - EXPECT_DOUBLE_EQ(longitudinal_utils::calcElevationAngle(p_from, p_to), 0.0); - p_to.pose.position.x = 1.0; - p_to.pose.position.z = 1.0; - EXPECT_DOUBLE_EQ(longitudinal_utils::calcElevationAngle(p_from, p_to), -M_PI_4); - p_to.pose.position.x = -1.0; - p_to.pose.position.z = 1.0; - EXPECT_DOUBLE_EQ(longitudinal_utils::calcElevationAngle(p_from, p_to), -M_PI_4); - p_to.pose.position.x = 0.0; - p_to.pose.position.z = 1.0; - EXPECT_DOUBLE_EQ(longitudinal_utils::calcElevationAngle(p_from, p_to), -M_PI_2); - p_to.pose.position.x = 1.0; - p_to.pose.position.z = -1.0; - EXPECT_DOUBLE_EQ(longitudinal_utils::calcElevationAngle(p_from, p_to), M_PI_4); - p_to.pose.position.x = -1.0; - p_to.pose.position.z = -1.0; - EXPECT_DOUBLE_EQ(longitudinal_utils::calcElevationAngle(p_from, p_to), M_PI_4); -} - TEST(TestLongitudinalControllerUtils, calcPoseAfterTimeDelay) { using geometry_msgs::msg::Pose; diff --git a/control/predicted_path_checker/CMakeLists.txt b/control/predicted_path_checker/CMakeLists.txt new file mode 100644 index 0000000000000..cfbe95df99e74 --- /dev/null +++ b/control/predicted_path_checker/CMakeLists.txt @@ -0,0 +1,37 @@ +cmake_minimum_required(VERSION 3.14) +project(predicted_path_checker) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +find_package(Eigen3 REQUIRED) + +include_directories( + include + SYSTEM + ${Eigen3_INCLUDE_DIRS} +) + +ament_auto_add_library(predicted_path_checker SHARED + src/predicted_path_checker_node/predicted_path_checker_node.cpp + src/predicted_path_checker_node/collision_checker.cpp + src/predicted_path_checker_node/utils.cpp + src/predicted_path_checker_node/debug_marker.cpp + +) + +rclcpp_components_register_node(predicted_path_checker + PLUGIN "autoware::motion::control::predicted_path_checker::PredictedPathCheckerNode" + EXECUTABLE predicted_path_checker_node +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package( + INSTALL_TO_SHARE + launch + config +) diff --git a/control/predicted_path_checker/README.md b/control/predicted_path_checker/README.md new file mode 100644 index 0000000000000..24e4b91ef441b --- /dev/null +++ b/control/predicted_path_checker/README.md @@ -0,0 +1,103 @@ +# Predicted Path Checker + +## Purpose + +The Predicted Path Checker package is designed for autonomous vehicles to check the predicted path generated by control +modules. It handles potential collisions that the planning module might not be able to handle and that in the brake +distance. In case of collision in brake distance, the package will send a diagnostic message labeled "ERROR" to alert +the system to send emergency and in the case of collisions in outside reference trajectory, it sends pause request to +pause interface to make the vehicle stop. + +![general-structure.png](images%2Fgeneral-structure.png) + +## Algorithm + +The package algorithm evaluates the predicted trajectory against the reference trajectory and the predicted objects in +the environment. It checks for potential collisions and, if necessary, generates an appropriate response to avoid them ( +emergency or pause request). + +### Inner Algorithm + +![FlowChart.png](images%2FFlowChart.png) + +**cutTrajectory() ->** It cuts the predicted trajectory with input length. Length is calculated by multiplying the +velocity +of ego vehicle with "trajectory_check_time" parameter and "min_trajectory_length". + +**filterObstacles() ->** It filters the predicted objects in the environment. It filters the objects which are not in +front of the vehicle and far away from predicted trajectory. + +**checkTrajectoryForCollision() ->** It checks the predicted trajectory for collision with the predicted objects. It +calculates both polygon of trajectory points and predicted objects and checks intersection of both polygons. If there is +an intersection, it calculates the nearest collision point. It returns the nearest collision point of polygon and the +predicted object. It also checks predicted objects history which are intersect with the footprint before to avoid +unexpected behaviors. Predicted objects history stores the objects if it was detected below the "chattering_threshold" +seconds ago. + +If the "enable_z_axis_obstacle_filtering" parameter is set to true, it filters the predicted objects in the Z-axis by +using "z_axis_filtering_buffer". If the object does not intersect with the Z-axis, it is filtered out. + +![Z_axis_filtering.png](images%2FZ_axis_filtering.png) + +**calculateProjectedVelAndAcc() ->** It calculates the projected velocity and acceleration of the predicted object on +predicted trajectory's collision point's axes. + +**isInBrakeDistance() ->** It checks if the stop point is in brake distance. It gets relative velocity and +acceleration of ego vehicle with respect to the predicted object. It calculates the brake distance, if the point in +brake distance, it returns true. + +**isItDiscretePoint() ->** It checks if the stop point on predicted trajectory is discrete point or not. If it is not +discrete point, planning should handle the stop. + +**isThereStopPointOnRefTrajectory() ->** It checks if there is a stop point on reference trajectory. If there is a stop +point before the stop index, it returns true. Otherwise, it returns false, and node is going to call pause interface to +make the vehicle stop. + +## Inputs + +| Name | Type | Description | +| ------------------------------------- | ----------------------------------------------------- | --------------------------------------------------- | +| `~/input/reference_trajectory` | `autoware_auto_planning_msgs::msg::Trajectory` | Reference trajectory | +| `~/input/predicted_trajectory` | `autoware_auto_planning_msgs::msg::Trajectory` | Predicted trajectory | +| `~/input/objects` | `autoware_auto_perception_msgs::msg::PredictedObject` | Dynamic objects in the environment | +| `~/input/odometry` | `nav_msgs::msg::Odometry` | Odometry message of vehicle to get current velocity | +| `~/input/current_accel` | `geometry_msgs::msg::AccelWithCovarianceStamped` | Current acceleration | +| `/control/vehicle_cmd_gate/is_paused` | `tier4_control_msgs::msg::IsPaused` | Current pause state of the vehicle | + +## Outputs + +| Name | Type | Description | +| ------------------------------------- | ---------------------------------------- | -------------------------------------- | +| `~/debug/marker` | `visualization_msgs::msg::MarkerArray` | Marker for visualization | +| `~/debug/virtual_wall` | `visualization_msgs::msg::MarkerArray` | Virtual wall marker for visualization | +| `/control/vehicle_cmd_gate/set_pause` | `tier4_control_msgs::srv::SetPause` | Pause service to make the vehicle stop | +| `/diagnostics` | `diagnostic_msgs::msg::DiagnosticStatus` | Diagnostic status of vehicle | + +## Parameters + +### Node Parameters + +| Name | Type | Description | Default value | +| :---------------------------------- | :------- | :-------------------------------------------------------------------- | :------------ | +| `update_rate` | `double` | The update rate [Hz] | 10.0 | +| `delay_time` | `double` | he time delay considered for the emergency response [s] | 0.17 | +| `max_deceleration` | `double` | Max deceleration for ego vehicle to stop [m/s^2] | 1.5 | +| `resample_interval` | `double` | Interval for resampling trajectory [m] | 0.5 | +| `stop_margin` | `double` | The stopping margin [m] | 0.5 | +| `ego_nearest_dist_threshold` | `double` | The nearest distance threshold for ego vehicle [m] | 3.0 | +| `ego_nearest_yaw_threshold` | `double` | The nearest yaw threshold for ego vehicle [rad] | 1.046 | +| `min_trajectory_check_length` | `double` | The minimum trajectory check length in meters [m] | 1.5 | +| `trajectory_check_time` | `double` | The trajectory check time in seconds. [s] | 3.0 | +| `distinct_point_distance_threshold` | `double` | The distinct point distance threshold [m] | 0.3 | +| `distinct_point_yaw_threshold` | `double` | The distinct point yaw threshold [deg] | 5.0 | +| `filtering_distance_threshold` | `double` | It ignores the objects if distance is higher than this [m] | 1.5 | +| `use_object_prediction` | `bool` | If true, node predicts current pose of the objects wrt delta time [-] | true | + +### Collision Checker Parameters + +| Name | Type | Description | Default value | +| :--------------------------------- | :------- | :---------------------------------------------------------------- | :------------ | +| `width_margin` | `double` | The width margin for collision checking [Hz] | 0.2 | +| `chattering_threshold` | `double` | The chattering threshold for collision detection [s] | 0.2 | +| `z_axis_filtering_buffer` | `double` | The Z-axis filtering buffer [m] | 0.3 | +| `enable_z_axis_obstacle_filtering` | `bool` | A boolean flag indicating if Z-axis obstacle filtering is enabled | false | diff --git a/control/predicted_path_checker/config/predicted_path_checker.param.yaml b/control/predicted_path_checker/config/predicted_path_checker.param.yaml new file mode 100644 index 0000000000000..780d86b5e9804 --- /dev/null +++ b/control/predicted_path_checker/config/predicted_path_checker.param.yaml @@ -0,0 +1,22 @@ +/**: + ros__parameters: + # Node + update_rate: 10.0 + delay_time: 0.17 + max_deceleration: 1.5 + resample_interval: 0.5 + stop_margin: 0.5 # [m] + ego_nearest_dist_threshold: 3.0 # [m] + ego_nearest_yaw_threshold: 1.046 # [rad] = 60 [deg] + min_trajectory_check_length: 1.5 # [m] + trajectory_check_time: 3.0 + distinct_point_distance_threshold: 0.3 + distinct_point_yaw_threshold: 5.0 # [deg] + filtering_distance_threshold: 1.5 # [m] + use_object_prediction: true + + collision_checker_params: + width_margin: 0.2 + chattering_threshold: 0.2 + z_axis_filtering_buffer: 0.3 + enable_z_axis_obstacle_filtering: false diff --git a/control/predicted_path_checker/images/FlowChart.png b/control/predicted_path_checker/images/FlowChart.png new file mode 100644 index 0000000000000..15b32417d2a73 Binary files /dev/null and b/control/predicted_path_checker/images/FlowChart.png differ diff --git a/control/predicted_path_checker/images/Z_axis_filtering.png b/control/predicted_path_checker/images/Z_axis_filtering.png new file mode 100644 index 0000000000000..0c064aecae42e Binary files /dev/null and b/control/predicted_path_checker/images/Z_axis_filtering.png differ diff --git a/control/predicted_path_checker/images/general-structure.png b/control/predicted_path_checker/images/general-structure.png new file mode 100644 index 0000000000000..86f3016db5c2c Binary files /dev/null and b/control/predicted_path_checker/images/general-structure.png differ diff --git a/control/predicted_path_checker/include/predicted_path_checker/collision_checker.hpp b/control/predicted_path_checker/include/predicted_path_checker/collision_checker.hpp new file mode 100644 index 0000000000000..d21b11e5b68de --- /dev/null +++ b/control/predicted_path_checker/include/predicted_path_checker/collision_checker.hpp @@ -0,0 +1,128 @@ +// Copyright 2023 LeoDrive A.Ş. All rights reserved. +// +// 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 PREDICTED_PATH_CHECKER__COLLISION_CHECKER_HPP_ +#define PREDICTED_PATH_CHECKER__COLLISION_CHECKER_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include + +#include +#include + +#include +#include +#include + +namespace autoware::motion::control::predicted_path_checker +{ +using autoware_auto_planning_msgs::msg::Trajectory; +using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using TrajectoryPoints = std::vector; +using autoware_auto_perception_msgs::msg::PredictedObject; +using autoware_auto_perception_msgs::msg::PredictedObjects; +using geometry_msgs::msg::Pose; +using geometry_msgs::msg::TransformStamped; +using tier4_autoware_utils::Point2d; +using tier4_autoware_utils::Polygon2d; +using PointArray = std::vector; + +namespace bg = boost::geometry; + +struct CollisionCheckerParam +{ + double width_margin; + double z_axis_filtering_buffer; + bool enable_z_axis_obstacle_filtering; + double chattering_threshold; +}; + +struct PredictedObjectWithDetectionTime +{ + explicit PredictedObjectWithDetectionTime( + const rclcpp::Time & t, geometry_msgs::msg::Point & p, PredictedObject obj) + : detection_time(t), point(p), object(std::move(obj)) + { + } + + rclcpp::Time detection_time; + geometry_msgs::msg::Point point; + PredictedObject object; +}; + +class CollisionChecker +{ +public: + explicit CollisionChecker( + rclcpp::Node * node, std::shared_ptr debug_ptr); + + boost::optional> + checkTrajectoryForCollision( + TrajectoryPoints & predicted_trajectory_array, + PredictedObjects::ConstSharedPtr dynamic_objects); + + void setParam(const CollisionCheckerParam & param); + +private: + // Functions + + boost::optional> checkObstacleHistory( + const Pose & base_pose, const Polygon2d & one_step_move_vehicle_polygon2d, const double z_min, + const double z_max); + + boost::optional> checkDynamicObjects( + const Pose & base_pose, PredictedObjects::ConstSharedPtr dynamic_objects, + const Polygon2d & one_step_move_vehicle_polygon2d, const double z_min, const double z_max); + + void updatePredictedObjectHistory(const rclcpp::Time & now) + { + for (auto itr = predicted_object_history_.begin(); itr != predicted_object_history_.end();) { + const auto expired = (now - itr->detection_time).seconds() > param_.chattering_threshold; + + if (expired) { + itr = predicted_object_history_.erase(itr); + continue; + } + + itr++; + } + } + + // Parameter + CollisionCheckerParam param_; + + // Variables + std::shared_ptr debug_ptr_; + rclcpp::Node * node_; + vehicle_info_util::VehicleInfo vehicle_info_; + std::vector predicted_object_history_{}; +}; +} // namespace autoware::motion::control::predicted_path_checker + +#endif // PREDICTED_PATH_CHECKER__COLLISION_CHECKER_HPP_ diff --git a/control/predicted_path_checker/include/predicted_path_checker/debug_marker.hpp b/control/predicted_path_checker/include/predicted_path_checker/debug_marker.hpp new file mode 100644 index 0000000000000..02ade624d630c --- /dev/null +++ b/control/predicted_path_checker/include/predicted_path_checker/debug_marker.hpp @@ -0,0 +1,92 @@ +// Copyright 2023 LeoDrive A.Ş. All rights reserved. +// +// 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 PREDICTED_PATH_CHECKER__DEBUG_MARKER_HPP_ +#define PREDICTED_PATH_CHECKER__DEBUG_MARKER_HPP_ + +#include + +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include + +#define EIGEN_MPL2_ONLY + +#include +#include +#include + +namespace autoware::motion::control::predicted_path_checker +{ + +enum class PolygonType : int8_t { Vehicle = 0, Collision }; + +enum class PointType : int8_t { Stop = 0 }; + +enum class PoseType : int8_t { Stop = 0, Collision }; + +class PredictedPathCheckerDebugNode +{ +public: + explicit PredictedPathCheckerDebugNode(rclcpp::Node * node, const double base_link2front); + + ~PredictedPathCheckerDebugNode() {} + + bool pushPolygon( + const tier4_autoware_utils::Polygon2d & polygon, const double z, const PolygonType & type); + + bool pushPolygon(const std::vector & polygon, const PolygonType & type); + + bool pushPolyhedron( + const tier4_autoware_utils::Polygon2d & polyhedron, const double z_min, const double z_max, + const PolygonType & type); + + bool pushPolyhedron(const std::vector & polyhedron, 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); + + visualization_msgs::msg::MarkerArray makeVirtualWallMarker(); + + visualization_msgs::msg::MarkerArray makeVisualizationMarker(); + + void publish(); + +private: + rclcpp::Publisher::SharedPtr virtual_wall_pub_; + rclcpp::Publisher::SharedPtr debug_viz_pub_; + rclcpp::Node * node_; + double base_link2front_; + + std::shared_ptr stop_pose_ptr_; + std::shared_ptr collision_pose_ptr_; + std::shared_ptr stop_obstacle_point_ptr_; + std::vector> vehicle_polygons_; + std::vector> collision_polygons_; + std::vector> vehicle_polyhedrons_; + std::vector> collision_polyhedrons_; +}; + +} // namespace autoware::motion::control::predicted_path_checker + +#endif // PREDICTED_PATH_CHECKER__DEBUG_MARKER_HPP_ diff --git a/control/predicted_path_checker/include/predicted_path_checker/predicted_path_checker_node.hpp b/control/predicted_path_checker/include/predicted_path_checker/predicted_path_checker_node.hpp new file mode 100644 index 0000000000000..e3c3b5cccfb8f --- /dev/null +++ b/control/predicted_path_checker/include/predicted_path_checker/predicted_path_checker_node.hpp @@ -0,0 +1,178 @@ +// Copyright 2023 LeoDrive A.Ş. All rights reserved. +// +// 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 PREDICTED_PATH_CHECKER__PREDICTED_PATH_CHECKER_NODE_HPP_ +#define PREDICTED_PATH_CHECKER__PREDICTED_PATH_CHECKER_NODE_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include + +#include +#include +#include + +namespace autoware::motion::control::predicted_path_checker +{ +using autoware_auto_planning_msgs::msg::Trajectory; +using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using TrajectoryPoints = std::vector; +using autoware_auto_perception_msgs::msg::PredictedObjects; +using geometry_msgs::msg::Pose; +using geometry_msgs::msg::TransformStamped; +using tier4_autoware_utils::Point2d; +using tier4_autoware_utils::Polygon2d; + +struct NodeParam +{ + double update_rate; + double delay_time; + double max_deceleration; + double resample_interval; + double ego_nearest_dist_threshold; + double ego_nearest_yaw_threshold; + double stop_margin; + double min_trajectory_check_length; + double trajectory_check_time; + double distinct_point_distance_threshold; + double distinct_point_yaw_threshold; + double filtering_distance_threshold; + bool use_object_prediction; +}; + +enum class State { + DRIVE = 0, + EMERGENCY = 1, + STOP = 2, +}; + +class PredictedPathCheckerNode : public rclcpp::Node +{ +public: + explicit PredictedPathCheckerNode(const rclcpp::NodeOptions & node_options); + +private: + rclcpp::CallbackGroup::SharedPtr group_cli_; + + // Subscriber + std::shared_ptr self_pose_listener_; + rclcpp::Subscription::SharedPtr sub_dynamic_objects_; + rclcpp::Subscription::SharedPtr + sub_reference_trajectory_; + rclcpp::Subscription::SharedPtr + sub_predicted_trajectory_; + rclcpp::Subscription::SharedPtr sub_odom_; + rclcpp::Subscription::SharedPtr sub_accel_; + component_interface_utils::Subscription::SharedPtr sub_stop_state_; + + // Client + component_interface_utils::Client::SharedPtr cli_set_stop_; + + // Data Buffer + geometry_msgs::msg::PoseStamped::ConstSharedPtr current_pose_; + geometry_msgs::msg::Twist::ConstSharedPtr current_twist_; + geometry_msgs::msg::AccelWithCovarianceStamped::SharedPtr current_accel_; + PredictedObjects::ConstSharedPtr object_ptr_{nullptr}; + autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr reference_trajectory_; + autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr predicted_trajectory_; + control_interface::IsStopped::Message::ConstSharedPtr is_stopped_ptr_{nullptr}; + + // Core + std::unique_ptr collision_checker_; + std::shared_ptr debug_ptr_; + + // Variables + State current_state_{State::DRIVE}; + vehicle_info_util::VehicleInfo vehicle_info_; + bool is_calling_set_stop_{false}; + bool is_stopped_by_node_{false}; + + // Callback + void onDynamicObjects(PredictedObjects::ConstSharedPtr msg); + void onReferenceTrajectory(const autoware_auto_planning_msgs::msg::Trajectory::SharedPtr msg); + void onPredictedTrajectory(const autoware_auto_planning_msgs::msg::Trajectory::SharedPtr msg); + void onOdom(const nav_msgs::msg::Odometry::SharedPtr msg); + void onAccel(const geometry_msgs::msg::AccelWithCovarianceStamped::SharedPtr msg); + void onIsStopped(const control_interface::IsStopped::Message::ConstSharedPtr msg); + + // Timer + rclcpp::TimerBase::SharedPtr timer_; + void initTimer(double period_s); + + // Functions + bool isDataReady(); + + bool isDataTimeout(); + + bool isThereStopPointOnReferenceTrajectory( + const geometry_msgs::msg::Pose & pose, const TrajectoryPoints & reference_trajectory_array); + + void onTimer(); + + void checkVehicleState(diagnostic_updater::DiagnosticStatusWrapper & stat); + + TrajectoryPoints trimTrajectoryFromSelfPose( + const TrajectoryPoints & input, const Pose & self_pose) const; + + void sendRequest(bool make_stop_vehicle); + + bool isItDiscretePoint( + const TrajectoryPoints & reference_trajectory, const TrajectoryPoint & collision_point) const; + + static Trajectory cutTrajectory(const Trajectory & trajectory, const double length); + + size_t insertStopPoint( + TrajectoryPoints & trajectory, const geometry_msgs::msg::Point collision_point); + + void extendTrajectoryPointsArray(TrajectoryPoints & trajectory); + + static std::pair calculateProjectedVelAndAcc( + const PredictedObject & object, const TrajectoryPoint & trajectory_point); + + void filterObstacles( + const Pose & ego_pose, const TrajectoryPoints & traj, const double dist_threshold, + const bool use_prediction, PredictedObjects & filtered_objects); + + // Parameters + CollisionCheckerParam collision_checker_param_; + NodeParam node_param_; + + // Diagnostic Updater + diagnostic_updater::Updater updater_; +}; +} // namespace autoware::motion::control::predicted_path_checker + +#endif // PREDICTED_PATH_CHECKER__PREDICTED_PATH_CHECKER_NODE_HPP_ diff --git a/control/predicted_path_checker/include/predicted_path_checker/utils.hpp b/control/predicted_path_checker/include/predicted_path_checker/utils.hpp new file mode 100644 index 0000000000000..75e90e624a17e --- /dev/null +++ b/control/predicted_path_checker/include/predicted_path_checker/utils.hpp @@ -0,0 +1,104 @@ +// Copyright 2023 LeoDrive A.Ş. All rights reserved. +// +// 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 PREDICTED_PATH_CHECKER__UTILS_HPP_ +#define PREDICTED_PATH_CHECKER__UTILS_HPP_ + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace utils +{ + +using autoware_auto_perception_msgs::msg::PredictedObject; +using autoware_auto_perception_msgs::msg::PredictedObjects; +using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using geometry_msgs::msg::Point; +using geometry_msgs::msg::Pose; +using geometry_msgs::msg::TransformStamped; +using std_msgs::msg::Header; +using tier4_autoware_utils::Point2d; +using tier4_autoware_utils::Polygon2d; +using vehicle_info_util::VehicleInfo; +using PointArray = std::vector; + +using TrajectoryPoints = std::vector; + +void appendPointToPolygon(Polygon2d & polygon, const geometry_msgs::msg::Point & geom_point); + +Polygon2d createOneStepPolygon( + const geometry_msgs::msg::Pose & base_step_pose, const geometry_msgs::msg::Pose & next_step_pose, + const vehicle_info_util::VehicleInfo & vehicle_info, const double expand_width); + +TrajectoryPoint calcInterpolatedPoint( + const TrajectoryPoints & trajectory, const geometry_msgs::msg::Point & target_point, + const size_t segment_idx, const bool use_zero_order_hold_for_twist); + +std::pair findStopPoint( + TrajectoryPoints & predicted_trajectory_array, const size_t collision_idx, + const double stop_margin, vehicle_info_util::VehicleInfo & vehicle_info); + +bool isInBrakeDistance( + const TrajectoryPoints & trajectory, const size_t stop_idx, const double relative_velocity, + const double relative_acceleration, const double max_deceleration, const double delay_time_sec); + +TrajectoryPoint getExtendTrajectoryPoint( + const double extend_distance, const TrajectoryPoint & goal_point); + +bool intersectsInZAxis(const PredictedObject & object, const double z_min, const double z_max); + +double getNearestPointAndDistanceForPredictedObject( + const PointArray & points, const Pose & base_pose, + geometry_msgs::msg::Point * nearest_collision_point); + +Polygon2d convertBoundingBoxObjectToGeometryPolygon( + 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 autoware_auto_perception_msgs::msg::Shape & obj_shape); + +Polygon2d convertPolygonObjectToGeometryPolygon( + const Pose & current_pose, const autoware_auto_perception_msgs::msg::Shape & obj_shape); + +Polygon2d convertObjToPolygon(const PredictedObject & obj); + +double calcObstacleMaxLength(const autoware_auto_perception_msgs::msg::Shape & shape); + +void getCurrentObjectPose( + PredictedObject & predicted_object, const rclcpp::Time & obj_base_time, + const rclcpp::Time & current_time); + +bool isFrontObstacle(const Pose & ego_pose, const geometry_msgs::msg::Point & obstacle_pos); +} // namespace utils + +#endif // PREDICTED_PATH_CHECKER__UTILS_HPP_ diff --git a/control/predicted_path_checker/launch/predicted_path_checker.launch.xml b/control/predicted_path_checker/launch/predicted_path_checker.launch.xml new file mode 100755 index 0000000000000..a35c11b80c1f7 --- /dev/null +++ b/control/predicted_path_checker/launch/predicted_path_checker.launch.xml @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/control/predicted_path_checker/package.xml b/control/predicted_path_checker/package.xml new file mode 100644 index 0000000000000..35f971907696a --- /dev/null +++ b/control/predicted_path_checker/package.xml @@ -0,0 +1,47 @@ + + + + predicted_path_checker + 1.0.0 + The predicted_path_checker package + + Berkay Karaman + Apache 2.0 + Berkay Karaman + + ament_cmake + autoware_cmake + + autoware_auto_perception_msgs + autoware_auto_planning_msgs + boost + component_interface_specs + component_interface_utils + diagnostic_updater + eigen + geometry_msgs + motion_utils + nav_msgs + rclcpp + rclcpp_components + sensor_msgs + std_msgs + tf2 + tf2_eigen + tf2_geometry_msgs + tf2_ros + tier4_autoware_utils + tier4_control_msgs + tier4_external_api_msgs + tier4_planning_msgs + vehicle_info_util + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + eigen + + + ament_cmake + + diff --git a/control/predicted_path_checker/src/predicted_path_checker_node/collision_checker.cpp b/control/predicted_path_checker/src/predicted_path_checker_node/collision_checker.cpp new file mode 100644 index 0000000000000..5e8d96805b832 --- /dev/null +++ b/control/predicted_path_checker/src/predicted_path_checker_node/collision_checker.cpp @@ -0,0 +1,231 @@ +// Copyright 2022 LeoDrive A.Ş. All rights reserved. +// +// 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 "predicted_path_checker/collision_checker.hpp" + +#include +#include + +#include +#include +#include + +namespace autoware::motion::control::predicted_path_checker +{ +CollisionChecker::CollisionChecker( + rclcpp::Node * node, std::shared_ptr debug_ptr) +: debug_ptr_(std::move(debug_ptr)), + node_(node), + vehicle_info_(vehicle_info_util::VehicleInfoUtil(*node).getVehicleInfo()) +{ +} + +void CollisionChecker::setParam(const CollisionCheckerParam & param) +{ + param_ = param; +} + +boost::optional> +CollisionChecker::checkTrajectoryForCollision( + TrajectoryPoints & predicted_trajectory_array, PredictedObjects::ConstSharedPtr dynamic_objects) +{ + // It checks the collision, if there is a collision, it updates the predicted_trajectory_array and + // returns the index of the stop point. + // If there is no collision, it returns boost::none. + const auto now = node_->now(); + + updatePredictedObjectHistory(now); + if (dynamic_objects->objects.empty() && predicted_object_history_.empty()) { + return boost::none; + } + + for (size_t i = 0; i < predicted_trajectory_array.size() - 1; i++) { + // create one step circle center for vehicle + const auto & p_front = predicted_trajectory_array.at(i).pose; + const auto & p_back = predicted_trajectory_array.at(i + 1).pose; + const auto z_min = p_front.position.z; + const auto z_max = + p_front.position.z + vehicle_info_.max_height_offset_m + param_.z_axis_filtering_buffer; + + // create one-step polygon for vehicle + Polygon2d one_step_move_vehicle_polygon2d = + utils::createOneStepPolygon(p_front, p_back, vehicle_info_, param_.width_margin); + if (param_.enable_z_axis_obstacle_filtering) { + debug_ptr_->pushPolyhedron( + one_step_move_vehicle_polygon2d, z_min, z_max, PolygonType::Vehicle); + } else { + debug_ptr_->pushPolygon( + one_step_move_vehicle_polygon2d, p_front.position.z, PolygonType::Vehicle); + } + + // check obstacle history + auto found_collision_at_history = + checkObstacleHistory(p_front, one_step_move_vehicle_polygon2d, z_min, z_max); + + auto found_collision_at_dynamic_objects = + checkDynamicObjects(p_front, dynamic_objects, one_step_move_vehicle_polygon2d, z_min, z_max); + + if (found_collision_at_dynamic_objects || found_collision_at_history) { + double distance_to_current = std::numeric_limits::max(); + double distance_to_history = std::numeric_limits::max(); + if (found_collision_at_dynamic_objects) { + distance_to_current = tier4_autoware_utils::calcDistance2d( + p_front, found_collision_at_dynamic_objects.get().first); + } + if (found_collision_at_history) { + distance_to_history = + tier4_autoware_utils::calcDistance2d(p_front, found_collision_at_history.get().first); + } else { + predicted_object_history_.clear(); + } + + if (param_.enable_z_axis_obstacle_filtering) { + debug_ptr_->pushPolyhedron( + one_step_move_vehicle_polygon2d, z_min, z_max, PolygonType::Collision); + } else { + debug_ptr_->pushPolygon( + one_step_move_vehicle_polygon2d, p_front.position.z, PolygonType::Collision); + } + + if (distance_to_current > distance_to_history) { + debug_ptr_->pushObstaclePoint(found_collision_at_history->first, PointType::Stop); + return found_collision_at_history; + } + + predicted_object_history_.emplace_back( + now, found_collision_at_dynamic_objects.get().first, + found_collision_at_dynamic_objects.get().second); + debug_ptr_->pushObstaclePoint(found_collision_at_dynamic_objects->first, PointType::Stop); + return found_collision_at_dynamic_objects; + } + } + return boost::none; +} + +boost::optional> +CollisionChecker::checkObstacleHistory( + const Pose & base_pose, const Polygon2d & one_step_move_vehicle_polygon2d, const double z_min, + const double z_max) +{ + if (predicted_object_history_.empty()) { + return boost::none; + } + + std::vector> collision_points_in_history; + for (const auto & obj_history : predicted_object_history_) { + if (param_.enable_z_axis_obstacle_filtering) { + if (!utils::intersectsInZAxis(obj_history.object, z_min, z_max)) { + continue; + } + } + const auto & point = obj_history.point; + const Point2d point2d(point.x, point.y); + if (bg::within(point2d, one_step_move_vehicle_polygon2d)) { + collision_points_in_history.emplace_back(point, obj_history.object); + } + } + if (!collision_points_in_history.empty()) { + double min_norm = 0.0; + bool is_init = false; + std::pair nearest_collision_object_with_point; + for (const auto & p : collision_points_in_history) { + double norm = tier4_autoware_utils::calcDistance2d(p.first, base_pose); + if (norm < min_norm || !is_init) { + min_norm = norm; + nearest_collision_object_with_point = p; + is_init = true; + } + } + return boost::make_optional(nearest_collision_object_with_point); + } + return boost::none; +} + +boost::optional> +CollisionChecker::checkDynamicObjects( + const Pose & base_pose, PredictedObjects::ConstSharedPtr dynamic_objects, + const Polygon2d & one_step_move_vehicle_polygon2d, const double z_min, const double z_max) +{ + if (dynamic_objects->objects.empty()) { + return boost::none; + } + double min_norm_collision_norm = 0.0; + bool is_init = false; + size_t nearest_collision_object_index = 0; + geometry_msgs::msg::Point nearest_collision_point; + + for (size_t i = 0; i < dynamic_objects->objects.size(); ++i) { + const auto & obj = dynamic_objects->objects.at(i); + if (param_.enable_z_axis_obstacle_filtering) { + if (!utils::intersectsInZAxis(obj, z_min, z_max)) { + continue; + } + } + const auto object_polygon = utils::convertObjToPolygon(obj); + if (object_polygon.outer().empty()) { + // unsupported type + continue; + } + + const auto found_collision_points = + bg::intersects(one_step_move_vehicle_polygon2d, object_polygon); + + if (found_collision_points) { + std::vector collision_points; + PointArray collision_point_array; + bg::intersection(one_step_move_vehicle_polygon2d, object_polygon, collision_points); + for (const auto & point : collision_points) { + geometry_msgs::msg::Point p; + p.x = point.x(); + p.y = point.y(); + collision_point_array.push_back(p); + } + + // Also check the corner points + + for (const auto & point : object_polygon.outer()) { + if (bg::within(point, one_step_move_vehicle_polygon2d)) { + geometry_msgs::msg::Point p; + p.x = point.x(); + p.y = point.y(); + collision_point_array.push_back(p); + } + } + geometry_msgs::msg::Point nearest_collision_point_tmp; + + double norm = utils::getNearestPointAndDistanceForPredictedObject( + collision_point_array, base_pose, &nearest_collision_point_tmp); + if (norm < min_norm_collision_norm || !is_init) { + min_norm_collision_norm = norm; + nearest_collision_point = nearest_collision_point_tmp; + is_init = true; + nearest_collision_object_index = i; + } + } + } + if (is_init) { + const auto & obj = dynamic_objects->objects.at(nearest_collision_object_index); + const auto obstacle_polygon = utils::convertObjToPolygon(obj); + if (param_.enable_z_axis_obstacle_filtering) { + debug_ptr_->pushPolyhedron(obstacle_polygon, z_min, z_max, PolygonType::Collision); + } else { + debug_ptr_->pushPolygon( + obstacle_polygon, obj.kinematics.initial_pose_with_covariance.pose.position.z, + PolygonType::Collision); + } + return boost::make_optional(std::make_pair(nearest_collision_point, obj)); + } + return boost::none; +} +} // namespace autoware::motion::control::predicted_path_checker diff --git a/control/predicted_path_checker/src/predicted_path_checker_node/debug_marker.cpp b/control/predicted_path_checker/src/predicted_path_checker_node/debug_marker.cpp new file mode 100644 index 0000000000000..3fb7b69d531c0 --- /dev/null +++ b/control/predicted_path_checker/src/predicted_path_checker_node/debug_marker.cpp @@ -0,0 +1,329 @@ +// Copyright 2023 LeoDrive A.Ş. All rights reserved. +// +// 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 "predicted_path_checker/debug_marker.hpp" + +#include +#include +#include +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif + +#include + +#include +#include + +using motion_utils::createDeletedStopVirtualWallMarker; +using motion_utils::createStopVirtualWallMarker; +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; + +namespace autoware::motion::control::predicted_path_checker +{ +PredictedPathCheckerDebugNode::PredictedPathCheckerDebugNode( + rclcpp::Node * node, const double base_link2front) +: node_(node), base_link2front_(base_link2front) +{ + virtual_wall_pub_ = + node->create_publisher("~/debug/virtual_wall", 1); + debug_viz_pub_ = + node_->create_publisher("~/debug/marker", 1); +} + +bool PredictedPathCheckerDebugNode::pushPolygon( + const tier4_autoware_utils::Polygon2d & polygon, const double z, const PolygonType & type) +{ + std::vector eigen_polygon; + for (const auto & point : polygon.outer()) { + Eigen::Vector3d eigen_point; + eigen_point << point.x(), point.y(), z; + eigen_polygon.push_back(eigen_point); + } + return pushPolygon(eigen_polygon, type); +} + +bool PredictedPathCheckerDebugNode::pushPolygon( + const std::vector & polygon, const PolygonType & type) +{ + switch (type) { + case PolygonType::Vehicle: + if (!polygon.empty()) { + vehicle_polygons_.push_back(polygon); + } + return true; + case PolygonType::Collision: + if (!polygon.empty()) { + collision_polygons_.push_back(polygon); + } + return true; + default: + return false; + } +} + +bool PredictedPathCheckerDebugNode::pushPolyhedron( + const tier4_autoware_utils::Polygon2d & polyhedron, const double z_min, const double z_max, + const PolygonType & type) +{ + std::vector eigen_polyhedron; + for (const auto & point : polyhedron.outer()) { + eigen_polyhedron.emplace_back(point.x(), point.y(), z_min); + eigen_polyhedron.emplace_back(point.x(), point.y(), z_max); + } + + return pushPolyhedron(eigen_polyhedron, type); +} + +bool PredictedPathCheckerDebugNode::pushPolyhedron( + const std::vector & polyhedron, const PolygonType & type) +{ + switch (type) { + case PolygonType::Vehicle: + if (!polyhedron.empty()) { + vehicle_polyhedrons_.push_back(polyhedron); + } + return true; + case PolygonType::Collision: + if (!polyhedron.empty()) { + collision_polyhedrons_.push_back(polyhedron); + } + return true; + default: + return false; + } +} + +bool PredictedPathCheckerDebugNode::pushPose( + const geometry_msgs::msg::Pose & pose, const PoseType & type) +{ + switch (type) { + case PoseType::Stop: + stop_pose_ptr_ = std::make_shared(pose); + return true; + case PoseType::Collision: + collision_pose_ptr_ = std::make_shared(pose); + return true; + default: + return false; + } +} + +bool PredictedPathCheckerDebugNode::pushObstaclePoint( + const geometry_msgs::msg::Point & obstacle_point, const PointType & type) +{ + switch (type) { + case PointType::Stop: + stop_obstacle_point_ptr_ = std::make_shared(obstacle_point); + return true; + default: + return false; + } +} + +void PredictedPathCheckerDebugNode::publish() +{ + /* publish debug marker for rviz */ + const auto virtual_wall_msg = makeVirtualWallMarker(); + virtual_wall_pub_->publish(virtual_wall_msg); + + /* publish debug marker for rviz */ + const auto visualization_msg = makeVisualizationMarker(); + debug_viz_pub_->publish(visualization_msg); + + /* reset variables */ + vehicle_polygons_.clear(); + collision_polygons_.clear(); + vehicle_polyhedrons_.clear(); + collision_polyhedrons_.clear(); + collision_pose_ptr_ = nullptr; + stop_pose_ptr_ = nullptr; + stop_obstacle_point_ptr_ = nullptr; +} + +visualization_msgs::msg::MarkerArray PredictedPathCheckerDebugNode::makeVirtualWallMarker() +{ + visualization_msgs::msg::MarkerArray msg; + rclcpp::Time current_time = node_->now(); + + if (stop_pose_ptr_ != nullptr) { + const auto p = calcOffsetPose(*stop_pose_ptr_, base_link2front_, 0.0, 0.0); + const auto markers = + createStopVirtualWallMarker(p, "obstacle on predicted path", current_time, 0); + appendMarkerArray(markers, &msg); + } else { + const auto markers = createDeletedStopVirtualWallMarker(current_time, 0); + appendMarkerArray(markers, &msg); + } + + if (collision_pose_ptr_ != nullptr) { + const auto markers = + createStopVirtualWallMarker(*collision_pose_ptr_, "collision_point", current_time, 1); + appendMarkerArray(markers, &msg); + } else { + const auto markers = createDeletedStopVirtualWallMarker(current_time, 1); + appendMarkerArray(markers, &msg); + } + + return msg; +} + +visualization_msgs::msg::MarkerArray PredictedPathCheckerDebugNode::makeVisualizationMarker() +{ + visualization_msgs::msg::MarkerArray msg; + rclcpp::Time current_time = node_->now(); + + // cube + if (!vehicle_polyhedrons_.empty()) { + auto marker = createDefaultMarker( + "map", current_time, "detection_cubes", 0, visualization_msgs::msg::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_polyhedrons_.size(); ++i) { + for (size_t j = 0; j < vehicle_polyhedrons_.at(i).size(); ++j) { + const auto & p = vehicle_polyhedrons_.at(i).at(j); + marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + } + } + + for (size_t i = 0; i < vehicle_polyhedrons_.size(); ++i) { + for (size_t j = 0; j + 2 < vehicle_polyhedrons_.at(i).size(); ++j) { + const auto & p = vehicle_polyhedrons_.at(i).at(j); + marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + const auto & p1 = vehicle_polyhedrons_.at(i).at(j + 2); + marker.points.push_back(createPoint(p1.x(), p1.y(), p1.z())); + } + const auto & p = vehicle_polyhedrons_.at(i).at(1); + marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + const auto & p1 = vehicle_polyhedrons_.at(i).at(vehicle_polyhedrons_.at(i).size() - 1); + marker.points.push_back(createPoint(p1.x(), p1.y(), p1.z())); + const auto & p2 = vehicle_polyhedrons_.at(i).at(0); + marker.points.push_back(createPoint(p2.x(), p2.y(), p2.z())); + const auto & p3 = vehicle_polyhedrons_.at(i).at(vehicle_polyhedrons_.at(i).size() - 2); + marker.points.push_back(createPoint(p3.x(), p3.y(), p3.z())); + } + + msg.markers.push_back(marker); + } + + if (!collision_polyhedrons_.empty()) { + auto marker = createDefaultMarker( + "map", current_time, "collision_cubes", 0, visualization_msgs::msg::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_polyhedrons_.size(); ++i) { + for (size_t j = 0; j < collision_polyhedrons_.at(i).size(); ++j) { + const auto & p = collision_polyhedrons_.at(i).at(j); + marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + } + } + + for (size_t i = 0; i < collision_polyhedrons_.size(); ++i) { + for (size_t j = 0; j + 2 < collision_polyhedrons_.at(i).size(); ++j) { + const auto & p = collision_polyhedrons_.at(i).at(j); + marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + const auto & p1 = collision_polyhedrons_.at(i).at(j + 2); + marker.points.push_back(createPoint(p1.x(), p1.y(), p1.z())); + } + const auto & p = collision_polyhedrons_.at(i).at(1); + marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + const auto & p1 = collision_polyhedrons_.at(i).at(collision_polyhedrons_.at(i).size() - 1); + marker.points.push_back(createPoint(p1.x(), p1.y(), p1.z())); + const auto & p2 = collision_polyhedrons_.at(i).at(0); + marker.points.push_back(createPoint(p2.x(), p2.y(), p2.z())); + const auto & p3 = collision_polyhedrons_.at(i).at(collision_polyhedrons_.at(i).size() - 2); + marker.points.push_back(createPoint(p3.x(), p3.y(), p3.z())); + } + + msg.markers.push_back(marker); + } + + // polygon + if (!vehicle_polygons_.empty()) { + auto marker = createDefaultMarker( + "map", current_time, "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)); + + for (size_t i = 0; i < vehicle_polygons_.size(); ++i) { + for (size_t j = 0; j < vehicle_polygons_.at(i).size(); ++j) { + { + const auto & p = vehicle_polygons_.at(i).at(j); + marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + } + if (j + 1 == vehicle_polygons_.at(i).size()) { + const auto & p = vehicle_polygons_.at(i).at(0); + marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + } else { + const auto & p = vehicle_polygons_.at(i).at(j + 1); + marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + } + } + } + msg.markers.push_back(marker); + } + + if (!collision_polygons_.empty()) { + auto marker = createDefaultMarker( + "map", current_time, "collision_polygons", 0, visualization_msgs::msg::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) { + for (size_t j = 0; j < collision_polygons_.at(i).size(); ++j) { + { + const auto & p = collision_polygons_.at(i).at(j); + marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + } + if (j + 1 == collision_polygons_.at(i).size()) { + const auto & p = collision_polygons_.at(i).at(0); + marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + } else { + const auto & p = collision_polygons_.at(i).at(j + 1); + marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + } + } + } + msg.markers.push_back(marker); + } + + if (stop_obstacle_point_ptr_ != nullptr) { + auto marker = createDefaultMarker( + "map", current_time, "stop_obstacle_point", 0, visualization_msgs::msg::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); + } + + 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)); + marker.pose.position = *stop_obstacle_point_ptr_; + marker.pose.position.z += 2.0; + marker.text = "!"; + msg.markers.push_back(marker); + } + + return msg; +} + +} // namespace autoware::motion::control::predicted_path_checker diff --git a/control/predicted_path_checker/src/predicted_path_checker_node/predicted_path_checker_node.cpp b/control/predicted_path_checker/src/predicted_path_checker_node/predicted_path_checker_node.cpp new file mode 100644 index 0000000000000..0cf2548d69746 --- /dev/null +++ b/control/predicted_path_checker/src/predicted_path_checker_node/predicted_path_checker_node.cpp @@ -0,0 +1,585 @@ +// Copyright 2023 LeoDrive A.Ş. All rights reserved. +// +// 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 "predicted_path_checker/predicted_path_checker_node.hpp" + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace autoware::motion::control::predicted_path_checker +{ + +PredictedPathCheckerNode::PredictedPathCheckerNode(const rclcpp::NodeOptions & node_options) +: Node("predicted_path_checker_node", node_options), updater_(this) +{ + using std::placeholders::_1; + + const auto adaptor = component_interface_utils::NodeAdaptor(this); + group_cli_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + adaptor.init_cli(cli_set_stop_, group_cli_); + adaptor.init_sub(sub_stop_state_, this, &PredictedPathCheckerNode::onIsStopped); + vehicle_info_ = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); + + // Node Parameter + node_param_.update_rate = declare_parameter("update_rate", 10.0); + node_param_.ego_nearest_dist_threshold = declare_parameter("ego_nearest_dist_threshold", 3.0); + node_param_.ego_nearest_yaw_threshold = declare_parameter("ego_nearest_yaw_threshold", 1.046); + node_param_.max_deceleration = declare_parameter("max_deceleration", 1.5); + node_param_.delay_time = declare_parameter("delay_time", 0.17); + node_param_.stop_margin = declare_parameter("stop_margin", 0.5); + node_param_.min_trajectory_check_length = declare_parameter("min_trajectory_check_length", 1.5); + node_param_.trajectory_check_time = declare_parameter("trajectory_check_time", 3.0); + node_param_.resample_interval = declare_parameter("resample_interval", 0.5); + node_param_.distinct_point_distance_threshold = + declare_parameter("distinct_point_distance_threshold", 0.3); + node_param_.distinct_point_yaw_threshold = declare_parameter("distinct_point_yaw_threshold", 5.0); + node_param_.filtering_distance_threshold = declare_parameter("filtering_distance_threshold", 1.5); + node_param_.use_object_prediction = declare_parameter("use_object_prediction", true); + + // Collision Checker Parameter + collision_checker_param_.width_margin = + declare_parameter("collision_checker_params.width_margin", 0.2); + collision_checker_param_.enable_z_axis_obstacle_filtering = + declare_parameter("collision_checker_params.enable_z_axis_obstacle_filtering", false); + collision_checker_param_.z_axis_filtering_buffer = + declare_parameter("collision_checker_params.z_axis_filtering_buffer", 0.3); + collision_checker_param_.chattering_threshold = + declare_parameter("collision_checker_params.chattering_threshold", 0.2); + + // Subscriber + self_pose_listener_ = std::make_shared(this); + + sub_dynamic_objects_ = create_subscription( + "~/input/objects", rclcpp::SensorDataQoS(), + std::bind(&PredictedPathCheckerNode::onDynamicObjects, this, _1)); + sub_reference_trajectory_ = create_subscription( + "~/input/reference_trajectory", 1, + std::bind(&PredictedPathCheckerNode::onReferenceTrajectory, this, _1)); + sub_predicted_trajectory_ = create_subscription( + "~/input/predicted_trajectory", 1, + std::bind(&PredictedPathCheckerNode::onPredictedTrajectory, this, _1)); + sub_odom_ = create_subscription( + "~/input/odometry", 1, std::bind(&PredictedPathCheckerNode::onOdom, this, _1)); + sub_accel_ = create_subscription( + "~/input/current_accel", rclcpp::QoS{1}, + std::bind(&PredictedPathCheckerNode::onAccel, this, _1)); + + debug_ptr_ = + std::make_shared(this, vehicle_info_.max_longitudinal_offset_m); + + // Core + + collision_checker_ = std::make_unique(this, debug_ptr_); + collision_checker_->setParam(collision_checker_param_); + + // Diagnostic Updater + updater_.setHardwareID("predicted_path_checker"); + updater_.add("predicted_path_checker", this, &PredictedPathCheckerNode::checkVehicleState); + + // Wait for first self pose + self_pose_listener_->waitForFirstPose(); + + // Timer + initTimer(1.0 / node_param_.update_rate); +} + +void PredictedPathCheckerNode::onDynamicObjects(const PredictedObjects::ConstSharedPtr msg) +{ + object_ptr_ = msg; +} + +void PredictedPathCheckerNode::onReferenceTrajectory( + const autoware_auto_planning_msgs::msg::Trajectory::SharedPtr msg) +{ + reference_trajectory_ = msg; +} + +void PredictedPathCheckerNode::onPredictedTrajectory( + const autoware_auto_planning_msgs::msg::Trajectory::SharedPtr msg) +{ + predicted_trajectory_ = msg; +} + +void PredictedPathCheckerNode::onOdom(const nav_msgs::msg::Odometry::SharedPtr msg) +{ + current_twist_ = std::make_shared(msg->twist.twist); +} + +void PredictedPathCheckerNode::onAccel( + const geometry_msgs::msg::AccelWithCovarianceStamped::SharedPtr msg) +{ + current_accel_ = msg; +} + +void PredictedPathCheckerNode::onIsStopped( + const control_interface::IsStopped::Message::ConstSharedPtr msg) +{ + is_stopped_ptr_ = msg; + + is_stopped_by_node_ = + is_stopped_ptr_->data && + std::find( + is_stopped_ptr_->requested_sources.begin(), is_stopped_ptr_->requested_sources.end(), + "predicted_path_checker") != is_stopped_ptr_->requested_sources.end(); +} + +void PredictedPathCheckerNode::initTimer(double period_s) +{ + const auto period_ns = + std::chrono::duration_cast(std::chrono::duration(period_s)); + timer_ = rclcpp::create_timer( + this, get_clock(), period_ns, std::bind(&PredictedPathCheckerNode::onTimer, this)); +} + +bool PredictedPathCheckerNode::isDataReady() +{ + if (!current_pose_) { + RCLCPP_INFO_THROTTLE( + this->get_logger(), *this->get_clock(), 5000 /* ms */, "waiting for current_pose..."); + return false; + } + + if (!object_ptr_) { + RCLCPP_INFO_THROTTLE( + this->get_logger(), *this->get_clock(), 5000 /* ms */, "waiting for dynamic objects msg..."); + return false; + } + + if (!reference_trajectory_) { + RCLCPP_INFO_THROTTLE( + this->get_logger(), *this->get_clock(), 5000 /* ms */, + "waiting for reference_trajectory msg..."); + return false; + } + + if (!predicted_trajectory_) { + RCLCPP_INFO_THROTTLE( + this->get_logger(), *this->get_clock(), 5000 /* ms */, + "waiting for predicted_trajectory msg..."); + return false; + } + + if (!current_twist_) { + RCLCPP_INFO_THROTTLE( + this->get_logger(), *this->get_clock(), 5000 /* ms */, "waiting for current_twist msg..."); + return false; + } + + if (!current_accel_) { + RCLCPP_INFO_THROTTLE( + this->get_logger(), *this->get_clock(), 5000 /* ms */, "waiting for current_accel msg..."); + return false; + } + + if (!is_stopped_ptr_) { + RCLCPP_INFO_THROTTLE( + this->get_logger(), *this->get_clock(), 5000 /* ms */, "waiting for is_stopped msg..."); + return false; + } + + if (!cli_set_stop_->service_is_ready()) { + RCLCPP_INFO_THROTTLE( + this->get_logger(), *this->get_clock(), 5000 /* ms */, "waiting for stop service..."); + return false; + } + + return true; +} + +bool PredictedPathCheckerNode::isDataTimeout() +{ + const auto now = this->now(); + + constexpr double th_pose_timeout = 1.0; + const auto pose_time_diff = rclcpp::Time(current_pose_->header.stamp).seconds() - now.seconds(); + if (pose_time_diff > th_pose_timeout) { + RCLCPP_WARN_THROTTLE( + this->get_logger(), *this->get_clock(), 5000 /* ms */, "pose is timeout..."); + return true; + } + + return false; +} + +void PredictedPathCheckerNode::onTimer() +{ + current_pose_ = self_pose_listener_->getCurrentPose(); + + if (!isDataReady()) { + return; + } + + if (isDataTimeout()) { + return; + } + + // Cut trajectory + const auto cut_trajectory = cutTrajectory( + *predicted_trajectory_, std::max( + node_param_.min_trajectory_check_length, + current_twist_->linear.x * node_param_.trajectory_check_time)); + + // Convert to trajectory array + + TrajectoryPoints predicted_trajectory_array = motion_utils::convertToTrajectoryPointArray( + motion_utils::resampleTrajectory(cut_trajectory, node_param_.resample_interval)); + + // Filter the objects + + PredictedObjects filtered_objects; + filterObstacles( + current_pose_.get()->pose, predicted_trajectory_array, node_param_.filtering_distance_threshold, + node_param_.use_object_prediction, filtered_objects); + + PredictedObjects::ConstSharedPtr filtered_obj_ptr = + std::make_shared(filtered_objects); + + // Check collision + + const auto collision_checker_output = + collision_checker_->checkTrajectoryForCollision(predicted_trajectory_array, filtered_obj_ptr); + + if (!collision_checker_output) { + // There is no need to stop + if (is_stopped_by_node_) { + sendRequest(false); + } + current_state_ = State::DRIVE; + updater_.force_update(); + debug_ptr_->publish(); + + return; + } + + // Extend trajectory + + extendTrajectoryPointsArray(predicted_trajectory_array); + + // Insert collision and stop points + + const auto stop_idx = + insertStopPoint(predicted_trajectory_array, collision_checker_output.get().first); + + // Check ego vehicle is stopped or not + constexpr double th_stopped_velocity = 0.001; + const bool is_ego_vehicle_stopped = current_twist_->linear.x < th_stopped_velocity; + + // If ego vehicle is not stopped, check obstacle is in the brake distance + if (!is_ego_vehicle_stopped) { + // Calculate projected velocity and acceleration of the object on the trajectory point + + const auto projected_obj_vel_acc = calculateProjectedVelAndAcc( + collision_checker_output->second, predicted_trajectory_array.at(stop_idx)); + + // Calculate relative velocity and acceleration wrt the object + + const double relative_velocity = current_twist_->linear.x - projected_obj_vel_acc.first; + const double relative_acceleration = + current_accel_->accel.accel.linear.x - projected_obj_vel_acc.second; + + // Check if the vehicle is in the brake distance + + const bool is_in_brake_distance = utils::isInBrakeDistance( + predicted_trajectory_array, stop_idx, relative_velocity, relative_acceleration, + node_param_.max_deceleration, node_param_.delay_time); + + if (is_in_brake_distance) { + // Send emergency and stop request + current_state_ = State::EMERGENCY; + updater_.force_update(); + if (!is_stopped_by_node_) { + sendRequest(true); + } + debug_ptr_->publish(); + RCLCPP_ERROR_THROTTLE( + this->get_logger(), *this->get_clock(), 5000 /* ms */, + "There is an obstacle in the brake distance. Sending emergency and stop request..."); + return; + } + } + + // If it is not in the brake distance, check if the collision point is discrete from the reference + // trajectory or not + + const auto reference_trajectory_array = + motion_utils::convertToTrajectoryPointArray(*reference_trajectory_); + + const auto is_discrete_point = + isItDiscretePoint(reference_trajectory_array, predicted_trajectory_array.at(stop_idx)); + + if (is_discrete_point) { + // Check reference trajectory has stop point or not + + const auto is_there_stop_in_ref_trajectory = isThereStopPointOnReferenceTrajectory( + predicted_trajectory_array.at(stop_idx).pose, reference_trajectory_array); + + if (!is_there_stop_in_ref_trajectory) { + // Send stop + if (!is_stopped_by_node_) { + sendRequest(true); + } + current_state_ = State::STOP; + updater_.force_update(); + + RCLCPP_WARN_THROTTLE( + this->get_logger(), *this->get_clock(), 5000 /* ms */, + "There is an obstacle on predicted path. Sending stop request..."); + + debug_ptr_->publish(); + return; + } + } + + // If it is not discrete point, planning should handle it. Send drive. + current_state_ = State::DRIVE; + updater_.force_update(); + + if (is_stopped_by_node_) { + sendRequest(false); + } + + debug_ptr_->publish(); +} + +TrajectoryPoints PredictedPathCheckerNode::trimTrajectoryFromSelfPose( + const TrajectoryPoints & input, const Pose & self_pose) const +{ + TrajectoryPoints output{}; + + const size_t min_distance_index = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + input, self_pose, node_param_.ego_nearest_dist_threshold, + node_param_.ego_nearest_yaw_threshold) + + 1; + + for (size_t i = min_distance_index; i < input.size(); ++i) { + output.push_back(input.at(i)); + } + + return output; +} + +bool PredictedPathCheckerNode::isThereStopPointOnReferenceTrajectory( + const geometry_msgs::msg::Pose & pose, const TrajectoryPoints & reference_trajectory_array) +{ + const auto trimmed_reference_trajectory_array = + trimTrajectoryFromSelfPose(reference_trajectory_array, current_pose_.get()->pose); + + const auto nearest_stop_point_on_ref_trajectory = + motion_utils::findNearestIndex(trimmed_reference_trajectory_array, pose); + + const auto stop_point_on_trajectory = motion_utils::searchZeroVelocityIndex( + trimmed_reference_trajectory_array, 0, *nearest_stop_point_on_ref_trajectory); + + return !!stop_point_on_trajectory; +} + +void PredictedPathCheckerNode::checkVehicleState(diagnostic_updater::DiagnosticStatusWrapper & stat) +{ + int8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; + std::string msg = "OK"; + + if (current_state_ == State::EMERGENCY) { + level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + msg = "vehicle will collide with obstacles"; + } + if (current_state_ == State::STOP) { + level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + msg = "vehicle will stop due to obstacle"; + } + + stat.summary(level, msg); +} + +void PredictedPathCheckerNode::sendRequest(bool make_stop_vehicle) +{ + if (!is_calling_set_stop_ && cli_set_stop_->service_is_ready()) { + const auto req = std::make_shared(); + req->stop = make_stop_vehicle; + req->request_source = "predicted_path_checker"; + is_calling_set_stop_ = true; + cli_set_stop_->async_send_request(req, [this](auto) { is_calling_set_stop_ = false; }); + } +} + +bool PredictedPathCheckerNode::isItDiscretePoint( + const TrajectoryPoints & reference_trajectory, const TrajectoryPoint & collision_point) const +{ + const auto nearest_segment = + motion_utils::findNearestSegmentIndex(reference_trajectory, collision_point.pose); + + const auto nearest_point = utils::calcInterpolatedPoint( + reference_trajectory, collision_point.pose.position, *nearest_segment, false); + + const auto distance = tier4_autoware_utils::calcDistance2d( + nearest_point.pose.position, collision_point.pose.position); + + const auto yaw_diff = + std::abs(tier4_autoware_utils::calcYawDeviation(nearest_point.pose, collision_point.pose)); + return distance >= node_param_.distinct_point_distance_threshold || + yaw_diff >= tier4_autoware_utils::deg2rad(node_param_.distinct_point_yaw_threshold); +} + +Trajectory PredictedPathCheckerNode::cutTrajectory( + const Trajectory & trajectory, const double length) +{ + Trajectory cut; + cut.header = trajectory.header; + if (trajectory.points.empty()) { + return cut; + } + double total_length = 0.0; + cut.points.push_back(trajectory.points.front()); + for (size_t i = 1; i < trajectory.points.size(); ++i) { + const auto & point = trajectory.points.at(i); + + const auto p1 = tier4_autoware_utils::fromMsg(cut.points.back().pose.position); + const auto p2 = tier4_autoware_utils::fromMsg(point.pose.position); + const auto points_distance = boost::geometry::distance(p1.to_2d(), p2.to_2d()); + + const auto remain_distance = length - total_length; + + // Over length + if (remain_distance <= 0.001) { + break; + } + + // Require interpolation + if (remain_distance <= points_distance) { + const Eigen::Vector3d p_interpolated = p1 + remain_distance * (p2 - p1).normalized(); + + TrajectoryPoint p; + p.pose.position.x = p_interpolated.x(); + p.pose.position.y = p_interpolated.y(); + p.pose.position.z = p_interpolated.z(); + p.pose.orientation = point.pose.orientation; + + cut.points.push_back(p); + break; + } + + cut.points.push_back(point); + total_length += points_distance; + } + motion_utils::removeOverlapPoints(cut.points); + + return cut; +} + +void PredictedPathCheckerNode::extendTrajectoryPointsArray(TrajectoryPoints & trajectory) +{ + // It extends the trajectory to the end of the footprint of the vehicle to get better distance to + // collision_point. + const double extend_distance = vehicle_info_.max_longitudinal_offset_m + node_param_.stop_margin; + const auto & goal_point = trajectory.back(); + const auto trajectory_point_extend = utils::getExtendTrajectoryPoint(extend_distance, goal_point); + trajectory.push_back(trajectory_point_extend); +} + +size_t PredictedPathCheckerNode::insertStopPoint( + TrajectoryPoints & trajectory, const geometry_msgs::msg::Point collision_point) +{ + const auto nearest_collision_segment = + motion_utils::findNearestSegmentIndex(trajectory, collision_point); + + const auto nearest_collision_point = + utils::calcInterpolatedPoint(trajectory, collision_point, nearest_collision_segment, true); + + const size_t collision_idx = nearest_collision_segment + 1; + + trajectory.insert(trajectory.begin() + static_cast(collision_idx), nearest_collision_point); + + const auto stop_point = + utils::findStopPoint(trajectory, collision_idx, node_param_.stop_margin, vehicle_info_); + + const size_t stop_idx = stop_point.first + 1; + trajectory.insert(trajectory.begin() + static_cast(stop_idx), stop_point.second); + + debug_ptr_->pushPose(stop_point.second.pose, PoseType::Stop); + return stop_idx; +} + +std::pair PredictedPathCheckerNode::calculateProjectedVelAndAcc( + const PredictedObject & object, const TrajectoryPoint & trajectory_point) +{ + const auto & orientation_obj = object.kinematics.initial_pose_with_covariance.pose.orientation; + const auto & orientation_stop_point = trajectory_point.pose.orientation; + const auto velocity_obj = object.kinematics.initial_twist_with_covariance.twist.linear.x; + const auto acceleration_obj = + object.kinematics.initial_acceleration_with_covariance.accel.linear.x; + const auto k = std::cos(tier4_autoware_utils::normalizeRadian( + tf2::getYaw(orientation_obj) - tf2::getYaw(orientation_stop_point))); + const auto projected_velocity = velocity_obj * k; + const auto projected_acceleration = acceleration_obj * k; + return std::make_pair(projected_velocity, projected_acceleration); +} + +void PredictedPathCheckerNode::filterObstacles( + const Pose & ego_pose, const TrajectoryPoints & traj, const double dist_threshold, + const bool use_prediction, PredictedObjects & filtered_objects) +{ + filtered_objects.header.frame_id = object_ptr_.get()->header.frame_id; + filtered_objects.header.stamp = this->now(); + + for (auto & object : object_ptr_.get()->objects) { + // Check is it in front of ego vehicle + if (!utils::isFrontObstacle( + ego_pose, object.kinematics.initial_pose_with_covariance.pose.position)) { + continue; + } + + // Check is it near to trajectory + const double max_length = utils::calcObstacleMaxLength(object.shape); + const size_t seg_idx = motion_utils::findNearestSegmentIndex( + traj, object.kinematics.initial_pose_with_covariance.pose.position); + const auto p_front = tier4_autoware_utils::getPoint(traj.at(seg_idx)); + const auto p_back = tier4_autoware_utils::getPoint(traj.at(seg_idx + 1)); + const auto & p_target = object.kinematics.initial_pose_with_covariance.pose.position; + const Eigen::Vector3d segment_vec{p_back.x - p_front.x, p_back.y - p_front.y, 0.0}; + const Eigen::Vector3d target_vec{p_target.x - p_front.x, p_target.y - p_front.y, 0.0}; + + if (seg_idx == traj.size() - 2) { + // Calculate longitudinal offset + const auto longitudinal_dist = std::abs(segment_vec.dot(target_vec) / segment_vec.norm()); + if ( + longitudinal_dist - max_length - vehicle_info_.max_longitudinal_offset_m - dist_threshold > + 0.0) { + continue; + } + } + const auto lateral_dist = std::abs(segment_vec.cross(target_vec)(2) / segment_vec.norm()); + if (lateral_dist - max_length - vehicle_info_.max_lateral_offset_m - dist_threshold > 0.0) { + continue; + } + PredictedObject filtered_object = object; + if (use_prediction) { + utils::getCurrentObjectPose(filtered_object, object_ptr_.get()->header.stamp, this->now()); + } + filtered_objects.objects.push_back(filtered_object); + } +} + +} // namespace autoware::motion::control::predicted_path_checker + +#include +RCLCPP_COMPONENTS_REGISTER_NODE( + autoware::motion::control::predicted_path_checker::PredictedPathCheckerNode) diff --git a/control/predicted_path_checker/src/predicted_path_checker_node/utils.cpp b/control/predicted_path_checker/src/predicted_path_checker_node/utils.cpp new file mode 100644 index 0000000000000..9cb095e908d41 --- /dev/null +++ b/control/predicted_path_checker/src/predicted_path_checker_node/utils.cpp @@ -0,0 +1,429 @@ +// Copyright 2023 LeoDrive A.Ş. All rights reserved. +// +// 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 "predicted_path_checker/utils.hpp" + +#include +#include +#include + +namespace utils +{ + +using motion_utils::findFirstNearestIndexWithSoftConstraints; +using motion_utils::findFirstNearestSegmentIndexWithSoftConstraints; +using tier4_autoware_utils::calcDistance2d; +using tier4_autoware_utils::getRPY; + +// Utils Functions +Polygon2d createOneStepPolygon( + const geometry_msgs::msg::Pose & base_step_pose, const geometry_msgs::msg::Pose & next_step_pose, + const vehicle_info_util::VehicleInfo & vehicle_info, const double expand_width) +{ + Polygon2d polygon; + + const double longitudinal_offset = vehicle_info.max_longitudinal_offset_m; + const double width = vehicle_info.vehicle_width_m / 2.0 + expand_width; + const double rear_overhang = vehicle_info.rear_overhang_m; + + { // base step + appendPointToPolygon( + polygon, tier4_autoware_utils::calcOffsetPose(base_step_pose, longitudinal_offset, width, 0.0) + .position); + appendPointToPolygon( + polygon, + tier4_autoware_utils::calcOffsetPose(base_step_pose, longitudinal_offset, -width, 0.0) + .position); + appendPointToPolygon( + polygon, + tier4_autoware_utils::calcOffsetPose(base_step_pose, -rear_overhang, -width, 0.0).position); + appendPointToPolygon( + polygon, + tier4_autoware_utils::calcOffsetPose(base_step_pose, -rear_overhang, width, 0.0).position); + } + + { // next step + appendPointToPolygon( + polygon, tier4_autoware_utils::calcOffsetPose(next_step_pose, longitudinal_offset, width, 0.0) + .position); + appendPointToPolygon( + polygon, + tier4_autoware_utils::calcOffsetPose(next_step_pose, longitudinal_offset, -width, 0.0) + .position); + appendPointToPolygon( + polygon, + tier4_autoware_utils::calcOffsetPose(next_step_pose, -rear_overhang, -width, 0.0).position); + appendPointToPolygon( + polygon, + tier4_autoware_utils::calcOffsetPose(next_step_pose, -rear_overhang, width, 0.0).position); + } + + polygon = tier4_autoware_utils::isClockwise(polygon) + ? polygon + : tier4_autoware_utils::inverseClockwise(polygon); + + Polygon2d hull_polygon; + boost::geometry::convex_hull(polygon, hull_polygon); + boost::geometry::correct(hull_polygon); + + return hull_polygon; +} + +TrajectoryPoint calcInterpolatedPoint( + const TrajectoryPoints & trajectory, const geometry_msgs::msg::Point & target_point, + const size_t segment_idx, const bool use_zero_order_hold_for_twist) +{ + if (trajectory.empty()) { + TrajectoryPoint interpolated_point{}; + interpolated_point.pose.position = target_point; + return interpolated_point; + } else if (trajectory.size() == 1) { + return trajectory.front(); + } + + // Calculate interpolation ratio + const auto & curr_pt = trajectory.at(segment_idx); + const auto & next_pt = trajectory.at(segment_idx + 1); + const auto v1 = tier4_autoware_utils::point2tfVector(curr_pt, next_pt); + const auto v2 = tier4_autoware_utils::point2tfVector(curr_pt, target_point); + if (v1.length2() < 1e-3) { + return curr_pt; + } + + const double ratio = v1.dot(v2) / v1.length2(); + const double clamped_ratio = std::clamp(ratio, 0.0, 1.0); + + // Interpolate + TrajectoryPoint interpolated_point{}; + + // pose interpolation + interpolated_point.pose = + tier4_autoware_utils::calcInterpolatedPose(curr_pt, next_pt, clamped_ratio); + + // twist interpolation + if (use_zero_order_hold_for_twist) { + interpolated_point.longitudinal_velocity_mps = curr_pt.longitudinal_velocity_mps; + interpolated_point.lateral_velocity_mps = curr_pt.lateral_velocity_mps; + interpolated_point.acceleration_mps2 = curr_pt.acceleration_mps2; + } else { + interpolated_point.longitudinal_velocity_mps = interpolation::lerp( + curr_pt.longitudinal_velocity_mps, next_pt.longitudinal_velocity_mps, clamped_ratio); + interpolated_point.lateral_velocity_mps = interpolation::lerp( + curr_pt.lateral_velocity_mps, next_pt.lateral_velocity_mps, clamped_ratio); + interpolated_point.acceleration_mps2 = + interpolation::lerp(curr_pt.acceleration_mps2, next_pt.acceleration_mps2, clamped_ratio); + } + + // heading rate interpolation + interpolated_point.heading_rate_rps = + interpolation::lerp(curr_pt.heading_rate_rps, next_pt.heading_rate_rps, clamped_ratio); + + // wheel interpolation + interpolated_point.front_wheel_angle_rad = interpolation::lerp( + curr_pt.front_wheel_angle_rad, next_pt.front_wheel_angle_rad, clamped_ratio); + interpolated_point.rear_wheel_angle_rad = + interpolation::lerp(curr_pt.rear_wheel_angle_rad, next_pt.rear_wheel_angle_rad, clamped_ratio); + + // time interpolation + const double interpolated_time = interpolation::lerp( + rclcpp::Duration(curr_pt.time_from_start).seconds(), + rclcpp::Duration(next_pt.time_from_start).seconds(), clamped_ratio); + interpolated_point.time_from_start = rclcpp::Duration::from_seconds(interpolated_time); + + return interpolated_point; +} + +std::pair findStopPoint( + TrajectoryPoints & trajectory_array, const size_t collision_idx, const double stop_margin, + vehicle_info_util::VehicleInfo & vehicle_info) +{ + // It returns the stop point and segment of the point on trajectory. + + double desired_distance_base_link_to_collision = + vehicle_info.max_longitudinal_offset_m + stop_margin; + size_t stop_segment_idx = 0; + bool found_stop_point = false; + double distance_point_to_collision = 0.0; + + for (size_t i = collision_idx; i > 0; i--) { + distance_point_to_collision = + motion_utils::calcSignedArcLength(trajectory_array, i - 1, collision_idx); + if (distance_point_to_collision >= desired_distance_base_link_to_collision) { + stop_segment_idx = i - 1; + found_stop_point = true; + break; + } + } + if (found_stop_point) { + const auto & base_point = trajectory_array.at(stop_segment_idx); + const auto & next_point = trajectory_array.at(stop_segment_idx + 1); + + double ratio = (distance_point_to_collision - desired_distance_base_link_to_collision) / + (std::hypot( + base_point.pose.position.x - next_point.pose.position.x, + base_point.pose.position.y - next_point.pose.position.y)); + + geometry_msgs::msg::Pose interpolated_pose = + tier4_autoware_utils::calcInterpolatedPose(base_point.pose, next_point.pose, ratio, false); + TrajectoryPoint output; + output.set__pose(interpolated_pose); + return std::make_pair(stop_segment_idx, output); + } else { + // It means that there is no enough distance between vehicle and collision point. + // So, we insert a stop at the first point of the trajectory. + return std::make_pair(0, trajectory_array.front()); + } +} + +bool isInBrakeDistance( + const TrajectoryPoints & trajectory, const size_t stop_idx, const double relative_velocity, + const double relative_acceleration, const double max_deceleration, const double delay_time_sec) +{ + if (relative_velocity < 0.0) { + return false; + } + + const auto distance_to_obstacle = motion_utils::calcSignedArcLength( + trajectory, trajectory.front().pose.position, trajectory.at(stop_idx).pose.position); + + const double distance_in_delay = relative_velocity * delay_time_sec + + relative_acceleration * delay_time_sec * delay_time_sec * 0.5; + + const double velocity_after_delay = relative_velocity + relative_acceleration * delay_time_sec; + + const double time_to_stop = velocity_after_delay / std::abs(max_deceleration); + const double distance_after_delay = + velocity_after_delay * time_to_stop - 0.5 * abs(max_deceleration) * time_to_stop * time_to_stop; + const double brake_distance = distance_in_delay + distance_after_delay; + + return brake_distance > distance_to_obstacle; +} + +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; +} + +bool intersectsInZAxis(const PredictedObject & object, const double z_min, const double z_max) +{ + const auto & obj_pose = object.kinematics.initial_pose_with_covariance; + const auto & obj_height = object.shape.dimensions.z; + return obj_pose.pose.position.z - obj_height / 2.0 <= z_max && + obj_pose.pose.position.z + obj_height / 2.0 >= z_min; +} + +double getNearestPointAndDistanceForPredictedObject( + const PointArray & points, const Pose & base_pose, + geometry_msgs::msg::Point * nearest_collision_point) +{ + double min_norm = 0.0; + bool is_init = false; + + for (const auto & p : points) { + double norm = tier4_autoware_utils::calcDistance2d(p, base_pose); + if (norm < min_norm || !is_init) { + min_norm = norm; + *nearest_collision_point = p; + is_init = true; + } + } + return min_norm; +} + +Polygon2d convertBoundingBoxObjectToGeometryPolygon( + 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 + 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::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()); + object_polygon.outer().emplace_back(p4_obj.x(), p4_obj.y()); + + object_polygon.outer().push_back(object_polygon.outer().front()); + boost::geometry::correct(object_polygon); + + return object_polygon; +} + +Polygon2d convertCylindricalObjectToGeometryPolygon( + const Pose & current_pose, const autoware_auto_perception_msgs::msg::Shape & obj_shape) +{ + Polygon2d object_polygon; + + const double obj_x = current_pose.position.x; + const double obj_y = current_pose.position.y; + + constexpr int N = 20; + const double r = obj_shape.dimensions.x / 2; + object_polygon.outer().reserve(N + 1); + for (int i = 0; i < N; ++i) { + object_polygon.outer().emplace_back( + obj_x + r * std::cos(2.0 * M_PI / N * i), obj_y + r * std::sin(2.0 * M_PI / N * i)); + } + + object_polygon.outer().push_back(object_polygon.outer().front()); + boost::geometry::correct(object_polygon); + + return object_polygon; +} + +Polygon2d convertPolygonObjectToGeometryPolygon( + const Pose & current_pose, const autoware_auto_perception_msgs::msg::Shape & obj_shape) +{ + Polygon2d object_polygon; + tf2::Transform tf_map2obj; + fromMsg(current_pose, tf_map2obj); + const auto obj_points = obj_shape.footprint.points; + object_polygon.outer().reserve(obj_points.size() + 1); + for (const auto & obj_point : obj_points) { + tf2::Vector3 obj(obj_point.x, obj_point.y, obj_point.z); + tf2::Vector3 tf_obj = tf_map2obj * obj; + object_polygon.outer().emplace_back(tf_obj.x(), tf_obj.y()); + } + object_polygon.outer().push_back(object_polygon.outer().front()); + boost::geometry::correct(object_polygon); + + return object_polygon; +} + +void appendPointToPolygon(Polygon2d & polygon, const geometry_msgs::msg::Point & geom_point) +{ + Point2d point; + point.x() = geom_point.x; + point.y() = geom_point.y; + + boost::geometry::append(polygon.outer(), point); +} + +Polygon2d convertObjToPolygon(const PredictedObject & obj) +{ + Polygon2d object_polygon{}; + if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { + object_polygon = utils::convertCylindricalObjectToGeometryPolygon( + obj.kinematics.initial_pose_with_covariance.pose, obj.shape); + } else if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + const double & length_m = obj.shape.dimensions.x / 2; + const double & width_m = obj.shape.dimensions.y / 2; + object_polygon = utils::convertBoundingBoxObjectToGeometryPolygon( + obj.kinematics.initial_pose_with_covariance.pose, length_m, length_m, width_m); + } else if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::POLYGON) { + object_polygon = utils::convertPolygonObjectToGeometryPolygon( + obj.kinematics.initial_pose_with_covariance.pose, obj.shape); + } else { + throw std::runtime_error("Unsupported shape type"); + } + return object_polygon; +} + +bool isFrontObstacle(const Pose & ego_pose, const geometry_msgs::msg::Point & obstacle_pos) +{ + const auto yaw = tier4_autoware_utils::getRPY(ego_pose).z; + const Eigen::Vector2d base_pose_vec(std::cos(yaw), std::sin(yaw)); + const Eigen::Vector2d obstacle_vec( + obstacle_pos.x - ego_pose.position.x, obstacle_pos.y - ego_pose.position.y); + + return base_pose_vec.dot(obstacle_vec) >= 0; +} + +double calcObstacleMaxLength(const autoware_auto_perception_msgs::msg::Shape & shape) +{ + if (shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + return std::hypot(shape.dimensions.x / 2.0, shape.dimensions.y / 2.0); + } else if (shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { + return shape.dimensions.x / 2.0; + } else if (shape.type == autoware_auto_perception_msgs::msg::Shape::POLYGON) { + double max_length_to_point = 0.0; + for (const auto rel_point : shape.footprint.points) { + const double length_to_point = std::hypot(rel_point.x, rel_point.y); + if (max_length_to_point < length_to_point) { + max_length_to_point = length_to_point; + } + } + return max_length_to_point; + } + + throw std::logic_error("The shape type is not supported in obstacle_cruise_planner."); +} + +void getCurrentObjectPose( + PredictedObject & predicted_object, const rclcpp::Time & obj_base_time, + const rclcpp::Time & current_time) +{ + const double yaw = tier4_autoware_utils::getRPY( + predicted_object.kinematics.initial_pose_with_covariance.pose.orientation) + .z; + const double vx = predicted_object.kinematics.initial_twist_with_covariance.twist.linear.x; + const double ax = predicted_object.kinematics.initial_acceleration_with_covariance.accel.linear.x; + + const double dt = (current_time - obj_base_time).seconds(); + const double ds = vx * dt + 0.5 * ax * dt * dt; + const double delta_yaw = + predicted_object.kinematics.initial_twist_with_covariance.twist.angular.z * dt; + geometry_msgs::msg::Transform transform; + transform.translation = tier4_autoware_utils::createTranslation(ds, 0.0, 0.0); + transform.rotation = tier4_autoware_utils::createQuaternionFromRPY(0.0, 0.0, yaw); + + tf2::Transform tf_pose; + tf2::Transform tf_offset; + tf2::fromMsg(transform, tf_offset); + tf2::fromMsg(predicted_object.kinematics.initial_pose_with_covariance.pose, tf_pose); + tf2::toMsg(tf_pose * tf_offset, predicted_object.kinematics.initial_pose_with_covariance.pose); + predicted_object.kinematics.initial_twist_with_covariance.twist.linear.x += ax * dt; + predicted_object.kinematics.initial_pose_with_covariance.pose.orientation = + tier4_autoware_utils::createQuaternionFromRPY( + 0.0, 0.0, tier4_autoware_utils::normalizeRadian(yaw + delta_yaw)); +} + +} // namespace utils diff --git a/control/pure_pursuit/include/pure_pursuit/pure_pursuit_lateral_controller.hpp b/control/pure_pursuit/include/pure_pursuit/pure_pursuit_lateral_controller.hpp index e9e57cf08bcc4..daaebaacde8de 100644 --- a/control/pure_pursuit/include/pure_pursuit/pure_pursuit_lateral_controller.hpp +++ b/control/pure_pursuit/include/pure_pursuit/pure_pursuit_lateral_controller.hpp @@ -100,10 +100,12 @@ struct DebugData class PurePursuitLateralController : public LateralControllerBase { public: + /// \param node Reference to the node used only for the component and parameter initialization. explicit PurePursuitLateralController(rclcpp::Node & node); private: - rclcpp::Node::SharedPtr node_; + rclcpp::Clock::SharedPtr clock_; + rclcpp::Logger logger_; std::vector output_tp_array_; autoware_auto_planning_msgs::msg::Trajectory::SharedPtr trajectory_resampled_; autoware_auto_planning_msgs::msg::Trajectory trajectory_; diff --git a/control/pure_pursuit/src/pure_pursuit/pure_pursuit_lateral_controller.cpp b/control/pure_pursuit/src/pure_pursuit/pure_pursuit_lateral_controller.cpp index cb812202d8651..f0c49b07e675a 100644 --- a/control/pure_pursuit/src/pure_pursuit/pure_pursuit_lateral_controller.cpp +++ b/control/pure_pursuit/src/pure_pursuit/pure_pursuit_lateral_controller.cpp @@ -57,48 +57,46 @@ enum TYPE { namespace pure_pursuit { PurePursuitLateralController::PurePursuitLateralController(rclcpp::Node & node) -: node_{&node}, tf_buffer_(node_->get_clock()), tf_listener_(tf_buffer_) +: clock_(node.get_clock()), + logger_(node.get_logger().get_child("lateral_controller")), + tf_buffer_(clock_), + tf_listener_(tf_buffer_) { pure_pursuit_ = std::make_unique(); // Vehicle Parameters - const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(*node_).getVehicleInfo(); + const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo(); param_.wheel_base = vehicle_info.wheel_base_m; param_.max_steering_angle = vehicle_info.max_steer_angle_rad; // Algorithm Parameters - param_.ld_velocity_ratio = node_->declare_parameter("ld_velocity_ratio"); - param_.ld_lateral_error_ratio = node_->declare_parameter("ld_lateral_error_ratio"); - param_.ld_curvature_ratio = node_->declare_parameter("ld_curvature_ratio"); + param_.ld_velocity_ratio = node.declare_parameter("ld_velocity_ratio"); + param_.ld_lateral_error_ratio = node.declare_parameter("ld_lateral_error_ratio"); + param_.ld_curvature_ratio = node.declare_parameter("ld_curvature_ratio"); param_.long_ld_lateral_error_threshold = - node_->declare_parameter("long_ld_lateral_error_threshold"); - param_.min_lookahead_distance = node_->declare_parameter("min_lookahead_distance"); - param_.max_lookahead_distance = node_->declare_parameter("max_lookahead_distance"); + node.declare_parameter("long_ld_lateral_error_threshold"); + param_.min_lookahead_distance = node.declare_parameter("min_lookahead_distance"); + param_.max_lookahead_distance = node.declare_parameter("max_lookahead_distance"); param_.reverse_min_lookahead_distance = - node_->declare_parameter("reverse_min_lookahead_distance"); - param_.converged_steer_rad_ = node_->declare_parameter("converged_steer_rad"); - param_.prediction_ds = node_->declare_parameter("prediction_ds"); - param_.prediction_distance_length = - node_->declare_parameter("prediction_distance_length"); - param_.resampling_ds = node_->declare_parameter("resampling_ds"); + node.declare_parameter("reverse_min_lookahead_distance"); + param_.converged_steer_rad_ = node.declare_parameter("converged_steer_rad"); + param_.prediction_ds = node.declare_parameter("prediction_ds"); + param_.prediction_distance_length = node.declare_parameter("prediction_distance_length"); + param_.resampling_ds = node.declare_parameter("resampling_ds"); param_.curvature_calculation_distance = - node_->declare_parameter("curvature_calculation_distance"); - param_.enable_path_smoothing = node_->declare_parameter("enable_path_smoothing"); - param_.path_filter_moving_ave_num = - node_->declare_parameter("path_filter_moving_ave_num"); + node.declare_parameter("curvature_calculation_distance"); + param_.enable_path_smoothing = node.declare_parameter("enable_path_smoothing"); + param_.path_filter_moving_ave_num = node.declare_parameter("path_filter_moving_ave_num"); // Debug Publishers pub_debug_marker_ = - node_->create_publisher("~/debug/markers", 0); - pub_debug_values_ = node_->create_publisher( + node.create_publisher("~/debug/markers", 0); + pub_debug_values_ = node.create_publisher( "~/debug/ld_outputs", rclcpp::QoS{1}); // Publish predicted trajectory - pub_predicted_trajectory_ = node_->create_publisher( + pub_predicted_trajectory_ = node.create_publisher( "~/output/predicted_trajectory", 1); - - // Wait for first current pose - tf_utils::waitForTransform(tf_buffer_, "map", "base_link"); } double PurePursuitLateralController::calcLookaheadDistance( @@ -128,7 +126,7 @@ double PurePursuitLateralController::calcLookaheadDistance( debug_msg.data.at(TYPE::VELOCITY) = static_cast(velocity); debug_msg.data.at(TYPE::CURVATURE) = static_cast(curvature); debug_msg.data.at(TYPE::LATERAL_ERROR) = static_cast(lateral_error); - debug_msg.stamp = node_->now(); + debug_msg.stamp = clock_->now(); pub_debug_values_->publish(debug_msg); }; @@ -220,7 +218,7 @@ void PurePursuitLateralController::averageFilterTrajectory( autoware_auto_planning_msgs::msg::Trajectory & u) { if (static_cast(u.points.size()) <= 2 * param_.path_filter_moving_ave_num) { - RCLCPP_ERROR(node_->get_logger(), "Cannot smooth path! Trajectory size is too low!"); + RCLCPP_ERROR(logger_, "Cannot smooth path! Trajectory size is too low!"); return; } @@ -303,9 +301,7 @@ boost::optional PurePursuitLateralController::generatePredictedTraje tmp_msg = generateCtrlCmdMsg(pp_output->curvature); predicted_trajectory.points.at(i).longitudinal_velocity_mps = pp_output->velocity; } else { - RCLCPP_WARN_THROTTLE( - node_->get_logger(), *node_->get_clock(), 5000, - "failed to solve pure_pursuit for prediction"); + RCLCPP_WARN_THROTTLE(logger_, *clock_, 5000, "failed to solve pure_pursuit for prediction"); tmp_msg = generateCtrlCmdMsg(0.0); } TrajectoryPoint p2; @@ -320,9 +316,7 @@ boost::optional PurePursuitLateralController::generatePredictedTraje tmp_msg = generateCtrlCmdMsg(pp_output->curvature); predicted_trajectory.points.at(i).longitudinal_velocity_mps = pp_output->velocity; } else { - RCLCPP_WARN_THROTTLE( - node_->get_logger(), *node_->get_clock(), 5000, - "failed to solve pure_pursuit for prediction"); + RCLCPP_WARN_THROTTLE(logger_, *clock_, 5000, "failed to solve pure_pursuit for prediction"); tmp_msg = generateCtrlCmdMsg(0.0); } predicted_trajectory.points.push_back( @@ -363,7 +357,7 @@ LateralOutput PurePursuitLateralController::run(const InputData & input_data) // calculate predicted trajectory with iterative calculation const auto predicted_trajectory = generatePredictedTrajectory(); if (!predicted_trajectory) { - RCLCPP_ERROR(node_->get_logger(), "Failed to generate predicted trajectory."); + RCLCPP_ERROR(logger_, "Failed to generate predicted trajectory."); } else { pub_predicted_trajectory_->publish(*predicted_trajectory); } @@ -389,8 +383,7 @@ AckermannLateralCommand PurePursuitLateralController::generateOutputControlCmd() publishDebugMarker(); } else { RCLCPP_WARN_THROTTLE( - node_->get_logger(), *node_->get_clock(), 5000, - "failed to solve pure_pursuit for control command calculation"); + logger_, *clock_, 5000, "failed to solve pure_pursuit for control command calculation"); if (prev_cmd_) { output_cmd = *prev_cmd_; } else { @@ -406,7 +399,7 @@ AckermannLateralCommand PurePursuitLateralController::generateCtrlCmdMsg( const double tmp_steering = planning_utils::convertCurvatureToSteeringAngle(param_.wheel_base, target_curvature); AckermannLateralCommand cmd; - cmd.stamp = node_->get_clock()->now(); + cmd.stamp = clock_->now(); cmd.steering_tire_angle = static_cast( std::min(std::max(tmp_steering, -param_.max_steering_angle), param_.max_steering_angle)); @@ -428,8 +421,7 @@ boost::optional PurePursuitLateralController::calcTargetCurvature( { // Ignore invalid trajectory if (trajectory_resampled_->points.size() < 3) { - RCLCPP_WARN_THROTTLE( - node_->get_logger(), *node_->get_clock(), 5000, "received path size is < 3, ignored"); + RCLCPP_WARN_THROTTLE(logger_, *clock_, 5000, "received path size is < 3, ignored"); return {}; } @@ -438,7 +430,7 @@ boost::optional PurePursuitLateralController::calcTargetCurvature( const auto closest_idx_result = motion_utils::findNearestIndex(output_tp_array_, pose, 3.0, M_PI_4); if (!closest_idx_result) { - RCLCPP_ERROR(node_->get_logger(), "cannot find closest waypoint"); + RCLCPP_ERROR(logger_, "cannot find closest waypoint"); return {}; } diff --git a/control/trajectory_follower_node/include/trajectory_follower_node/controller_node.hpp b/control/trajectory_follower_node/include/trajectory_follower_node/controller_node.hpp index 4868fa7a6f51d..c108bec2671b3 100644 --- a/control/trajectory_follower_node/include/trajectory_follower_node/controller_node.hpp +++ b/control/trajectory_follower_node/include/trajectory_follower_node/controller_node.hpp @@ -19,6 +19,7 @@ #include "tf2/utils.h" #include "tf2_ros/buffer.h" #include "tf2_ros/transform_listener.h" +#include "tier4_autoware_utils/ros/logger_level_configure.hpp" #include "trajectory_follower_base/lateral_controller_base.hpp" #include "trajectory_follower_base/longitudinal_controller_base.hpp" #include "trajectory_follower_node/visibility_control.hpp" @@ -36,6 +37,7 @@ #include "geometry_msgs/msg/pose_stamped.hpp" #include "nav_msgs/msg/odometry.hpp" #include "tf2_msgs/msg/tf_message.hpp" +#include "visualization_msgs/msg/marker_array.hpp" #include #include @@ -110,6 +112,8 @@ class TRAJECTORY_FOLLOWER_PUBLIC Controller : public rclcpp::Node void publishDebugMarker( const trajectory_follower::InputData & input_data, const trajectory_follower::LateralOutput & lat_out) const; + + std::unique_ptr logger_configure_; }; } // namespace trajectory_follower_node } // namespace autoware::motion::control diff --git a/control/trajectory_follower_node/package.xml b/control/trajectory_follower_node/package.xml index 8673634e24058..34e9fb45424a7 100644 --- a/control/trajectory_follower_node/package.xml +++ b/control/trajectory_follower_node/package.xml @@ -32,6 +32,7 @@ rclcpp_components trajectory_follower_base vehicle_info_util + visualization_msgs ros2launch diff --git a/control/trajectory_follower_node/param/lateral/mpc.param.yaml b/control/trajectory_follower_node/param/lateral/mpc.param.yaml index dce35f2c9f6d4..4222082d40deb 100644 --- a/control/trajectory_follower_node/param/lateral/mpc.param.yaml +++ b/control/trajectory_follower_node/param/lateral/mpc.param.yaml @@ -65,6 +65,7 @@ keep_steer_control_until_converged: true new_traj_duration_time: 1.0 new_traj_end_dist: 0.3 + mpc_converged_threshold_rps: 0.01 # threshold of mpc convergence check [rad/s] # steer offset steering_offset: diff --git a/control/trajectory_follower_node/src/controller_node.cpp b/control/trajectory_follower_node/src/controller_node.cpp index 5ee9f7f4f1e71..e35848e9495af 100644 --- a/control/trajectory_follower_node/src/controller_node.cpp +++ b/control/trajectory_follower_node/src/controller_node.cpp @@ -17,6 +17,7 @@ #include "mpc_lateral_controller/mpc_lateral_controller.hpp" #include "pid_longitudinal_controller/pid_longitudinal_controller.hpp" #include "pure_pursuit/pure_pursuit_lateral_controller.hpp" +#include "tier4_autoware_utils/ros/marker_helper.hpp" #include #include @@ -84,6 +85,8 @@ Controller::Controller(const rclcpp::NodeOptions & node_options) : Node("control timer_control_ = rclcpp::create_timer( this, get_clock(), period_ns, std::bind(&Controller::callbackTimerControl, this)); } + + logger_configure_ = std::make_unique(this); } Controller::LateralControllerMode Controller::getLateralControllerMode( diff --git a/control/trajectory_follower_node/src/simple_trajectory_follower.cpp b/control/trajectory_follower_node/src/simple_trajectory_follower.cpp index f073574cc229f..5e9488bf2ca93 100644 --- a/control/trajectory_follower_node/src/simple_trajectory_follower.cpp +++ b/control/trajectory_follower_node/src/simple_trajectory_follower.cpp @@ -14,8 +14,8 @@ #include "trajectory_follower_node/simple_trajectory_follower.hpp" -#include -#include +#include +#include #include diff --git a/control/vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml b/control/vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml index 92844c61f6f4f..191e894622af7 100644 --- a/control/vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml +++ b/control/vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml @@ -15,6 +15,8 @@ nominal: vel_lim: 25.0 reference_speed_points: [20.0, 30.0] + steer_lim: [1.0, 0.8] + steer_rate_lim: [1.0, 0.8] lon_acc_lim: [5.0, 4.0] lon_jerk_lim: [5.0, 4.0] lat_acc_lim: [5.0, 4.0] @@ -23,6 +25,8 @@ on_transition: vel_lim: 50.0 reference_speed_points: [20.0, 30.0] + steer_lim: [1.0, 0.8] + steer_rate_lim: [1.0, 0.8] lon_acc_lim: [1.0, 0.9] lon_jerk_lim: [0.5, 0.4] lat_acc_lim: [2.0, 1.8] diff --git a/control/vehicle_cmd_gate/package.xml b/control/vehicle_cmd_gate/package.xml index 36bb662fb2f53..deb988d61738f 100644 --- a/control/vehicle_cmd_gate/package.xml +++ b/control/vehicle_cmd_gate/package.xml @@ -34,6 +34,7 @@ tier4_system_msgs tier4_vehicle_msgs vehicle_info_util + visualization_msgs rosidl_default_runtime diff --git a/control/vehicle_cmd_gate/src/marker_helper.hpp b/control/vehicle_cmd_gate/src/marker_helper.hpp new file mode 100644 index 0000000000000..44c43a3630151 --- /dev/null +++ b/control/vehicle_cmd_gate/src/marker_helper.hpp @@ -0,0 +1,119 @@ +// 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. + +#ifndef MARKER_HELPER_HPP_ +#define MARKER_HELPER_HPP_ + +#include + +#include + +#include + +namespace vehicle_cmd_gate +{ +inline geometry_msgs::msg::Point createMarkerPosition(double x, double y, double z) +{ + geometry_msgs::msg::Point point; + + point.x = x; + point.y = y; + point.z = z; + + return point; +} + +inline geometry_msgs::msg::Quaternion createMarkerOrientation( + double x, double y, double z, double w) +{ + geometry_msgs::msg::Quaternion quaternion; + + quaternion.x = x; + quaternion.y = y; + quaternion.z = z; + quaternion.w = w; + + return quaternion; +} + +inline geometry_msgs::msg::Vector3 createMarkerScale(double x, double y, double z) +{ + geometry_msgs::msg::Vector3 scale; + + scale.x = x; + scale.y = y; + scale.z = z; + + return scale; +} + +inline std_msgs::msg::ColorRGBA createMarkerColor(float r, float g, float b, float a) +{ + std_msgs::msg::ColorRGBA color; + + color.r = r; + color.g = g; + color.b = b; + color.a = a; + + return color; +} + +inline visualization_msgs::msg::Marker createMarker( + const std::string & frame_id, const std::string & ns, const int32_t id, const int32_t type, + geometry_msgs::msg::Point point, geometry_msgs::msg::Vector3 scale, + const std_msgs::msg::ColorRGBA & color) +{ + visualization_msgs::msg::Marker marker; + + marker.header.frame_id = frame_id; + marker.header.stamp = rclcpp::Time(0); + marker.ns = ns; + marker.id = id; + marker.type = type; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.lifetime = rclcpp::Duration::from_seconds(0.2); + marker.pose.position = point; + marker.pose.orientation = createMarkerOrientation(0.0, 0.0, 0.0, 1.0); + marker.scale = scale; + marker.color = color; + marker.frame_locked = false; + + return marker; +} + +inline visualization_msgs::msg::Marker createStringMarker( + const std::string & frame_id, const std::string & ns, const int32_t id, const int32_t type, + geometry_msgs::msg::Point point, geometry_msgs::msg::Vector3 scale, + const std_msgs::msg::ColorRGBA & color, const std::string text) +{ + visualization_msgs::msg::Marker marker; + + marker = createMarker(frame_id, ns, id, type, point, scale, color); + marker.text = text; + + return marker; +} + +inline void appendMarkerArray( + const visualization_msgs::msg::MarkerArray & additional_marker_array, + visualization_msgs::msg::MarkerArray * marker_array) +{ + for (const auto & marker : additional_marker_array.markers) { + marker_array->markers.push_back(marker); + } +} +} // namespace vehicle_cmd_gate + +#endif // MARKER_HELPER_HPP_ diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_filter.cpp b/control/vehicle_cmd_gate/src/vehicle_cmd_filter.cpp index 8dec06c455868..4431534a76a5e 100644 --- a/control/vehicle_cmd_gate/src/vehicle_cmd_filter.cpp +++ b/control/vehicle_cmd_gate/src/vehicle_cmd_filter.cpp @@ -29,7 +29,8 @@ bool VehicleCmdFilter::setParameterWithValidation(const VehicleCmdFilterParam & const auto s = p.reference_speed_points.size(); if ( p.lon_acc_lim.size() != s || p.lon_jerk_lim.size() != s || p.lat_acc_lim.size() != s || - p.lat_jerk_lim.size() != s || p.actual_steer_diff_lim.size() != s) { + p.lat_jerk_lim.size() != s || p.actual_steer_diff_lim.size() != s || p.steer_lim.size() != s || + p.steer_rate_lim.size() != s) { std::cerr << "VehicleCmdFilter::setParam() There is a size mismatch in the parameter. " "Parameter initialization failed." << std::endl; @@ -39,7 +40,18 @@ bool VehicleCmdFilter::setParameterWithValidation(const VehicleCmdFilterParam & param_ = p; return true; } - +void VehicleCmdFilter::setSteerLim(LimitArray v) +{ + auto tmp = param_; + tmp.steer_lim = v; + setParameterWithValidation(tmp); +} +void VehicleCmdFilter::setSteerRateLim(LimitArray v) +{ + auto tmp = param_; + tmp.steer_rate_lim = v; + setParameterWithValidation(tmp); +} void VehicleCmdFilter::setLonAccLim(LimitArray v) { auto tmp = param_; @@ -104,25 +116,28 @@ void VehicleCmdFilter::VehicleCmdFilter::limitLongitudinalWithJerk( std::clamp(static_cast(input.longitudinal.jerk), -lon_jerk_lim, lon_jerk_lim); } +// Use ego vehicle speed (not speed command) for the lateral acceleration calculation, otherwise the +// filtered steering angle oscillates if the input velocity oscillates. void VehicleCmdFilter::limitLateralWithLatAcc( [[maybe_unused]] const double dt, AckermannControlCommand & input) const { const auto lat_acc_lim = getLatAccLim(); - double latacc = calcLatAcc(input); + double latacc = calcLatAcc(input, current_speed_); if (std::fabs(latacc) > lat_acc_lim) { - double v_sq = - std::max(static_cast(input.longitudinal.speed * input.longitudinal.speed), 0.001); + double v_sq = std::max(static_cast(current_speed_ * current_speed_), 0.001); double steer_lim = std::atan(lat_acc_lim * param_.wheel_base / v_sq); input.lateral.steering_tire_angle = latacc > 0.0 ? steer_lim : -steer_lim; } } +// Use ego vehicle speed (not speed command) for the lateral acceleration calculation, otherwise the +// filtered steering angle oscillates if the input velocity oscillates. void VehicleCmdFilter::limitLateralWithLatJerk( const double dt, AckermannControlCommand & input) const { - double curr_latacc = calcLatAcc(input); - double prev_latacc = calcLatAcc(prev_cmd_); + double curr_latacc = calcLatAcc(input, current_speed_); + double prev_latacc = calcLatAcc(prev_cmd_, current_speed_); const auto lat_jerk_lim = getLatJerkLim(); @@ -130,9 +145,9 @@ void VehicleCmdFilter::limitLateralWithLatJerk( const double latacc_min = prev_latacc - lat_jerk_lim * dt; if (curr_latacc > latacc_max) { - input.lateral.steering_tire_angle = calcSteerFromLatacc(input.longitudinal.speed, latacc_max); + input.lateral.steering_tire_angle = calcSteerFromLatacc(current_speed_, latacc_max); } else if (curr_latacc < latacc_min) { - input.lateral.steering_tire_angle = calcSteerFromLatacc(input.longitudinal.speed, latacc_min); + input.lateral.steering_tire_angle = calcSteerFromLatacc(current_speed_, latacc_min); } } @@ -148,12 +163,36 @@ void VehicleCmdFilter::limitActualSteerDiff( void VehicleCmdFilter::limitLateralSteer(AckermannControlCommand & input) const { - // TODO(Horibe): parametrize the max steering angle. - // TODO(Horibe): support steering greater than PI/2. Now the lateral acceleration - // calculation does not support bigger steering value than PI/2 due to tan/atan calculation. - constexpr float steer_limit = M_PI_2; + const float steer_limit = getSteerLim(); + input.lateral.steering_tire_angle = std::clamp(input.lateral.steering_tire_angle, -steer_limit, steer_limit); + + // TODO(Horibe): support steering greater than PI/2. Now the lateral acceleration + // calculation does not support bigger steering value than PI/2 due to tan/atan calculation. + if (std::abs(input.lateral.steering_tire_angle) > M_PI_2f) { + std::cerr << "[vehicle_Cmd_gate] limitLateralSteer(): steering limit is set to pi/2 since the " + "current filtering logic can not handle the steering larger than pi/2. Please " + "check the steering angle limit." + << std::endl; + + std::clamp(input.lateral.steering_tire_angle, -M_PI_2f, M_PI_2f); + } +} + +void VehicleCmdFilter::limitLateralSteerRate(const double dt, AckermannControlCommand & input) const +{ + const float steer_rate_limit = getSteerRateLim(); + + // for steering angle rate + input.lateral.steering_tire_rotation_rate = + std::clamp(input.lateral.steering_tire_rotation_rate, -steer_rate_limit, steer_rate_limit); + + // for steering angle + const float steer_diff_limit = steer_rate_limit * dt; + float ds = input.lateral.steering_tire_angle - prev_cmd_.lateral.steering_tire_angle; + ds = std::clamp(ds, -steer_diff_limit, steer_diff_limit); + input.lateral.steering_tire_angle = prev_cmd_.lateral.steering_tire_angle + ds; } void VehicleCmdFilter::filterAll( @@ -162,6 +201,7 @@ void VehicleCmdFilter::filterAll( { const auto cmd_orig = cmd; limitLateralSteer(cmd); + limitLateralSteerRate(dt, cmd); limitLongitudinalWithJerk(dt, cmd); limitLongitudinalWithAcc(dt, cmd); limitLongitudinalWithVel(cmd); @@ -205,6 +245,11 @@ double VehicleCmdFilter::calcLatAcc(const AckermannControlCommand & cmd) const return v * v * std::tan(cmd.lateral.steering_tire_angle) / param_.wheel_base; } +double VehicleCmdFilter::calcLatAcc(const AckermannControlCommand & cmd, const double v) const +{ + return v * v * std::tan(cmd.lateral.steering_tire_angle) / param_.wheel_base; +} + double VehicleCmdFilter::limitDiff( const double curr, const double prev, const double diff_lim) const { @@ -259,6 +304,14 @@ double VehicleCmdFilter::getLatJerkLim() const { return interpolateFromSpeed(param_.lat_jerk_lim); } +double VehicleCmdFilter::getSteerLim() const +{ + return interpolateFromSpeed(param_.steer_lim); +} +double VehicleCmdFilter::getSteerRateLim() const +{ + return interpolateFromSpeed(param_.steer_rate_lim); +} double VehicleCmdFilter::getSteerDiffLim() const { return interpolateFromSpeed(param_.actual_steer_diff_lim); diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_filter.hpp b/control/vehicle_cmd_gate/src/vehicle_cmd_filter.hpp index eb85fcbeb8606..cf7a1f627a68a 100644 --- a/control/vehicle_cmd_gate/src/vehicle_cmd_filter.hpp +++ b/control/vehicle_cmd_gate/src/vehicle_cmd_filter.hpp @@ -37,6 +37,8 @@ struct VehicleCmdFilterParam LimitArray lon_jerk_lim; LimitArray lat_acc_lim; LimitArray lat_jerk_lim; + LimitArray steer_lim; + LimitArray steer_rate_lim; LimitArray actual_steer_diff_lim; }; class VehicleCmdFilter @@ -47,6 +49,8 @@ class VehicleCmdFilter void setWheelBase(double v) { param_.wheel_base = v; } void setVelLim(double v) { param_.vel_lim = v; } + void setSteerLim(LimitArray v); + void setSteerRateLim(LimitArray v); void setLonAccLim(LimitArray v); void setLonJerkLim(LimitArray v); void setLatAccLim(LimitArray v); @@ -64,6 +68,7 @@ class VehicleCmdFilter void limitActualSteerDiff( const double current_steer_angle, AckermannControlCommand & input) const; void limitLateralSteer(AckermannControlCommand & input) const; + void limitLateralSteerRate(const double dt, AckermannControlCommand & input) const; void filterAll( const double dt, const double current_steer_angle, AckermannControlCommand & input, IsFilterActivated & is_activated) const; @@ -81,6 +86,7 @@ class VehicleCmdFilter bool setParameterWithValidation(const VehicleCmdFilterParam & p); double calcLatAcc(const AckermannControlCommand & cmd) const; + double calcLatAcc(const AckermannControlCommand & cmd, const double v) const; double calcSteerFromLatacc(const double v, const double latacc) const; double limitDiff(const double curr, const double prev, const double diff_lim) const; @@ -89,6 +95,8 @@ class VehicleCmdFilter double getLonJerkLim() const; double getLatAccLim() const; double getLatJerkLim() const; + double getSteerLim() const; + double getSteerRateLim() const; double getSteerDiffLim() const; }; } // namespace vehicle_cmd_gate diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp index 9030521b6b334..e681bc22cd24b 100644 --- a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp +++ b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp @@ -14,6 +14,8 @@ #include "vehicle_cmd_gate.hpp" +#include "marker_helper.hpp" + #include #include @@ -73,6 +75,8 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options) is_filter_activated_pub_ = create_publisher("~/is_filter_activated", durable_qos); + filter_activated_marker_pub_ = + create_publisher("~/is_filter_activated/marker", durable_qos); // Subscriber external_emergency_stop_heartbeat_sub_ = create_subscription( @@ -165,6 +169,8 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options) p.vel_lim = declare_parameter("nominal.vel_lim"); p.reference_speed_points = declare_parameter>("nominal.reference_speed_points"); + p.steer_lim = declare_parameter>("nominal.steer_lim"); + p.steer_rate_lim = declare_parameter>("nominal.steer_rate_lim"); p.lon_acc_lim = declare_parameter>("nominal.lon_acc_lim"); p.lon_jerk_lim = declare_parameter>("nominal.lon_jerk_lim"); p.lat_acc_lim = declare_parameter>("nominal.lat_acc_lim"); @@ -180,6 +186,8 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options) p.vel_lim = declare_parameter("on_transition.vel_lim"); p.reference_speed_points = declare_parameter>("on_transition.reference_speed_points"); + p.steer_lim = declare_parameter>("on_transition.steer_lim"); + p.steer_rate_lim = declare_parameter>("on_transition.steer_rate_lim"); p.lon_acc_lim = declare_parameter>("on_transition.lon_acc_lim"); p.lon_jerk_lim = declare_parameter>("on_transition.lon_jerk_lim"); p.lat_acc_lim = declare_parameter>("on_transition.lat_acc_lim"); @@ -225,6 +233,8 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options) rclcpp::create_timer(this, get_clock(), period_ns, std::bind(&VehicleCmdGate::onTimer, this)); timer_pub_status_ = rclcpp::create_timer( this, get_clock(), period_ns, std::bind(&VehicleCmdGate::publishStatus, this)); + + logger_configure_ = std::make_unique(this); } bool VehicleCmdGate::isHeartbeatTimeout( @@ -419,8 +429,7 @@ void VehicleCmdGate::publishControlCommands(const Commands & commands) // Check pause. Place this check after all other checks as it needs the final output. adapi_pause_->update(filtered_commands.control); if (adapi_pause_->is_paused()) { - filtered_commands.control.longitudinal.speed = 0.0; - filtered_commands.control.longitudinal.acceleration = stop_hold_acceleration_; + filtered_commands.control = createStopControlCmd(); } // Check if command filtering option is enable @@ -546,9 +555,7 @@ AckermannControlCommand VehicleCmdGate::filterControlCommand(const AckermannCont // set prev value for both to keep consistency over switching: // Actual steer, vel, acc should be considered in manual mode to prevent sudden motion when // switching from manual to autonomous - const auto in_autonomous = - (mode.mode == OperationModeState::AUTONOMOUS && mode.is_autoware_control_enabled); - auto prev_values = in_autonomous ? out : current_status_cmd; + auto prev_values = mode.is_autoware_control_enabled ? out : current_status_cmd; if (ego_is_stopped) { prev_values.longitudinal = out.longitudinal; @@ -565,6 +572,7 @@ AckermannControlCommand VehicleCmdGate::filterControlCommand(const AckermannCont is_filter_activated.stamp = now(); is_filter_activated_pub_->publish(is_filter_activated); + filter_activated_marker_pub_->publish(createMarkerArray(is_filter_activated)); return out; } @@ -732,6 +740,53 @@ void VehicleCmdGate::checkExternalEmergencyStop(diagnostic_updater::DiagnosticSt stat.summary(status.level, status.message); } +MarkerArray VehicleCmdGate::createMarkerArray(const IsFilterActivated & filter_activated) +{ + MarkerArray msg; + + if (!filter_activated.is_activated) { + return msg; + } + + /* add string marker */ + bool first_msg = true; + std::string reason = "filter activated on"; + + if (filter_activated.is_activated_on_acceleration) { + reason += " lon_acc"; + first_msg = false; + } + if (filter_activated.is_activated_on_jerk) { + reason += first_msg ? " jerk" : ", jerk"; + first_msg = false; + } + if (filter_activated.is_activated_on_speed) { + reason += first_msg ? " speed" : ", speed"; + first_msg = false; + } + if (filter_activated.is_activated_on_steering) { + reason += first_msg ? " steer" : ", steer"; + first_msg = false; + } + if (filter_activated.is_activated_on_steering_rate) { + reason += first_msg ? " steer_rate" : ", steer_rate"; + first_msg = false; + } + + msg.markers.emplace_back(createStringMarker( + "base_link", "msg", 0, visualization_msgs::msg::Marker::TEXT_VIEW_FACING, + createMarkerPosition(0.0, 0.0, 1.0), createMarkerScale(0.0, 0.0, 1.0), + createMarkerColor(1.0, 0.0, 0.0, 1.0), reason)); + + /* add sphere marker */ + msg.markers.emplace_back(createMarker( + "base_link", "sphere", 0, visualization_msgs::msg::Marker::SPHERE, + createMarkerPosition(0.0, 0.0, 0.0), createMarkerScale(3.0, 3.0, 3.0), + createMarkerColor(1.0, 0.0, 0.0, 0.3))); + + return msg; +} + } // namespace vehicle_cmd_gate #include diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp index d516ecac29ca2..8f0a6aa14d08b 100644 --- a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp +++ b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp @@ -17,10 +17,11 @@ #include "adapi_pause_interface.hpp" #include "moderate_stop_interface.hpp" +#include "tier4_autoware_utils/ros/logger_level_configure.hpp" #include "vehicle_cmd_filter.hpp" #include -#include +#include #include #include #include @@ -44,6 +45,7 @@ #include #include #include +#include #include @@ -66,6 +68,7 @@ using tier4_external_api_msgs::srv::SetEmergency; using tier4_system_msgs::msg::MrmBehaviorStatus; using tier4_vehicle_msgs::msg::VehicleEmergencyStamped; using vehicle_cmd_gate::msg::IsFilterActivated; +using visualization_msgs::msg::MarkerArray; using diagnostic_msgs::msg::DiagnosticStatus; using nav_msgs::msg::Odometry; @@ -102,6 +105,7 @@ class VehicleCmdGate : public rclcpp::Node rclcpp::Publisher::SharedPtr engage_pub_; rclcpp::Publisher::SharedPtr operation_mode_pub_; rclcpp::Publisher::SharedPtr is_filter_activated_pub_; + rclcpp::Publisher::SharedPtr filter_activated_marker_pub_; // Subscription rclcpp::Subscription::SharedPtr external_emergency_stop_heartbeat_sub_; @@ -229,6 +233,11 @@ class VehicleCmdGate : public rclcpp::Node // stop checker std::unique_ptr vehicle_stop_checker_; double stop_check_duration_; + + // debug + MarkerArray createMarkerArray(const IsFilterActivated & filter_activated); + + std::unique_ptr logger_configure_; }; } // namespace vehicle_cmd_gate diff --git a/control/vehicle_cmd_gate/test/src/test_filter_in_vehicle_cmd_gate_node.cpp b/control/vehicle_cmd_gate/test/src/test_filter_in_vehicle_cmd_gate_node.cpp index 6384fbfb22f60..f02235ed1ecbf 100644 --- a/control/vehicle_cmd_gate/test/src/test_filter_in_vehicle_cmd_gate_node.cpp +++ b/control/vehicle_cmd_gate/test/src/test_filter_in_vehicle_cmd_gate_node.cpp @@ -45,6 +45,8 @@ void print_values(int i, const T1 & name, const T2 & a, const T3 &... b) // global params const std::vector reference_speed_points = {5., 10., 15., 20.}; +const std::vector steer_lim = {0.5, 0.3, 0.2, 0.1}; +const std::vector steer_rate_lim = {0.5, 0.3, 0.2, 0.1}; const std::vector lon_acc_lim = {1.5, 1.0, 0.8, 0.6}; const std::vector lon_jerk_lim = {1.4, 0.9, 0.7, 0.5}; const std::vector lat_acc_lim = {2.0, 1.6, 1.2, 0.8}; @@ -150,7 +152,6 @@ class PubSubNode : public rclcpp::Node if (!cmd_history_.empty()) { // ego moves as commanded. msg.twist.twist.linear.x = cmd_history_.back()->longitudinal.speed; // ego moves as commanded. - } else { } pub_odom_->publish(msg); } @@ -238,16 +239,19 @@ class PubSubNode : public rclcpp::Node const auto lon_jerk = (lon_acc - cmd_prev->longitudinal.acceleration) / dt; const auto lat_acc = lon_vel * lon_vel * std::tan(cmd_curr->lateral.steering_tire_angle) / wheelbase; - const auto prev_lon_vel = cmd_prev->longitudinal.speed; + + // TODO(Horibe): prev_lat_acc should use the previous velocity, but use the current velocity + // since the current filtering logic uses the current velocity. const auto prev_lat_acc = - prev_lon_vel * prev_lon_vel * std::tan(cmd_prev->lateral.steering_tire_angle) / wheelbase; + lon_vel * lon_vel * std::tan(cmd_prev->lateral.steering_tire_angle) / wheelbase; const auto lat_jerk = (lat_acc - prev_lat_acc) / dt; /* debug print */ // const auto steer = cmd_curr->lateral.steering_tire_angle; // PRINT_VALUES( - // dt, lon_vel, lon_acc, lon_jerk, lat_acc, lat_jerk, steer, max_lon_acc_lim, - // max_lon_jerk_lim, max_lat_acc_lim, max_lat_jerk_lim); + // dt, i_curr, i_prev, steer, lon_vel, prev_lon_vel, lon_acc, lon_jerk, lat_acc, prev_lat_acc, + // prev_lat_acc2, lat_jerk, max_lon_acc_lim, max_lon_jerk_lim, max_lat_acc_lim, + // max_lat_jerk_lim); // Output command must be smaller than maximum limit. // TODO(Horibe): check for each velocity range. @@ -334,6 +338,8 @@ std::shared_ptr generateNode() node_options.append_parameter_override("wheel_base", wheelbase); override("nominal.reference_speed_points", reference_speed_points); override("nominal.reference_speed_points", reference_speed_points); + override("nominal.steer_lim", steer_lim); + override("nominal.steer_rate_lim", steer_rate_lim); override("nominal.lon_acc_lim", lon_acc_lim); override("nominal.lon_jerk_lim", lon_jerk_lim); override("nominal.lat_acc_lim", lat_acc_lim); @@ -368,6 +374,16 @@ TEST_P(TestFixture, CheckFilterForSinCmd) [[maybe_unused]] auto b = std::system("ros2 node info /test_vehicle_cmd_gate_filter_pubsub"); [[maybe_unused]] auto c = std::system("ros2 node info /vehicle_cmd_gate"); + // std::cerr << "speed signal: " << cmd_generator_.p_.velocity.max << " * sin(2pi * " + // << cmd_generator_.p_.velocity.freq << " * dt + " << cmd_generator_.p_.velocity.bias + // << ")" << std::endl; + // std::cerr << "accel signal: " << cmd_generator_.p_.acceleration.max << " * sin(2pi * " + // << cmd_generator_.p_.acceleration.freq << " * dt + " + // << cmd_generator_.p_.acceleration.bias << ")" << std::endl; + // std::cerr << "steer signal: " << cmd_generator_.p_.steering.max << " * sin(2pi * " + // << cmd_generator_.p_.steering.freq << " * dt + " << cmd_generator_.p_.steering.bias + // << ")" << std::endl; + for (size_t i = 0; i < 100; ++i) { const bool reset_clock = (i == 0); const auto cmd = cmd_generator_.calcSinWaveCommand(reset_clock); @@ -384,17 +400,23 @@ TEST_P(TestFixture, CheckFilterForSinCmd) }; // High frequency, large value -CmdParams p1 = {/*steer*/ {10, 1, 0}, /*velocity*/ {10, 1.2, 0}, /*acc*/ {5, 1.5, 2}}; +CmdParams p1 = {/*steer*/ {0.5, 1, 0}, /*velocity*/ {10, 0.0, 0}, /*acc*/ {5, 1.5, 2}}; INSTANTIATE_TEST_SUITE_P(TestParam1, TestFixture, ::testing::Values(p1)); // High frequency, normal value -CmdParams p2 = {/*steer*/ {1.5, 2, 1}, /*velocity*/ {5, 1.0, 0}, /*acc*/ {2.0, 3.0, 2}}; +CmdParams p2 = {/*steer*/ {0.5, 2, 1}, /*velocity*/ {5, 1.0, 0}, /*acc*/ {2.0, 3.0, 2}}; INSTANTIATE_TEST_SUITE_P(TestParam2, TestFixture, ::testing::Values(p2)); // High frequency, small value -CmdParams p3 = {/*steer*/ {1.5, 3, 2}, /*velocity*/ {2, 3, 0}, /*acc*/ {0.5, 3, 2}}; +CmdParams p3 = {/*steer*/ {0.5, 3, 2}, /*velocity*/ {2, 3, 0}, /*acc*/ {0.5, 3, 2}}; INSTANTIATE_TEST_SUITE_P(TestParam3, TestFixture, ::testing::Values(p3)); // Low frequency -CmdParams p4 = {/*steer*/ {10, 0.1, 0.5}, /*velocity*/ {10, 0.2, 0}, /*acc*/ {5, 0.1, 2}}; +CmdParams p4 = {/*steer*/ {0.5, 0.1, 0.5}, /*velocity*/ {10, 0.2, 0}, /*acc*/ {5, 0.1, 2}}; INSTANTIATE_TEST_SUITE_P(TestParam4, TestFixture, ::testing::Values(p4)); + +// Large steer, large velocity -> this test fails. +// Lateral acceleration and lateral jerk affect both steer and velocity, and if both steer and +// velocity changes significantly, the current logic cannot adequately handle the situation. +// CmdParams p5 = {/*steer*/ {10.0, 1.0, 0.5}, /*velocity*/ {10, 0.2, 0}, /*acc*/ {5, 0.1, 2}}; +// INSTANTIATE_TEST_SUITE_P(TestParam5, TestFixture, ::testing::Values(p5)); 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 7ce8580120652..b8200490dd1d5 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 @@ -31,12 +31,15 @@ constexpr double NOMINAL_INTERVAL = 1.0; void setFilterParams( vehicle_cmd_gate::VehicleCmdFilter & f, double v, LimitArray speed_points, LimitArray a, - LimitArray j, LimitArray lat_a, LimitArray lat_j, LimitArray steer_diff, const double wheelbase) + LimitArray j, LimitArray lat_a, LimitArray lat_j, LimitArray steer_diff, LimitArray steer_lim, + LimitArray steer_rate_lim, const double wheelbase) { vehicle_cmd_gate::VehicleCmdFilterParam p; p.vel_lim = v; p.wheel_base = wheelbase; p.reference_speed_points = speed_points; + p.steer_lim = steer_lim; + p.steer_rate_lim = steer_rate_lim; p.lat_acc_lim = lat_a; p.lat_jerk_lim = lat_j; p.lon_acc_lim = a; @@ -56,12 +59,20 @@ AckermannControlCommand genCmd(double s, double sr, double v, double a) return cmd; } +// calc from ego velocity +double calcLatAcc(const AckermannControlCommand & cmd, const double wheelbase, const double ego_v) +{ + return ego_v * ego_v * std::tan(cmd.lateral.steering_tire_angle) / wheelbase; +} + +// calc from command velocity double calcLatAcc(const AckermannControlCommand & cmd, const double wheelbase) { double v = cmd.longitudinal.speed; return v * v * std::tan(cmd.lateral.steering_tire_angle) / wheelbase; } +// calc from command velocity double calcLatJerk( const AckermannControlCommand & cmd, const AckermannControlCommand & prev_cmd, const double wheelbase, const double dt) @@ -75,16 +86,31 @@ double calcLatJerk( return (curr - prev) / dt; } +// calc from ego velocity +double calcLatJerk( + const AckermannControlCommand & cmd, const AckermannControlCommand & prev_cmd, + const double wheelbase, const double dt, const double ego_v) +{ + const auto prev = ego_v * ego_v * std::tan(prev_cmd.lateral.steering_tire_angle) / wheelbase; + + const auto curr = ego_v * ego_v * std::tan(cmd.lateral.steering_tire_angle) / wheelbase; + + return (curr - prev) / dt; +} + void test_1d_limit( - double V_LIM, double A_LIM, double J_LIM, double LAT_A_LIM, double LAT_J_LIM, double STEER_DIFF, + double ego_v, double V_LIM, double A_LIM, double J_LIM, double LAT_A_LIM, double LAT_J_LIM, + double STEER_DIFF, double STEER_LIM, double STEER_RATE_LIM, const AckermannControlCommand & prev_cmd, const AckermannControlCommand & raw_cmd) { const double WHEELBASE = 3.0; const double DT = 0.1; // [s] vehicle_cmd_gate::VehicleCmdFilter filter; + filter.setCurrentSpeed(ego_v); setFilterParams( - filter, V_LIM, {0.0}, {A_LIM}, {J_LIM}, {LAT_A_LIM}, {LAT_J_LIM}, {STEER_DIFF}, WHEELBASE); + filter, V_LIM, {0.0}, {A_LIM}, {J_LIM}, {LAT_A_LIM}, {LAT_J_LIM}, {STEER_DIFF}, {STEER_LIM}, + {STEER_RATE_LIM}, WHEELBASE); filter.setPrevCmd(prev_cmd); // velocity filter @@ -153,8 +179,8 @@ void test_1d_limit( { auto filtered_cmd = raw_cmd; filter.limitLateralWithLatAcc(DT, filtered_cmd); - const double filtered_latacc = calcLatAcc(filtered_cmd, WHEELBASE); - const double raw_latacc = calcLatAcc(raw_cmd, WHEELBASE); + const double filtered_latacc = calcLatAcc(filtered_cmd, WHEELBASE, ego_v); + const double raw_latacc = calcLatAcc(raw_cmd, WHEELBASE, ego_v); // check if the filtered value does not exceed the limit. ASSERT_LT_NEAR(std::abs(filtered_latacc), LAT_A_LIM); @@ -169,9 +195,9 @@ void test_1d_limit( { auto filtered_cmd = raw_cmd; filter.limitLateralWithLatJerk(DT, filtered_cmd); - const double prev_lat_acc = calcLatAcc(prev_cmd, WHEELBASE); - const double filtered_lat_acc = calcLatAcc(filtered_cmd, WHEELBASE); - const double raw_lat_acc = calcLatAcc(raw_cmd, WHEELBASE); + const double prev_lat_acc = calcLatAcc(prev_cmd, WHEELBASE, ego_v); + const double filtered_lat_acc = calcLatAcc(filtered_cmd, WHEELBASE, ego_v); + const double raw_lat_acc = calcLatAcc(raw_cmd, WHEELBASE, ego_v); const double filtered_lateral_jerk = (filtered_lat_acc - prev_lat_acc) / DT; // check if the filtered value does not exceed the limit. @@ -211,6 +237,9 @@ TEST(VehicleCmdFilter, VehicleCmdFilter) const std::vector lat_a_arr = {0.01, 1.0, 100.0}; const std::vector lat_j_arr = {0.01, 1.0, 100.0}; const std::vector steer_diff_arr = {0.01, 1.0, 100.0}; + const std::vector steer_lim_arr = {0.01, 1.0, 100.0}; + const std::vector steer_rate_lim_arr = {0.01, 1.0, 100.0}; + const std::vector ego_v_arr = {0.0, 0.1, 1.0, 3.0, 15.0}; const std::vector prev_cmd_arr = { genCmd(0.0, 0.0, 0.0, 0.0), genCmd(1.0, 1.0, 1.0, 1.0)}; @@ -226,7 +255,14 @@ TEST(VehicleCmdFilter, VehicleCmdFilter) for (const auto & prev_cmd : prev_cmd_arr) { for (const auto & raw_cmd : raw_cmd_arr) { for (const auto & steer_diff : steer_diff_arr) { - test_1d_limit(v, a, j, la, lj, steer_diff, prev_cmd, raw_cmd); + for (const auto & steer : steer_lim_arr) { + for (const auto & steer_rate : steer_rate_lim_arr) { + for (const auto & ego_v : ego_v_arr) { + test_1d_limit( + ego_v, v, a, j, la, lj, steer_diff, steer, steer_rate, prev_cmd, raw_cmd); + } + } + } } } } @@ -246,6 +282,8 @@ TEST(VehicleCmdFilter, VehicleCmdFilterInterpolate) p.wheel_base = WHEELBASE; p.vel_lim = 20.0; p.reference_speed_points = std::vector{2.0, 4.0, 10.0}; + p.steer_lim = std::vector{0.1, 0.2, 0.3}; + p.steer_rate_lim = std::vector{0.2, 0.1, 0.05}; p.lon_acc_lim = std::vector{0.3, 0.4, 0.5}; p.lon_jerk_lim = std::vector{0.4, 0.4, 0.7}; p.lat_acc_lim = std::vector{0.1, 0.2, 0.3}; @@ -268,7 +306,16 @@ TEST(VehicleCmdFilter, VehicleCmdFilterInterpolate) const auto set_speed_and_reset_prev = [&](const auto & current_vel) { filter.setCurrentSpeed(current_vel); }; - + const auto _limitSteer = [&](const auto & in) { + auto out = in; + filter.limitLateralSteer(out); + return out; + }; + const auto _limitSteerRate = [&](const auto & in) { + auto out = in; + filter.limitLateralSteerRate(DT, out); + return out; + }; const auto _limitLongitudinalWithVel = [&](const auto & in) { auto out = in; filter.limitLongitudinalWithVel(out); @@ -309,6 +356,77 @@ TEST(VehicleCmdFilter, VehicleCmdFilterInterpolate) EXPECT_NEAR(_limitLongitudinalWithVel(orig_cmd).longitudinal.speed, 20.0, ep); } + // steer angle lim + // p.reference_speed_points = std::vector{2.0, 4.0, 10.0}; + // p.steer_lim = std::vector{0.1, 0.2, 0.3}; + { + set_speed_and_reset_prev(0.0); + EXPECT_NEAR(_limitSteer(orig_cmd).lateral.steering_tire_angle, 0.1, ep); + + set_speed_and_reset_prev(2.0); + EXPECT_NEAR(_limitSteer(orig_cmd).lateral.steering_tire_angle, 0.1, ep); + + set_speed_and_reset_prev(3.0); + EXPECT_NEAR(_limitSteer(orig_cmd).lateral.steering_tire_angle, 0.15, ep); + + set_speed_and_reset_prev(5.0); + EXPECT_NEAR(_limitSteer(orig_cmd).lateral.steering_tire_angle, 0.2 + 0.1 / 6.0, ep); + + set_speed_and_reset_prev(8.0); + EXPECT_NEAR(_limitSteer(orig_cmd).lateral.steering_tire_angle, 0.2 + 0.1 * 4.0 / 6.0, ep); + + set_speed_and_reset_prev(10.0); + EXPECT_NEAR(_limitSteer(orig_cmd).lateral.steering_tire_angle, 0.3, ep); + + set_speed_and_reset_prev(15.0); + EXPECT_NEAR(_limitSteer(orig_cmd).lateral.steering_tire_angle, 0.3, ep); + } + + // steer angle rate lim + // p.reference_speed_points = std::vector{2.0, 4.0, 10.0}; + // p.steer_rate_lim = std::vector{0.2, 0.1, 0.05}; + { + const auto calcSteerRateFromAngle = [&](const auto & cmd) { + return (cmd.steering_tire_angle - 0.0) / DT; + }; + autoware_auto_control_msgs::msg::AckermannLateralCommand filtered; + + set_speed_and_reset_prev(0.0); + filtered = _limitSteerRate(orig_cmd).lateral; + EXPECT_NEAR(calcSteerRateFromAngle(filtered), 0.2, ep); + EXPECT_NEAR(filtered.steering_tire_rotation_rate, 0.2, ep); + + set_speed_and_reset_prev(2.0); + filtered = _limitSteerRate(orig_cmd).lateral; + EXPECT_NEAR(calcSteerRateFromAngle(filtered), 0.2, ep); + EXPECT_NEAR(filtered.steering_tire_rotation_rate, 0.2, ep); + + set_speed_and_reset_prev(3.0); + filtered = _limitSteerRate(orig_cmd).lateral; + EXPECT_NEAR(calcSteerRateFromAngle(filtered), 0.15, ep); + EXPECT_NEAR(filtered.steering_tire_rotation_rate, 0.15, ep); + + set_speed_and_reset_prev(5.0); + filtered = _limitSteerRate(orig_cmd).lateral; + EXPECT_NEAR(calcSteerRateFromAngle(filtered), 0.1 - 0.05 * 1.0 / 6.0, ep); + EXPECT_NEAR(filtered.steering_tire_rotation_rate, 0.1 - 0.05 * 1.0 / 6.0, ep); + + set_speed_and_reset_prev(8.0); + filtered = _limitSteerRate(orig_cmd).lateral; + EXPECT_NEAR(calcSteerRateFromAngle(filtered), 0.1 - 0.05 * 4.0 / 6.0, ep); + EXPECT_NEAR(filtered.steering_tire_rotation_rate, 0.1 - 0.05 * 4.0 / 6.0, ep); + + set_speed_and_reset_prev(10.0); + filtered = _limitSteerRate(orig_cmd).lateral; + EXPECT_NEAR(calcSteerRateFromAngle(filtered), 0.05, ep); + EXPECT_NEAR(filtered.steering_tire_rotation_rate, 0.05, ep); + + set_speed_and_reset_prev(15.0); + filtered = _limitSteerRate(orig_cmd).lateral; + EXPECT_NEAR(calcSteerRateFromAngle(filtered), 0.05, ep); + EXPECT_NEAR(filtered.steering_tire_rotation_rate, 0.05, ep); + } + // longitudinal acc lim { set_speed_and_reset_prev(0.0); @@ -371,66 +489,72 @@ TEST(VehicleCmdFilter, VehicleCmdFilterInterpolate) // lateral acc lim // p.reference_speed_points = std::vector{2.0, 4.0, 10.0}; // p.lat_acc_lim = std::vector{0.1, 0.2, 0.3}; - const auto _calcLatAcc = [&](const auto & cmd) { return calcLatAcc(cmd, WHEELBASE); }; + const auto _calcLatAcc = [&](const auto & cmd, const double ego_v) { + return calcLatAcc(cmd, WHEELBASE, ego_v); + }; { + // since the lateral acceleration is 0 when the velocity is 0, the target value is 0 only in + // this case set_speed_and_reset_prev(0.0); - EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatAcc(orig_cmd)), 0.1, ep); + EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatAcc(orig_cmd), 0.0), 0.0, ep); set_speed_and_reset_prev(2.0); - EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatAcc(orig_cmd)), 0.1, ep); + EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatAcc(orig_cmd), 2.0), 0.1, ep); set_speed_and_reset_prev(3.0); - EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatAcc(orig_cmd)), 0.15, ep); + EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatAcc(orig_cmd), 3.0), 0.15, ep); set_speed_and_reset_prev(5.0); - EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatAcc(orig_cmd)), 0.2 + 0.1 / 6.0, ep); + EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatAcc(orig_cmd), 5.0), 0.2 + 0.1 / 6.0, ep); set_speed_and_reset_prev(8.0); - EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatAcc(orig_cmd)), 0.2 + 0.4 / 6.0, ep); + EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatAcc(orig_cmd), 8.0), 0.2 + 0.4 / 6.0, ep); set_speed_and_reset_prev(10.0); - EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatAcc(orig_cmd)), 0.3, ep); + EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatAcc(orig_cmd), 10.0), 0.3, ep); set_speed_and_reset_prev(15.0); - EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatAcc(orig_cmd)), 0.3, ep); + EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatAcc(orig_cmd), 15.0), 0.3, ep); } // lateral jerk lim // p.reference_speed_points = std::vector{2.0, 4.0, 10.0}; // p.lat_jerk_lim = std::vector{0.9, 0.7, 0.1}; - const auto _calcLatJerk = [&](const auto & cmd) { - return calcLatJerk(cmd, AckermannControlCommand{}, WHEELBASE, DT); + const auto _calcLatJerk = [&](const auto & cmd, const double ego_v) { + return calcLatJerk(cmd, AckermannControlCommand{}, WHEELBASE, DT, ego_v); }; { + // since the lateral acceleration and jerk is 0 when the velocity is 0, the target value is 0 + // only in this case set_speed_and_reset_prev(0.0); - EXPECT_NEAR(_calcLatJerk(_limitLateralWithLatJerk(orig_cmd)), 0.9, ep); - EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatJerk(orig_cmd)), DT * 0.9, ep); + EXPECT_NEAR(_calcLatJerk(_limitLateralWithLatJerk(orig_cmd), 0.0), 0.0, ep); + EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatJerk(orig_cmd), 0.0), DT * 0.0, ep); set_speed_and_reset_prev(2.0); - EXPECT_NEAR(_calcLatJerk(_limitLateralWithLatJerk(orig_cmd)), 0.9, ep); - EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatJerk(orig_cmd)), DT * 0.9, ep); + EXPECT_NEAR(_calcLatJerk(_limitLateralWithLatJerk(orig_cmd), 2.0), 0.9, ep); + EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatJerk(orig_cmd), 2.0), DT * 0.9, ep); set_speed_and_reset_prev(3.0); - EXPECT_NEAR(_calcLatJerk(_limitLateralWithLatJerk(orig_cmd)), 0.8, ep); - EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatJerk(orig_cmd)), DT * 0.8, ep); + EXPECT_NEAR(_calcLatJerk(_limitLateralWithLatJerk(orig_cmd), 3.0), 0.8, ep); + EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatJerk(orig_cmd), 3.0), DT * 0.8, ep); set_speed_and_reset_prev(5.0); const auto expect_v5 = 0.7 - 0.6 * (1.0 / 6.0); - EXPECT_NEAR(_calcLatJerk(_limitLateralWithLatJerk(orig_cmd)), expect_v5, ep); - EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatJerk(orig_cmd)), DT * expect_v5, ep); + EXPECT_NEAR(_calcLatJerk(_limitLateralWithLatJerk(orig_cmd), 5.0), expect_v5, ep); + EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatJerk(orig_cmd), 5.0), DT * expect_v5, ep); set_speed_and_reset_prev(8.0); const auto expect_v8 = 0.7 - 0.6 * (4.0 / 6.0); - EXPECT_NEAR(_calcLatJerk(_limitLateralWithLatJerk(orig_cmd)), expect_v8, ep); - EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatJerk(orig_cmd)), DT * expect_v8, ep); + EXPECT_NEAR(_calcLatJerk(_limitLateralWithLatJerk(orig_cmd), 8.0), expect_v8, ep); + EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatJerk(orig_cmd), 8.0), DT * expect_v8, ep); set_speed_and_reset_prev(10.0); - EXPECT_NEAR(_calcLatJerk(_limitLateralWithLatJerk(orig_cmd)), 0.1, ep); - EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatJerk(orig_cmd)), DT * 0.1, ep); + EXPECT_NEAR(_calcLatJerk(_limitLateralWithLatJerk(orig_cmd), 10.0), 0.1, ep); + EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatJerk(orig_cmd), 10.0), DT * 0.1, ep); set_speed_and_reset_prev(15.0); - EXPECT_NEAR(_calcLatJerk(_limitLateralWithLatJerk(orig_cmd)), 0.1, ep); - EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatJerk(orig_cmd)), DT * 0.1, ep); + EXPECT_NEAR(_calcLatJerk(_limitLateralWithLatJerk(orig_cmd), 15.0), 0.1, ep); + EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatJerk(orig_cmd), 15.0), DT * 0.1, ep); } // steering diff lim diff --git a/evaluator/kinematic_evaluator/src/metrics/kinematic_metrics.cpp b/evaluator/kinematic_evaluator/src/metrics/kinematic_metrics.cpp index 2a22692dc5ac9..2bb0dad86bf71 100644 --- a/evaluator/kinematic_evaluator/src/metrics/kinematic_metrics.cpp +++ b/evaluator/kinematic_evaluator/src/metrics/kinematic_metrics.cpp @@ -14,8 +14,6 @@ #include "kinematic_evaluator/metrics/kinematic_metrics.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" - namespace kinematic_diagnostics { namespace metrics diff --git a/evaluator/localization_evaluator/src/metrics/localization_metrics.cpp b/evaluator/localization_evaluator/src/metrics/localization_metrics.cpp index 240a7dd91e9b2..fab3377913bd4 100644 --- a/evaluator/localization_evaluator/src/metrics/localization_metrics.cpp +++ b/evaluator/localization_evaluator/src/metrics/localization_metrics.cpp @@ -14,7 +14,7 @@ #include "localization_evaluator/metrics/localization_metrics.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" +#include namespace localization_diagnostics { diff --git a/evaluator/planning_evaluator/src/metrics/deviation_metrics.cpp b/evaluator/planning_evaluator/src/metrics/deviation_metrics.cpp index 5146fcd3ec0f2..e1ad9c1a2eeec 100644 --- a/evaluator/planning_evaluator/src/metrics/deviation_metrics.cpp +++ b/evaluator/planning_evaluator/src/metrics/deviation_metrics.cpp @@ -15,7 +15,8 @@ #include "planning_evaluator/metrics/deviation_metrics.hpp" #include "motion_utils/trajectory/trajectory.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" +#include "tier4_autoware_utils/geometry/pose_deviation.hpp" namespace planning_diagnostics { diff --git a/evaluator/planning_evaluator/src/metrics/metrics_utils.cpp b/evaluator/planning_evaluator/src/metrics/metrics_utils.cpp index e2fd04a1d7a6e..6134a100f31a4 100644 --- a/evaluator/planning_evaluator/src/metrics/metrics_utils.cpp +++ b/evaluator/planning_evaluator/src/metrics/metrics_utils.cpp @@ -13,8 +13,7 @@ // limitations under the License. #include "planning_evaluator/metrics/trajectory_metrics.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" - +#include "tier4_autoware_utils/geometry/geometry.hpp" namespace planning_diagnostics { namespace metrics diff --git a/evaluator/planning_evaluator/src/metrics/obstacle_metrics.cpp b/evaluator/planning_evaluator/src/metrics/obstacle_metrics.cpp index 4d97757c3c33e..82d6dcc51097e 100644 --- a/evaluator/planning_evaluator/src/metrics/obstacle_metrics.cpp +++ b/evaluator/planning_evaluator/src/metrics/obstacle_metrics.cpp @@ -14,7 +14,7 @@ #include "planning_evaluator/metrics/obstacle_metrics.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" #include diff --git a/evaluator/planning_evaluator/src/metrics/stability_metrics.cpp b/evaluator/planning_evaluator/src/metrics/stability_metrics.cpp index abd3ca5584951..7a3418da881c6 100644 --- a/evaluator/planning_evaluator/src/metrics/stability_metrics.cpp +++ b/evaluator/planning_evaluator/src/metrics/stability_metrics.cpp @@ -15,7 +15,7 @@ #include "planning_evaluator/metrics/stability_metrics.hpp" #include "motion_utils/trajectory/trajectory.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" #include diff --git a/evaluator/planning_evaluator/src/metrics/trajectory_metrics.cpp b/evaluator/planning_evaluator/src/metrics/trajectory_metrics.cpp index cba03e8b2d90e..d0d8d61c4c231 100644 --- a/evaluator/planning_evaluator/src/metrics/trajectory_metrics.cpp +++ b/evaluator/planning_evaluator/src/metrics/trajectory_metrics.cpp @@ -15,8 +15,7 @@ #include "planning_evaluator/metrics/trajectory_metrics.hpp" #include "planning_evaluator/metrics/metrics_utils.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" - +#include "tier4_autoware_utils/geometry/geometry.hpp" namespace planning_diagnostics { namespace metrics diff --git a/evaluator/planning_evaluator/src/metrics_calculator.cpp b/evaluator/planning_evaluator/src/metrics_calculator.cpp index da04d6fcd2bfa..38113513dbad2 100644 --- a/evaluator/planning_evaluator/src/metrics_calculator.cpp +++ b/evaluator/planning_evaluator/src/metrics_calculator.cpp @@ -14,13 +14,12 @@ #include "planning_evaluator/metrics_calculator.hpp" -#include "motion_utils/motion_utils.hpp" +#include "motion_utils/trajectory/trajectory.hpp" #include "planning_evaluator/metrics/deviation_metrics.hpp" #include "planning_evaluator/metrics/obstacle_metrics.hpp" #include "planning_evaluator/metrics/stability_metrics.hpp" #include "planning_evaluator/metrics/trajectory_metrics.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" - +#include "tier4_autoware_utils/geometry/geometry.hpp" namespace planning_diagnostics { std::optional> MetricsCalculator::calculate( diff --git a/launch/tier4_control_launch/launch/control.launch.py b/launch/tier4_control_launch/launch/control.launch.py index 116055dd9e87e..06718a2cf0ffa 100644 --- a/launch/tier4_control_launch/launch/control.launch.py +++ b/launch/tier4_control_launch/launch/control.launch.py @@ -63,6 +63,8 @@ def launch_setup(context, *args, **kwargs): obstacle_collision_checker_param = yaml.safe_load(f)["/**"]["ros__parameters"] with open(LaunchConfiguration("aeb_param_path").perform(context), "r") as f: aeb_param = yaml.safe_load(f)["/**"]["ros__parameters"] + with open(LaunchConfiguration("predicted_path_checker_param_path").perform(context), "r") as f: + predicted_path_checker_param = yaml.safe_load(f)["/**"]["ros__parameters"] controller_component = ComposableNode( package="trajectory_follower_node", @@ -176,6 +178,34 @@ def launch_setup(context, *args, **kwargs): target_container="/control/control_container", ) + # autonomous emergency braking + predicted_path_checker = ComposableNode( + package="predicted_path_checker", + plugin="autoware::motion::control::predicted_path_checker::PredictedPathCheckerNode", + name="predicted_path_checker", + remappings=[ + ("~/input/objects", "/perception/object_recognition/objects"), + ("~/input/reference_trajectory", "/planning/scenario_planning/trajectory"), + ("~/input/current_accel", "/localization/acceleration"), + ("~/input/odometry", "/localization/kinematic_state"), + ( + "~/input/predicted_trajectory", + "/control/trajectory_follower/lateral/predicted_trajectory", + ), + ], + parameters=[ + vehicle_info_param, + predicted_path_checker_param, + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + + predicted_path_checker_loader = LoadComposableNodes( + condition=IfCondition(LaunchConfiguration("enable_predicted_path_checker")), + composable_node_descriptions=[predicted_path_checker], + target_container="/control/control_container", + ) + # vehicle cmd gate vehicle_cmd_gate_component = ComposableNode( package="vehicle_cmd_gate", @@ -239,6 +269,7 @@ def launch_setup(context, *args, **kwargs): ("steering", "/vehicle/status/steering_status"), ("trajectory", "/planning/scenario_planning/trajectory"), ("control_cmd", "/control/command/control_cmd"), + ("trajectory_follower_control_cmd", "/control/trajectory_follower/control_cmd"), ("control_mode_report", "/vehicle/status/control_mode"), ("gate_operation_mode", "/control/vehicle_cmd_gate/operation_mode"), # output @@ -337,6 +368,7 @@ def launch_setup(context, *args, **kwargs): external_cmd_converter_loader, obstacle_collision_checker_loader, autonomous_emergency_braking_loader, + predicted_path_checker_loader, ] ) @@ -371,6 +403,8 @@ def add_launch_arg(name: str, default_value=None, description=None): add_launch_arg("obstacle_collision_checker_param_path") add_launch_arg("external_cmd_selector_param_path") add_launch_arg("aeb_param_path") + add_launch_arg("predicted_path_checker_param_path") + add_launch_arg("enable_predicted_path_checker") add_launch_arg("enable_autonomous_emergency_braking") add_launch_arg("check_external_emergency_heartbeat") diff --git a/launch/tier4_localization_launch/launch/pose_twist_estimator/ar_tag_based_localizer.launch.xml b/launch/tier4_localization_launch/launch/pose_twist_estimator/ar_tag_based_localizer.launch.xml index 4de9aec81bb38..a767d2d253cff 100644 --- a/launch/tier4_localization_launch/launch/pose_twist_estimator/ar_tag_based_localizer.launch.xml +++ b/launch/tier4_localization_launch/launch/pose_twist_estimator/ar_tag_based_localizer.launch.xml @@ -4,8 +4,9 @@ - - + + + @@ -13,9 +14,9 @@ - + - + diff --git a/launch/tier4_localization_launch/launch/pose_twist_estimator/pose_twist_estimator.launch.xml b/launch/tier4_localization_launch/launch/pose_twist_estimator/pose_twist_estimator.launch.xml index abbb15c797d19..7733b4e3117a1 100644 --- a/launch/tier4_localization_launch/launch/pose_twist_estimator/pose_twist_estimator.launch.xml +++ b/launch/tier4_localization_launch/launch/pose_twist_estimator/pose_twist_estimator.launch.xml @@ -20,7 +20,9 @@ - + + + @@ -42,7 +44,9 @@ - + + + @@ -74,7 +78,9 @@ - + + + @@ -99,7 +105,9 @@ - + + + @@ -131,7 +139,9 @@ - + + + diff --git a/launch/tier4_localization_launch/launch/pose_twist_estimator/yabloc.launch.xml b/launch/tier4_localization_launch/launch/pose_twist_estimator/yabloc.launch.xml index a9b0b93947054..8f0191d6156fe 100644 --- a/launch/tier4_localization_launch/launch/pose_twist_estimator/yabloc.launch.xml +++ b/launch/tier4_localization_launch/launch/pose_twist_estimator/yabloc.launch.xml @@ -1,7 +1,7 @@ - + diff --git a/launch/tier4_localization_launch/package.xml b/launch/tier4_localization_launch/package.xml index f00752d1c14fc..6b3b3d3bbbe01 100644 --- a/launch/tier4_localization_launch/package.xml +++ b/launch/tier4_localization_launch/package.xml @@ -21,6 +21,7 @@ eagleye_rt ekf_localizer gyro_odometer + landmark_tf_caster ndt_scan_matcher pointcloud_preprocessor pose_initializer 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 79e04e1d6fc61..59fe3f13f1231 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 @@ -1,8 +1,25 @@ + + + + - + + + + + + + + + + + + + + @@ -28,16 +45,6 @@ - - - - - - - - - - - + - + + @@ -68,13 +76,54 @@ - - + + - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + @@ -89,6 +138,8 @@ + + @@ -119,7 +170,8 @@ - + + @@ -140,7 +192,7 @@ - + @@ -264,6 +316,7 @@ + @@ -276,19 +329,24 @@ + + + + - + - + + @@ -303,16 +361,20 @@ + + - + + + - + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml index 3a24ec2fdf500..11deaad5d06cc 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml @@ -1,70 +1,74 @@ - + + + + - - - + + + + + + - - + + + + - + + + + + + + + + - + + + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + - + + + + @@ -80,10 +84,351 @@ - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml index ca5d1d0f7e8bb..1fcf29606891b 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml @@ -1,16 +1,16 @@ + - + - - + @@ -29,14 +29,16 @@ - - - - - + + + + + + + - + @@ -63,12 +65,18 @@ - + + + + + + + - + @@ -94,10 +102,12 @@ + + - + @@ -106,11 +116,14 @@ + + + - + @@ -119,14 +132,16 @@ + - + + 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 5b6134dc45584..822c251ddad33 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 @@ -4,44 +4,40 @@ - - - + - + - + - + + - - - + - - + @@ -62,7 +58,7 @@ - + @@ -74,7 +70,7 @@ - + @@ -119,6 +115,7 @@ + @@ -131,18 +128,23 @@ + + + + - + - + + @@ -157,16 +159,20 @@ + + - + + + - + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_radar_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_radar_based_detection.launch.xml index a47e6a86f1c7b..cee6829ea3b4a 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_radar_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_radar_based_detection.launch.xml @@ -1,32 +1,33 @@ - + - - + - + + - + + - + - + @@ -42,7 +43,7 @@ - + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/pointcloud_map_filter.launch.py b/launch/tier4_perception_launch/launch/object_recognition/detection/pointcloud_map_filter.launch.py index a5d8d2113d2db..93d395ca3e466 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/pointcloud_map_filter.launch.py +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/pointcloud_map_filter.launch.py @@ -35,7 +35,6 @@ def __init__(self, context): ) with open(pointcloud_map_filter_param_path, "r") as f: self.pointcloud_map_filter_param = yaml.safe_load(f)["/**"]["ros__parameters"] - self.use_down_sample_filter = self.pointcloud_map_filter_param["use_down_sample_filter"] self.voxel_size = self.pointcloud_map_filter_param["down_sample_voxel_size"] self.distance_threshold = self.pointcloud_map_filter_param["distance_threshold"] self.downsize_ratio_z_axis = self.pointcloud_map_filter_param["downsize_ratio_z_axis"] @@ -46,47 +45,40 @@ def __init__(self, context): ] self.map_loader_radius = self.pointcloud_map_filter_param["map_loader_radius"] self.publish_debug_pcd = self.pointcloud_map_filter_param["publish_debug_pcd"] + self.use_pointcloud_map = LaunchConfiguration("use_pointcloud_map").perform(context) def create_pipeline(self): - if self.use_down_sample_filter: - return self.create_down_sample_pipeline() + if self.use_pointcloud_map == "true": + return self.create_compare_map_pipeline() else: - return self.create_normal_pipeline() + return self.create_no_compare_map_pipeline() - def create_normal_pipeline(self): + def create_no_compare_map_pipeline(self): components = [] components.append( ComposableNode( - package="compare_map_segmentation", - plugin="compare_map_segmentation::VoxelBasedCompareMapFilterComponent", - name="voxel_based_compare_map_filter", + package="pointcloud_preprocessor", + plugin="pointcloud_preprocessor::ApproximateDownsampleFilterComponent", + name="voxel_grid_downsample_filter", remappings=[ ("input", LaunchConfiguration("input_topic")), - ("map", "/map/pointcloud_map"), ("output", LaunchConfiguration("output_topic")), - ("map_loader_service", "/map/get_differential_pointcloud_map"), - ("kinematic_state", "/localization/kinematic_state"), ], parameters=[ { - "distance_threshold": self.distance_threshold, - "downsize_ratio_z_axis": self.downsize_ratio_z_axis, - "timer_interval_ms": self.timer_interval_ms, - "use_dynamic_map_loading": self.use_dynamic_map_loading, - "map_update_distance_threshold": self.map_update_distance_threshold, - "map_loader_radius": self.map_loader_radius, - "publish_debug_pcd": self.publish_debug_pcd, - "input_frame": "map", + "voxel_size_x": self.voxel_size, + "voxel_size_y": self.voxel_size, + "voxel_size_z": self.voxel_size, } ], extra_arguments=[ - {"use_intra_process_comms": False}, + {"use_intra_process_comms": LaunchConfiguration("use_intra_process")} ], - ) + ), ) return components - def create_down_sample_pipeline(self): + def create_compare_map_pipeline(self): components = [] down_sample_topic = ( "/perception/obstacle_segmentation/pointcloud_map_filtered/downsampled/pointcloud" @@ -94,7 +86,7 @@ def create_down_sample_pipeline(self): components.append( ComposableNode( package="pointcloud_preprocessor", - plugin="pointcloud_preprocessor::VoxelGridDownsampleFilterComponent", + plugin="pointcloud_preprocessor::ApproximateDownsampleFilterComponent", name="voxel_grid_downsample_filter", remappings=[ ("input", LaunchConfiguration("input_topic")), @@ -177,6 +169,7 @@ def add_launch_arg(name: str, default_value=None): add_launch_arg("use_intra_process", "True") add_launch_arg("use_pointcloud_container", "False") add_launch_arg("container_name", "pointcloud_map_filter_pipeline_container") + add_launch_arg("use_pointcloud_map", "true") set_container_executable = SetLaunchConfiguration( "container_executable", "component_container", diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/radar_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/radar_based_detection.launch.xml index 8e2e23b510a07..5b5646a061ac7 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/radar_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/radar_based_detection.launch.xml @@ -2,14 +2,14 @@ - - - - - + + + + + - - + + @@ -20,14 +20,8 @@ - - - - - - - + @@ -40,8 +34,14 @@ + + + + + + - + diff --git a/launch/tier4_perception_launch/launch/object_recognition/prediction/prediction.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/prediction/prediction.launch.xml index 711e6374f5786..fa343f49840ad 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/prediction/prediction.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/prediction/prediction.launch.xml @@ -1,12 +1,13 @@ - + + diff --git a/launch/tier4_perception_launch/launch/object_recognition/tracking/tracking.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/tracking/tracking.launch.xml index 1ba1e8de9d42e..07b53fb671732 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/tracking/tracking.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/tracking/tracking.launch.xml @@ -3,27 +3,54 @@ + + + + + + + + - + + + - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py b/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py index 7ed5860612601..12190fb659235 100644 --- a/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py +++ b/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py @@ -122,12 +122,16 @@ def create_ransac_pipeline(self): plugin="pointcloud_preprocessor::PointCloudConcatenateDataSynchronizerComponent", name="concatenate_data", namespace="plane_fitting", - remappings=[("output", "concatenated/pointcloud")], + remappings=[ + ("~/input/odom", "/localization/kinematic_state"), + ("output", "concatenated/pointcloud"), + ], parameters=[ { "input_topics": self.ground_segmentation_param["ransac_input_topics"], "output_frame": LaunchConfiguration("base_frame"), "timeout_sec": 1.0, + "input_twist_topic_type": "odom", } ], extra_arguments=[ @@ -432,11 +436,15 @@ def get_additional_lidars_concatenated_component(input_topics, output_topic): package="pointcloud_preprocessor", plugin="pointcloud_preprocessor::PointCloudConcatenateDataSynchronizerComponent", name="concatenate_data", - remappings=[("output", output_topic)], + remappings=[ + ("~/input/odom", "/localization/kinematic_state"), + ("output", output_topic), + ], parameters=[ { "input_topics": input_topics, "output_frame": LaunchConfiguration("base_frame"), + "input_twist_topic_type": "odom", } ], extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], @@ -448,11 +456,15 @@ def get_single_frame_obstacle_segmentation_concatenated_component(input_topics, package="pointcloud_preprocessor", plugin="pointcloud_preprocessor::PointCloudConcatenateDataSynchronizerComponent", name="concatenate_no_ground_data", - remappings=[("output", output_topic)], + remappings=[ + ("~/input/odom", "/localization/kinematic_state"), + ("output", output_topic), + ], parameters=[ { "input_topics": input_topics, "output_frame": LaunchConfiguration("base_frame"), + "input_twist_topic_type": "odom", } ], extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], diff --git a/launch/tier4_perception_launch/launch/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml index 6634c36ff3768..347606d91759f 100644 --- a/launch/tier4_perception_launch/launch/perception.launch.xml +++ b/launch/tier4_perception_launch/launch/perception.launch.xml @@ -1,10 +1,8 @@ - - @@ -13,24 +11,27 @@ + + - + - + - + + @@ -59,8 +60,9 @@ - + + - + + - + + + + + + - + + - + + + + + + + - + @@ -96,7 +123,7 @@ - + @@ -114,10 +141,10 @@ - + - + @@ -143,29 +170,39 @@ - - + - + - + + + - + + + + + + - + + @@ -181,7 +218,7 @@ - + diff --git a/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light.launch.xml b/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light.launch.xml index 1f86a89ddfc7a..58e9e231e4aa0 100644 --- a/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light.launch.xml +++ b/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light.launch.xml @@ -3,20 +3,30 @@ - - - - + + + + + + + + + + + + + + @@ -62,7 +72,7 @@ - + @@ -112,7 +122,7 @@ - + @@ -124,9 +134,28 @@ + + + + + + + + + + + + + + + + + + + diff --git a/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light_node_container.launch.py b/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light_node_container.launch.py index 8b3d15f2cff95..a6bcb40e81252 100644 --- a/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light_node_container.launch.py +++ b/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light_node_container.launch.py @@ -73,7 +73,6 @@ def add_launch_arg(name: str, default_value=None, description=None): add_launch_arg("classifier_mean", "[123.675, 116.28, 103.53]") add_launch_arg("classifier_std", "[58.395, 57.12, 57.375]") - add_launch_arg("use_crosswalk_traffic_light_estimator", "True") add_launch_arg("use_intra_process", "False") add_launch_arg("use_multithread", "False") @@ -137,46 +136,6 @@ def create_parameter_dict(*args): output="both", ) - estimator_loader = LoadComposableNodes( - composable_node_descriptions=[ - ComposableNode( - package="crosswalk_traffic_light_estimator", - plugin="traffic_light::CrosswalkTrafficLightEstimatorNode", - name="crosswalk_traffic_light_estimator", - namespace="classification", - remappings=[ - ("~/input/vector_map", "/map/vector_map"), - ("~/input/route", "/planning/mission_planning/route"), - ("~/input/classified/traffic_signals", "classified/traffic_signals"), - ("~/output/traffic_signals", "estimated/traffic_signals"), - ], - extra_arguments=[{"use_intra_process_comms": False}], - ), - ], - target_container=container, - condition=IfCondition(LaunchConfiguration("use_crosswalk_traffic_light_estimator")), - ) - - relay_loader = LoadComposableNodes( - composable_node_descriptions=[ - ComposableNode( - package="topic_tools", - plugin="topic_tools::RelayNode", - name="classified_signals_relay", - namespace="classification", - parameters=[ - {"input_topic": "classified/traffic_signals"}, - {"output_topic": "estimated/traffic_signals"}, - ], - extra_arguments=[ - {"use_intra_process_comms": LaunchConfiguration("use_intra_process")} - ], - ) - ], - target_container=container, - condition=UnlessCondition(LaunchConfiguration("use_crosswalk_traffic_light_estimator")), - ) - decompressor_loader = LoadComposableNodes( composable_node_descriptions=[ ComposableNode( @@ -251,7 +210,5 @@ def create_parameter_dict(*args): container, decompressor_loader, fine_detector_loader, - estimator_loader, - relay_loader, ] ) diff --git a/launch/tier4_perception_launch/package.xml b/launch/tier4_perception_launch/package.xml index c1723c1fa07e8..336588891d9b2 100644 --- a/launch/tier4_perception_launch/package.xml +++ b/launch/tier4_perception_launch/package.xml @@ -12,6 +12,7 @@ ament_cmake_auto autoware_cmake + cluster_merger compare_map_segmentation crosswalk_traffic_light_estimator detected_object_feature_remover diff --git a/launch/tier4_planning_launch/launch/planning.launch.xml b/launch/tier4_planning_launch/launch/planning.launch.xml index 7e78c8842f7df..6ace3f423d1bc 100644 --- a/launch/tier4_planning_launch/launch/planning.launch.xml +++ b/launch/tier4_planning_launch/launch/planning.launch.xml @@ -15,8 +15,6 @@ - - diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving.launch.xml index 24a33b9d3f3df..5f17256df6b5c 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving.launch.xml @@ -14,11 +14,6 @@ - - - - - diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py index 59e8038e49188..62d4c5b7188ee 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py @@ -177,10 +177,6 @@ def launch_setup(context, *args, **kwargs): "~/input/traffic_signals", "/perception/traffic_light_recognition/traffic_signals", ), - ( - "~/input/external_traffic_signals", - "/external/traffic_light_recognition/traffic_signals", - ), ( "~/input/external_velocity_limit_mps", "/planning/scenario_planning/max_velocity_default", diff --git a/launch/tier4_planning_launch/package.xml b/launch/tier4_planning_launch/package.xml index 39b95286bb6cc..5988d34cded88 100644 --- a/launch/tier4_planning_launch/package.xml +++ b/launch/tier4_planning_launch/package.xml @@ -68,7 +68,6 @@ obstacle_stop_planner planning_evaluator planning_validator - rtc_auto_mode_manager scenario_selector surround_obstacle_checker diff --git a/launch/tier4_simulator_launch/launch/simulator.launch.xml b/launch/tier4_simulator_launch/launch/simulator.launch.xml index aef1e45f517b1..4da6720b0af47 100644 --- a/launch/tier4_simulator_launch/launch/simulator.launch.xml +++ b/launch/tier4_simulator_launch/launch/simulator.launch.xml @@ -65,7 +65,16 @@ - + + + + + + + diff --git a/launch/tier4_system_launch/launch/system.launch.xml b/launch/tier4_system_launch/launch/system.launch.xml index 016d613cfa72d..2de6a61547498 100644 --- a/launch/tier4_system_launch/launch/system.launch.xml +++ b/launch/tier4_system_launch/launch/system.launch.xml @@ -3,6 +3,7 @@ + @@ -84,6 +85,12 @@ + + + + + + diff --git a/localization/ar_tag_based_localizer/README.md b/localization/ar_tag_based_localizer/README.md deleted file mode 100644 index e471378773cbb..0000000000000 --- a/localization/ar_tag_based_localizer/README.md +++ /dev/null @@ -1,154 +0,0 @@ -# AR Tag Based Localizer - -**ArTagBasedLocalizer** is a vision-based localization package. - - - -This package uses [the ArUco library](https://index.ros.org/p/aruco/) to detect AR-Tags from camera images and calculates and publishes the pose of the ego vehicle based on these detections. -The positions and orientations of the AR-Tags are assumed to be written in the Lanelet2 format. - -This package includes two nodes. - -- `ar_tag_based_localizer` : A node that detects AR-Tags from camera images and publishes the pose of the ego vehicle. -- `tag_tf_caster` : A node that publishes the pose of the AR-Tags applied in Lanelet2 as `tf_static`. - -## Inputs / Outputs - -### `ar_tag_based_localizer` node - -#### Input - -| Name | Type | Description | -| :-------------------- | :----------------------------- | :----------- | -| `~/input/image` | `sensor_msgs::msg::Image` | Camera Image | -| `~/input/camera_info` | `sensor_msgs::msg::CameraInfo` | Camera Info | - -#### Output - -| Name | Type | Description | -| :------------------------------ | :---------------------------------------------- | :---------------------------------------------------------------------------------------- | -| `~/output/pose_with_covariance` | `geometry_msgs::msg::PoseWithCovarianceStamped` | Estimated Pose | -| `~/debug/result` | `sensor_msgs::msg::Image` | [debug topic] Image in which marker detection results are superimposed on the input image | -| `tf` | `geometry_msgs::msg::TransformStamped` | [debug topic] TF from camera to detected tag | - -### `tag_tf_caster` node - -#### Input - -| Name | Type | Description | -| :--------------------- | :------------------------------------------- | :--------------- | -| `~/input/lanelet2_map` | `autoware_auto_mapping_msgs::msg::HADMapBin` | Data of lanelet2 | - -#### Output - -| Name | Type | Description | -| :---------- | :------------------------------------- | :----------------- | -| `tf_static` | `geometry_msgs::msg::TransformStamped` | TF from map to tag | - -## How to launch - -When launching Autoware, specify `artag` for `pose_source`. - -```bash -ros2 launch autoware_launch ... \ - pose_source:=artag \ - ... -``` - -[Sample rosbag and map](https://drive.google.com/file/d/1wiCQjyjRnYbb0dg8G6mRecdSGh8tv3zR/view) - -This dataset contains issues such as missing IMU data, and overall the accuracy is low. Even when running AR tag-based self-localization, significant difference from the true trajectory can be observed. - -The image below shows the trajectory when the sample is executed and plotted. - -![sample_result](./doc_image/sample_result.png) - -The pull request video below should also be helpful. - - - -## Architecture - -![node diagram](./doc_image/node_diagram.drawio.svg) - -## Principle - -### `ar_tag_based_localizer` node - -![principle](./doc_image/principle.png) - -### `tag_tf_caster` node - -The definitions of the tags written to the map are introduced in the next section. See `Map Specifications`. - -The `tag_tf_caster` node publishes the TF from the map to the tag. - -- Translation : The center of the four vertices of the tag -- Rotation : Let the vertex numbers be 1, 2, 3, 4 counterclockwise as shown in the next section. Direction is defined as the cross product of the vector from 1 to 2 and the vector from 2 to 3. - -Users can define tags as Lanelet2 4-vertex polygons. -In this case, it is possible to define an arrangement in which the four vertices cannot be considered to be on the same plane. The direction of the tag in that case is difficult to calculate. -So, if the 4 vertices are considered as forming a tetrahedron and its volume exceeds the `volume_threshold` parameter, the tag will not publish tf_static. - -## Map specifications - -For this package to function correctly, the pose of the AR-Tags must be specified in the Lanelet2 map format that Autoware can interpret. - -The four vertices of AR-Tag are defined counterclockwise. - -The order of the four vertices is defined as follows. In the coordinate system of AR-Tag, - -- the x-axis is parallel to the vector from the first vertex to the second vertex -- the y-axis is parallel to the vector from the second vertex to the third vertex - -![lanelet2 data structure](./doc_image/lanelet2_data_structure.drawio.svg) - -### example of `lanelet2_map.osm` - -The values provided below are placeholders. -Ensure to input the correct coordinates corresponding to the actual location where the AR-Tag is placed, such as `lat`, `lon`, `mgrs_code`, `local_x`, `local_y`. - -```xml -... - - - - - - - - - - - - - - - - - - - - - - - - - - -... - - - - - - - - - - - - -... - -``` diff --git a/localization/ar_tag_based_localizer/config/ar_tag_based_localizer.param.yaml b/localization/ar_tag_based_localizer/config/ar_tag_based_localizer.param.yaml deleted file mode 100644 index 385e86498c58c..0000000000000 --- a/localization/ar_tag_based_localizer/config/ar_tag_based_localizer.param.yaml +++ /dev/null @@ -1,26 +0,0 @@ -/**: - ros__parameters: - # marker_size - marker_size: 0.6 - - # target_tag_ids - target_tag_ids: ['0','4','5'] - - # covariance - covariance: [0.1, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.1, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.01] - - # Detect AR-Tags within this range and publish the pose of ego vehicle - distance_threshold: 6.0 # [m] - - # Camera frame id - camera_frame: "camera" - - # Detector parameters - # See https://github.com/pal-robotics/aruco_ros/blob/7787a6794d30c248bc546d1582e65dd47bc40c12/aruco/include/aruco/markerdetector.h#L106-L126 - detection_mode: "DM_NORMAL" # select from [DM_NORMAL, DM_FAST, DM_VIDEO_FAST] - min_marker_size: 0.02 diff --git a/localization/ar_tag_based_localizer/include/ar_tag_based_localizer/utils.hpp b/localization/ar_tag_based_localizer/include/ar_tag_based_localizer/utils.hpp deleted file mode 100644 index 06401dad8fdd1..0000000000000 --- a/localization/ar_tag_based_localizer/include/ar_tag_based_localizer/utils.hpp +++ /dev/null @@ -1,66 +0,0 @@ -// Copyright 2023 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. - -// This source code is derived from the https://github.com/pal-robotics/aruco_ros. -// Here is the license statement. -/***************************** - Copyright 2011 Rafael Muñoz Salinas. All rights reserved. - - Redistribution and use in source and binary forms, with or without modification, are - permitted provided that the following conditions are met: - - 1. Redistributions of source code must retain the above copyright notice, this list of - conditions and the following disclaimer. - - 2. Redistributions in binary form must reproduce the above copyright notice, this list - of conditions and the following disclaimer in the documentation and/or other materials - provided with the distribution. - - THIS SOFTWARE IS PROVIDED BY Rafael Muñoz Salinas ''AS IS'' AND ANY EXPRESS OR IMPLIED - WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND - FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL Rafael Muñoz Salinas OR - CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING - NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF - ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - - The views and conclusions contained in the software and documentation are those of the - authors and should not be interpreted as representing official policies, either expressed - or implied, of Rafael Muñoz Salinas. - ********************************/ - -#ifndef AR_TAG_BASED_LOCALIZER__UTILS_HPP_ -#define AR_TAG_BASED_LOCALIZER__UTILS_HPP_ - -#include - -#include -#include - -/** - * @brief ros_camera_info_to_aruco_cam_params gets the camera intrinsics from a CameraInfo message - * and copies them to aruco's data structure - * @param cam_info - * @param use_rectified_parameters if true, the intrinsics are taken from cam_info.P and the - * distortion parameters are set to 0. Otherwise, cam_info.K and cam_info.D are taken. - * @return - */ -aruco::CameraParameters ros_camera_info_to_aruco_cam_params( - const sensor_msgs::msg::CameraInfo & cam_info, bool use_rectified_parameters); - -tf2::Transform aruco_marker_to_tf2(const aruco::Marker & marker); - -#endif // AR_TAG_BASED_LOCALIZER__UTILS_HPP_ diff --git a/localization/ar_tag_based_localizer/launch/tag_tf_caster.launch.xml b/localization/ar_tag_based_localizer/launch/tag_tf_caster.launch.xml deleted file mode 100644 index 6a9c3dcd17e2e..0000000000000 --- a/localization/ar_tag_based_localizer/launch/tag_tf_caster.launch.xml +++ /dev/null @@ -1,12 +0,0 @@ - - - - - - - - - - - - diff --git a/localization/ar_tag_based_localizer/src/utils.cpp b/localization/ar_tag_based_localizer/src/utils.cpp deleted file mode 100644 index 9f582830280d9..0000000000000 --- a/localization/ar_tag_based_localizer/src/utils.cpp +++ /dev/null @@ -1,116 +0,0 @@ -// Copyright 2023 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. - -// This source code is derived from the https://github.com/pal-robotics/aruco_ros. -// Here is the license statement. -/***************************** - Copyright 2011 Rafael Muñoz Salinas. All rights reserved. - - Redistribution and use in source and binary forms, with or without modification, are - permitted provided that the following conditions are met: - - 1. Redistributions of source code must retain the above copyright notice, this list of - conditions and the following disclaimer. - - 2. Redistributions in binary form must reproduce the above copyright notice, this list - of conditions and the following disclaimer in the documentation and/or other materials - provided with the distribution. - - THIS SOFTWARE IS PROVIDED BY Rafael Muñoz Salinas ''AS IS'' AND ANY EXPRESS OR IMPLIED - WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND - FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL Rafael Muñoz Salinas OR - CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING - NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF - ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - - The views and conclusions contained in the software and documentation are those of the - authors and should not be interpreted as representing official policies, either expressed - or implied, of Rafael Muñoz Salinas. - ********************************/ - -#include "ar_tag_based_localizer/utils.hpp" - -#include -#include - -aruco::CameraParameters ros_camera_info_to_aruco_cam_params( - const sensor_msgs::msg::CameraInfo & cam_info, bool use_rectified_parameters) -{ - cv::Mat camera_matrix(3, 4, CV_64FC1, 0.0); - cv::Mat distortion_coeff(4, 1, CV_64FC1); - cv::Size size(static_cast(cam_info.width), static_cast(cam_info.height)); - - if (use_rectified_parameters) { - camera_matrix.setTo(0); - camera_matrix.at(0, 0) = cam_info.p[0]; - camera_matrix.at(0, 1) = cam_info.p[1]; - camera_matrix.at(0, 2) = cam_info.p[2]; - camera_matrix.at(0, 3) = cam_info.p[3]; - camera_matrix.at(1, 0) = cam_info.p[4]; - camera_matrix.at(1, 1) = cam_info.p[5]; - camera_matrix.at(1, 2) = cam_info.p[6]; - camera_matrix.at(1, 3) = cam_info.p[7]; - camera_matrix.at(2, 0) = cam_info.p[8]; - camera_matrix.at(2, 1) = cam_info.p[9]; - camera_matrix.at(2, 2) = cam_info.p[10]; - camera_matrix.at(2, 3) = cam_info.p[11]; - - for (int i = 0; i < 4; ++i) { - distortion_coeff.at(i, 0) = 0; - } - } else { - cv::Mat camera_matrix_from_k(3, 3, CV_64FC1, 0.0); - for (int i = 0; i < 9; ++i) { - camera_matrix_from_k.at(i % 3, i - (i % 3) * 3) = cam_info.k[i]; - } - camera_matrix_from_k.copyTo(camera_matrix(cv::Rect(0, 0, 3, 3))); - - if (cam_info.d.size() == 4) { - for (int i = 0; i < 4; ++i) { - distortion_coeff.at(i, 0) = cam_info.d[i]; - } - } else { - RCLCPP_WARN( - rclcpp::get_logger("ar_tag_based_localizer"), - "length of camera_info D vector is not 4, assuming zero distortion..."); - for (int i = 0; i < 4; ++i) { - distortion_coeff.at(i, 0) = 0; - } - } - } - - return {camera_matrix, distortion_coeff, size}; -} - -tf2::Transform aruco_marker_to_tf2(const aruco::Marker & marker) -{ - cv::Mat rot(3, 3, CV_64FC1); - cv::Mat r_vec64; - marker.Rvec.convertTo(r_vec64, CV_64FC1); - cv::Rodrigues(r_vec64, rot); - cv::Mat tran64; - marker.Tvec.convertTo(tran64, CV_64FC1); - - tf2::Matrix3x3 tf_rot( - rot.at(0, 0), rot.at(0, 1), rot.at(0, 2), rot.at(1, 0), - rot.at(1, 1), rot.at(1, 2), rot.at(2, 0), rot.at(2, 1), - rot.at(2, 2)); - - tf2::Vector3 tf_orig(tran64.at(0, 0), tran64.at(1, 0), tran64.at(2, 0)); - - return tf2::Transform(tf_rot, tf_orig); -} diff --git a/localization/ekf_localizer/CMakeLists.txt b/localization/ekf_localizer/CMakeLists.txt index 6e7e194f7cf72..05c91bff13867 100644 --- a/localization/ekf_localizer/CMakeLists.txt +++ b/localization/ekf_localizer/CMakeLists.txt @@ -16,6 +16,7 @@ ament_auto_find_build_dependencies() ament_auto_add_library(ekf_localizer_lib SHARED src/ekf_localizer.cpp src/covariance.cpp + src/diagnostics.cpp src/mahalanobis.cpp src/measurement.cpp src/state_transition.cpp @@ -51,14 +52,7 @@ if(BUILD_TESTING) ) find_package(ament_cmake_gtest REQUIRED) - set(TEST_FILES - test/test_aged_object_queue.cpp - test/test_mahalanobis.cpp - test/test_measurement.cpp - test/test_numeric.cpp - test/test_state_transition.cpp - test/test_string.cpp - test/test_warning_message.cpp) + file(GLOB_RECURSE TEST_FILES test/*.cpp) foreach(filepath ${TEST_FILES}) add_testcase(${filepath}) diff --git a/localization/ekf_localizer/README.md b/localization/ekf_localizer/README.md index 748b5ee5becc0..6492f20331a66 100644 --- a/localization/ekf_localizer/README.md +++ b/localization/ekf_localizer/README.md @@ -88,6 +88,10 @@ The parameters and input topic names can be set in the `ekf_localizer.launch` fi The estimated twist with covariance. +- diagnostics (diagnostic_msgs/DiagnosticArray) + + The diagnostic information. + ### Published TF - base_link @@ -148,6 +152,21 @@ The parameters are set in `launch/ekf_localizer.launch` . note: process noise for positions x & y are calculated automatically from nonlinear dynamics. +### For diagnostics + +| Name | Type | Description | Default value | +| :------------------------------------ | :----- | :----------------------------------------------------------------------------------------------------------------------------------------- | :------------ | +| pose_no_update_count_threshold_warn | size_t | The threshold at which a WARN state is triggered due to the Pose Topic update not happening continuously for a certain number of times. | 50 | +| pose_no_update_count_threshold_error | size_t | The threshold at which an ERROR state is triggered due to the Pose Topic update not happening continuously for a certain number of times. | 250 | +| twist_no_update_count_threshold_warn | size_t | The threshold at which a WARN state is triggered due to the Twist Topic update not happening continuously for a certain number of times. | 50 | +| twist_no_update_count_threshold_error | size_t | The threshold at which an ERROR state is triggered due to the Twist Topic update not happening continuously for a certain number of times. | 250 | + +### Misc + +| Name | Type | Description | Default value | +| :-------------------------------- | :----- | :------------------------------------------------------------------------------------------------- | :------------- | +| threshold_observable_velocity_mps | double | Minimum value for velocity that will be used for EKF. Mainly used for dead zone in velocity sensor | 0.0 (disabled) | + ## How to tune EKF parameters ### 0. Preliminaries @@ -194,6 +213,23 @@ Note that, although the dimension gets larger since the analytical expansion can

+## Diagnostics + +

+ +

+ +### The conditions that result in a WARN state + +- The node is not in the activate state. +- The number of consecutive no measurement update via the Pose/Twist topic exceeds the `pose_no_update_count_threshold_warn`/`twist_no_update_count_threshold_warn`. +- The timestamp of the Pose/Twist topic is beyond the delay compensation range. +- The Pose/Twist topic is beyond the range of Mahalanobis distance for covariance estimation. + +### The conditions that result in an ERROR state + +- The number of consecutive no measurement update via the Pose/Twist topic exceeds the `pose_no_update_count_threshold_error`/`twist_no_update_count_threshold_error`. + ## Known issues - In the presence of multiple inputs with yaw estimation, yaw bias `b_k` in the current EKF state would not make any sense, since it is intended to capture the extrinsic parameter's calibration error of a sensor. Thus, future work includes introducing yaw bias for each sensor with yaw estimation. diff --git a/localization/ekf_localizer/config/ekf_localizer.param.yaml b/localization/ekf_localizer/config/ekf_localizer.param.yaml index 4d3f5b9643462..667217d2591dc 100644 --- a/localization/ekf_localizer/config/ekf_localizer.param.yaml +++ b/localization/ekf_localizer/config/ekf_localizer.param.yaml @@ -21,3 +21,12 @@ proc_stddev_yaw_c: 0.005 proc_stddev_vx_c: 10.0 proc_stddev_wz_c: 5.0 + + # for diagnostics + pose_no_update_count_threshold_warn: 50 + pose_no_update_count_threshold_error: 250 + twist_no_update_count_threshold_warn: 50 + twist_no_update_count_threshold_error: 250 + + # for velocity measurement limitation (Set 0.0 if you want to ignore) + threshold_observable_velocity_mps: 0.0 # [m/s] diff --git a/localization/ekf_localizer/include/ekf_localizer/diagnostics.hpp b/localization/ekf_localizer/include/ekf_localizer/diagnostics.hpp new file mode 100644 index 0000000000000..f4dc6436f6a40 --- /dev/null +++ b/localization/ekf_localizer/include/ekf_localizer/diagnostics.hpp @@ -0,0 +1,40 @@ +// Copyright 2023 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__DIAGNOSTICS_HPP_ +#define EKF_LOCALIZER__DIAGNOSTICS_HPP_ + +#include + +#include +#include + +diagnostic_msgs::msg::DiagnosticStatus checkProcessActivated(const bool is_activated); + +diagnostic_msgs::msg::DiagnosticStatus checkMeasurementUpdated( + const std::string & measurement_type, const size_t no_update_count, + const size_t no_update_count_threshold_warn, const size_t no_update_count_threshold_error); +diagnostic_msgs::msg::DiagnosticStatus checkMeasurementQueueSize( + const std::string & measurement_type, const size_t queue_size); +diagnostic_msgs::msg::DiagnosticStatus checkMeasurementDelayGate( + const std::string & measurement_type, const bool is_passed_delay_gate, const double delay_time, + const double delay_time_threshold); +diagnostic_msgs::msg::DiagnosticStatus checkMeasurementMahalanobisGate( + const std::string & measurement_type, const bool is_passed_mahalanobis_gate, + const double mahalanobis_distance, const double mahalanobis_distance_threshold); + +diagnostic_msgs::msg::DiagnosticStatus mergeDiagnosticStatus( + const std::vector & stat_array); + +#endif // EKF_LOCALIZER__DIAGNOSTICS_HPP_ diff --git a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp index a4ae47b670897..4fc2305cc7adc 100644 --- a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp @@ -25,6 +25,7 @@ #include #include +#include #include #include #include @@ -126,6 +127,8 @@ class EKFLocalizer : public rclcpp::Node rclcpp::Publisher::SharedPtr pub_biased_pose_; //!< @brief ekf estimated yaw bias publisher rclcpp::Publisher::SharedPtr pub_biased_pose_cov_; + //!< @brief diagnostics publisher + rclcpp::Publisher::SharedPtr pub_diag_; //!< @brief initial pose subscriber rclcpp::Subscription::SharedPtr sub_initialpose_; //!< @brief measurement pose with covariance subscriber @@ -144,6 +147,7 @@ class EKFLocalizer : public rclcpp::Node rclcpp::TimerBase::SharedPtr timer_tf_; //!< @brief tf broadcaster std::shared_ptr tf_br_; + //!< @brief extended kalman filter instance. TimeDelayKalmanFilter ekf_; Simple1DFilter z_filter_; @@ -167,6 +171,22 @@ class EKFLocalizer : public rclcpp::Node bool is_activated_; + size_t pose_no_update_count_; + size_t pose_queue_size_; + bool pose_is_passed_delay_gate_; + double pose_delay_time_; + double pose_delay_time_threshold_; + bool pose_is_passed_mahalanobis_gate_; + double pose_mahalanobis_distance_; + + size_t twist_no_update_count_; + size_t twist_queue_size_; + bool twist_is_passed_delay_gate_; + double twist_delay_time_; + double twist_delay_time_threshold_; + bool twist_is_passed_mahalanobis_gate_; + double twist_mahalanobis_distance_; + AgedObjectQueue pose_queue_; AgedObjectQueue twist_queue_; @@ -221,13 +241,13 @@ class EKFLocalizer : public rclcpp::Node * @brief compute EKF update with pose measurement * @param pose measurement value */ - void measurementUpdatePose(const geometry_msgs::msg::PoseWithCovarianceStamped & pose); + bool measurementUpdatePose(const geometry_msgs::msg::PoseWithCovarianceStamped & pose); /** * @brief compute EKF update with pose measurement * @param twist measurement value */ - void measurementUpdateTwist(const geometry_msgs::msg::TwistWithCovarianceStamped & twist); + bool measurementUpdateTwist(const geometry_msgs::msg::TwistWithCovarianceStamped & twist); /** * @brief get transform from frame_id @@ -246,6 +266,11 @@ class EKFLocalizer : public rclcpp::Node */ void publishEstimateResult(); + /** + * @brief publish diagnostics message + */ + void publishDiagnostics(); + /** * @brief for debug */ diff --git a/localization/ekf_localizer/include/ekf_localizer/hyper_parameters.hpp b/localization/ekf_localizer/include/ekf_localizer/hyper_parameters.hpp index c74fa9be79525..01ef658cf445d 100644 --- a/localization/ekf_localizer/include/ekf_localizer/hyper_parameters.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/hyper_parameters.hpp @@ -39,7 +39,17 @@ class HyperParameters twist_smoothing_steps(node->declare_parameter("twist_smoothing_steps", 2)), proc_stddev_vx_c(node->declare_parameter("proc_stddev_vx_c", 5.0)), proc_stddev_wz_c(node->declare_parameter("proc_stddev_wz_c", 1.0)), - proc_stddev_yaw_c(node->declare_parameter("proc_stddev_yaw_c", 0.005)) + proc_stddev_yaw_c(node->declare_parameter("proc_stddev_yaw_c", 0.005)), + pose_no_update_count_threshold_warn( + node->declare_parameter("pose_no_update_count_threshold_warn", 50)), + pose_no_update_count_threshold_error( + node->declare_parameter("pose_no_update_count_threshold_error", 250)), + twist_no_update_count_threshold_warn( + node->declare_parameter("twist_no_update_count_threshold_warn", 50)), + twist_no_update_count_threshold_error( + node->declare_parameter("twist_no_update_count_threshold_error", 250)), + threshold_observable_velocity_mps( + node->declare_parameter("threshold_observable_velocity_mps", 0.5)) { } @@ -59,6 +69,11 @@ class HyperParameters const double proc_stddev_vx_c; //!< @brief vx process noise const double proc_stddev_wz_c; //!< @brief wz process noise const double proc_stddev_yaw_c; //!< @brief yaw process noise + const size_t pose_no_update_count_threshold_warn; + const size_t pose_no_update_count_threshold_error; + const size_t twist_no_update_count_threshold_warn; + const size_t twist_no_update_count_threshold_error; + const double threshold_observable_velocity_mps; }; #endif // EKF_LOCALIZER__HYPER_PARAMETERS_HPP_ diff --git a/localization/ekf_localizer/media/ekf_diagnostics.png b/localization/ekf_localizer/media/ekf_diagnostics.png new file mode 100644 index 0000000000000..2580d6d973290 Binary files /dev/null and b/localization/ekf_localizer/media/ekf_diagnostics.png differ diff --git a/localization/ekf_localizer/package.xml b/localization/ekf_localizer/package.xml index 5bc9c5e42712d..4101bae88b6e2 100644 --- a/localization/ekf_localizer/package.xml +++ b/localization/ekf_localizer/package.xml @@ -18,13 +18,12 @@ eigen + diagnostic_msgs fmt geometry_msgs kalman_filter nav_msgs rclcpp - sensor_msgs - std_msgs std_srvs tf2 tf2_ros diff --git a/localization/ekf_localizer/src/diagnostics.cpp b/localization/ekf_localizer/src/diagnostics.cpp new file mode 100644 index 0000000000000..9ff36561abaa9 --- /dev/null +++ b/localization/ekf_localizer/src/diagnostics.cpp @@ -0,0 +1,169 @@ +// Copyright 2023 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/diagnostics.hpp" + +#include + +#include +#include + +diagnostic_msgs::msg::DiagnosticStatus checkProcessActivated(const bool is_activated) +{ + diagnostic_msgs::msg::DiagnosticStatus stat; + + diagnostic_msgs::msg::KeyValue key_value; + key_value.key = "is_activated"; + key_value.value = is_activated ? "True" : "False"; + stat.values.push_back(key_value); + + stat.level = diagnostic_msgs::msg::DiagnosticStatus::OK; + stat.message = "OK"; + if (!is_activated) { + stat.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + stat.message = "[WARN]process is not activated"; + } + + return stat; +} + +diagnostic_msgs::msg::DiagnosticStatus checkMeasurementUpdated( + const std::string & measurement_type, const size_t no_update_count, + const size_t no_update_count_threshold_warn, const size_t no_update_count_threshold_error) +{ + diagnostic_msgs::msg::DiagnosticStatus stat; + + diagnostic_msgs::msg::KeyValue key_value; + key_value.key = measurement_type + "_no_update_count"; + key_value.value = std::to_string(no_update_count); + stat.values.push_back(key_value); + key_value.key = measurement_type + "_no_update_count_threshold_warn"; + key_value.value = std::to_string(no_update_count_threshold_warn); + stat.values.push_back(key_value); + key_value.key = measurement_type + "_no_update_count_threshold_error"; + key_value.value = std::to_string(no_update_count_threshold_error); + stat.values.push_back(key_value); + + stat.level = diagnostic_msgs::msg::DiagnosticStatus::OK; + stat.message = "OK"; + if (no_update_count >= no_update_count_threshold_warn) { + stat.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + stat.message = "[WARN]" + measurement_type + " is not updated"; + } + if (no_update_count >= no_update_count_threshold_error) { + stat.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + stat.message = "[ERROR]" + measurement_type + " is not updated"; + } + + return stat; +} + +diagnostic_msgs::msg::DiagnosticStatus checkMeasurementQueueSize( + const std::string & measurement_type, const size_t queue_size) +{ + diagnostic_msgs::msg::DiagnosticStatus stat; + + diagnostic_msgs::msg::KeyValue key_value; + key_value.key = measurement_type + "_queue_size"; + key_value.value = std::to_string(queue_size); + stat.values.push_back(key_value); + + stat.level = diagnostic_msgs::msg::DiagnosticStatus::OK; + stat.message = "OK"; + + return stat; +} + +diagnostic_msgs::msg::DiagnosticStatus checkMeasurementDelayGate( + const std::string & measurement_type, const bool is_passed_delay_gate, const double delay_time, + const double delay_time_threshold) +{ + diagnostic_msgs::msg::DiagnosticStatus stat; + + diagnostic_msgs::msg::KeyValue key_value; + key_value.key = measurement_type + "_is_passed_delay_gate"; + key_value.value = is_passed_delay_gate ? "True" : "False"; + stat.values.push_back(key_value); + key_value.key = measurement_type + "_delay_time"; + key_value.value = std::to_string(delay_time); + stat.values.push_back(key_value); + key_value.key = measurement_type + "_delay_time_threshold"; + key_value.value = std::to_string(delay_time_threshold); + stat.values.push_back(key_value); + + stat.level = diagnostic_msgs::msg::DiagnosticStatus::OK; + stat.message = "OK"; + if (!is_passed_delay_gate) { + stat.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + stat.message = "[WARN]" + measurement_type + " topic is delay"; + } + + return stat; +} + +diagnostic_msgs::msg::DiagnosticStatus checkMeasurementMahalanobisGate( + const std::string & measurement_type, const bool is_passed_mahalanobis_gate, + const double mahalanobis_distance, const double mahalanobis_distance_threshold) +{ + diagnostic_msgs::msg::DiagnosticStatus stat; + + diagnostic_msgs::msg::KeyValue key_value; + key_value.key = measurement_type + "_is_passed_mahalanobis_gate"; + key_value.value = is_passed_mahalanobis_gate ? "True" : "False"; + stat.values.push_back(key_value); + key_value.key = measurement_type + "_mahalanobis_distance"; + key_value.value = std::to_string(mahalanobis_distance); + stat.values.push_back(key_value); + key_value.key = measurement_type + "_mahalanobis_distance_threshold"; + key_value.value = std::to_string(mahalanobis_distance_threshold); + stat.values.push_back(key_value); + + stat.level = diagnostic_msgs::msg::DiagnosticStatus::OK; + stat.message = "OK"; + if (!is_passed_mahalanobis_gate) { + stat.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + stat.message = "[WARN]mahalanobis distance of " + measurement_type + " topic is large"; + } + + return stat; +} + +// The highest level within the stat_array will be reflected in the merged_stat. +// When all stat_array entries are 'OK,' the message of merged_stat will be "OK" +diagnostic_msgs::msg::DiagnosticStatus mergeDiagnosticStatus( + const std::vector & stat_array) +{ + diagnostic_msgs::msg::DiagnosticStatus merged_stat; + + for (const auto & stat : stat_array) { + if ((stat.level > diagnostic_msgs::msg::DiagnosticStatus::OK)) { + if (!merged_stat.message.empty()) { + merged_stat.message += "; "; + } + merged_stat.message += stat.message; + } + if (stat.level > merged_stat.level) { + merged_stat.level = stat.level; + } + for (const auto & value : stat.values) { + merged_stat.values.push_back(value); + } + } + + if (merged_stat.level == diagnostic_msgs::msg::DiagnosticStatus::OK) { + merged_stat.message = "OK"; + } + + return merged_stat; +} diff --git a/localization/ekf_localizer/src/ekf_localizer.cpp b/localization/ekf_localizer/src/ekf_localizer.cpp index 0e46a26add852..b39112b1d8d62 100644 --- a/localization/ekf_localizer/src/ekf_localizer.cpp +++ b/localization/ekf_localizer/src/ekf_localizer.cpp @@ -15,6 +15,7 @@ #include "ekf_localizer/ekf_localizer.hpp" #include "ekf_localizer/covariance.hpp" +#include "ekf_localizer/diagnostics.hpp" #include "ekf_localizer/mahalanobis.hpp" #include "ekf_localizer/matrix_types.hpp" #include "ekf_localizer/measurement.hpp" @@ -87,6 +88,7 @@ EKFLocalizer::EKFLocalizer(const std::string & node_name, const rclcpp::NodeOpti pub_biased_pose_ = create_publisher("ekf_biased_pose", 1); pub_biased_pose_cov_ = create_publisher( "ekf_biased_pose_with_covariance", 1); + pub_diag_ = this->create_publisher("/diagnostics", 10); sub_initialpose_ = create_subscription( "initialpose", 1, std::bind(&EKFLocalizer::callbackInitialPose, this, _1)); sub_pose_with_cov_ = create_subscription( @@ -143,6 +145,7 @@ void EKFLocalizer::timerCallback() if (!is_activated_) { warning_.warnThrottle( "The node is not activated. Provide initial pose to pose_initializer", 2000); + publishDiagnostics(); return; } @@ -176,6 +179,16 @@ void EKFLocalizer::timerCallback() DEBUG_INFO(get_logger(), "------------------------- end prediction -------------------------\n"); /* pose measurement update */ + + pose_queue_size_ = pose_queue_.size(); + pose_is_passed_delay_gate_ = true; + pose_delay_time_ = 0.0; + pose_delay_time_threshold_ = 0.0; + pose_is_passed_mahalanobis_gate_ = true; + pose_mahalanobis_distance_ = 0.0; + + bool pose_is_updated = false; + if (!pose_queue_.empty()) { DEBUG_INFO(get_logger(), "------------------------- start Pose -------------------------"); stop_watch_.tic(); @@ -184,13 +197,27 @@ void EKFLocalizer::timerCallback() const size_t n = pose_queue_.size(); for (size_t i = 0; i < n; ++i) { const auto pose = pose_queue_.pop_increment_age(); - measurementUpdatePose(*pose); + bool is_updated = measurementUpdatePose(*pose); + if (is_updated) { + pose_is_updated = true; + } } DEBUG_INFO(get_logger(), "[EKF] measurementUpdatePose calc time = %f [ms]", stop_watch_.toc()); DEBUG_INFO(get_logger(), "------------------------- end Pose -------------------------\n"); } + pose_no_update_count_ = pose_is_updated ? 0 : (pose_no_update_count_ + 1); /* twist measurement update */ + + twist_queue_size_ = twist_queue_.size(); + twist_is_passed_delay_gate_ = true; + twist_delay_time_ = 0.0; + twist_delay_time_threshold_ = 0.0; + twist_is_passed_mahalanobis_gate_ = true; + twist_mahalanobis_distance_ = 0.0; + + bool twist_is_updated = false; + if (!twist_queue_.empty()) { DEBUG_INFO(get_logger(), "------------------------- start Twist -------------------------"); stop_watch_.tic(); @@ -199,11 +226,15 @@ void EKFLocalizer::timerCallback() const size_t n = twist_queue_.size(); for (size_t i = 0; i < n; ++i) { const auto twist = twist_queue_.pop_increment_age(); - measurementUpdateTwist(*twist); + bool is_updated = measurementUpdateTwist(*twist); + if (is_updated) { + twist_is_updated = true; + } } DEBUG_INFO(get_logger(), "[EKF] measurementUpdateTwist calc time = %f [ms]", stop_watch_.toc()); DEBUG_INFO(get_logger(), "------------------------- end Twist -------------------------\n"); } + twist_no_update_count_ = twist_is_updated ? 0 : (twist_no_update_count_ + 1); const double x = ekf_.getXelement(IDX::X); const double y = ekf_.getXelement(IDX::Y); @@ -235,6 +266,7 @@ void EKFLocalizer::timerCallback() /* publish ekf result */ publishEstimateResult(); + publishDiagnostics(); } void EKFLocalizer::showCurrentX() @@ -353,6 +385,11 @@ void EKFLocalizer::callbackPoseWithCovariance( void EKFLocalizer::callbackTwistWithCovariance( geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg) { + // Ignore twist if velocity is too small. + // Note that this inequality must not include "equal". + if (std::abs(msg->twist.twist.linear.x) < params_.threshold_observable_velocity_mps) { + msg->twist.covariance[0 * 6 + 0] = 10000.0; + } twist_queue_.push(msg); } @@ -376,7 +413,7 @@ void EKFLocalizer::initEKF() /* * measurementUpdatePose */ -void EKFLocalizer::measurementUpdatePose(const geometry_msgs::msg::PoseWithCovarianceStamped & pose) +bool EKFLocalizer::measurementUpdatePose(const geometry_msgs::msg::PoseWithCovarianceStamped & pose) { if (pose.header.frame_id != params_.pose_frame_id) { warning_.warnThrottle( @@ -400,10 +437,14 @@ void EKFLocalizer::measurementUpdatePose(const geometry_msgs::msg::PoseWithCovar delay_time = std::max(delay_time, 0.0); int delay_step = std::roundf(delay_time / ekf_dt_); + + pose_delay_time_ = std::max(delay_time, pose_delay_time_); + pose_delay_time_threshold_ = params_.extend_state_step * ekf_dt_; if (delay_step >= params_.extend_state_step) { + pose_is_passed_delay_gate_ = false; warning_.warnThrottle( poseDelayStepWarningMessage(delay_time, params_.extend_state_step, ekf_dt_), 2000); - return; + return false; } DEBUG_INFO(get_logger(), "delay_time: %f [s]", delay_time); @@ -420,7 +461,7 @@ void EKFLocalizer::measurementUpdatePose(const geometry_msgs::msg::PoseWithCovar if (hasNan(y) || hasInf(y)) { warning_.warn( "[EKF] pose measurement matrix includes NaN of Inf. ignore update. check pose message."); - return; + return false; } /* Gate */ @@ -431,10 +472,12 @@ void EKFLocalizer::measurementUpdatePose(const geometry_msgs::msg::PoseWithCovar const Eigen::MatrixXd P_y = P_curr.block(0, 0, dim_y, dim_y); const double distance = mahalanobis(y_ekf, y, P_y); + pose_mahalanobis_distance_ = std::max(distance, pose_mahalanobis_distance_); if (distance > params_.pose_gate_dist) { + pose_is_passed_mahalanobis_gate_ = false; warning_.warnThrottle(mahalanobisWarningMessage(distance, params_.pose_gate_dist), 2000); warning_.warnThrottle("Ignore the measurement data.", 2000); - return; + return false; } DEBUG_PRINT_MAT(y.transpose()); @@ -460,12 +503,14 @@ void EKFLocalizer::measurementUpdatePose(const geometry_msgs::msg::PoseWithCovar const Eigen::MatrixXd X_result = ekf_.getLatestX(); DEBUG_PRINT_MAT(X_result.transpose()); DEBUG_PRINT_MAT((X_result - X_curr).transpose()); + + return true; } /* * measurementUpdateTwist */ -void EKFLocalizer::measurementUpdateTwist( +bool EKFLocalizer::measurementUpdateTwist( const geometry_msgs::msg::TwistWithCovarianceStamped & twist) { if (twist.header.frame_id != "base_link") { @@ -488,10 +533,14 @@ void EKFLocalizer::measurementUpdateTwist( delay_time = std::max(delay_time, 0.0); int delay_step = std::roundf(delay_time / ekf_dt_); + + twist_delay_time_ = std::max(delay_time, twist_delay_time_); + twist_delay_time_threshold_ = params_.extend_state_step * ekf_dt_; if (delay_step >= params_.extend_state_step) { + twist_is_passed_delay_gate_ = false; warning_.warnThrottle( twistDelayStepWarningMessage(delay_time, params_.extend_state_step, ekf_dt_), 2000); - return; + return false; } DEBUG_INFO(get_logger(), "delay_time: %f [s]", delay_time); @@ -502,7 +551,7 @@ void EKFLocalizer::measurementUpdateTwist( if (hasNan(y) || hasInf(y)) { warning_.warn( "[EKF] twist measurement matrix includes NaN of Inf. ignore update. check twist message."); - return; + return false; } const Eigen::Vector2d y_ekf( @@ -512,10 +561,12 @@ void EKFLocalizer::measurementUpdateTwist( const Eigen::MatrixXd P_y = P_curr.block(4, 4, dim_y, dim_y); const double distance = mahalanobis(y_ekf, y, P_y); + twist_mahalanobis_distance_ = std::max(distance, twist_mahalanobis_distance_); if (distance > params_.twist_gate_dist) { + twist_is_passed_mahalanobis_gate_ = false; warning_.warnThrottle(mahalanobisWarningMessage(distance, params_.twist_gate_dist), 2000); warning_.warnThrottle("Ignore the measurement data.", 2000); - return; + return false; } DEBUG_PRINT_MAT(y.transpose()); @@ -532,6 +583,8 @@ void EKFLocalizer::measurementUpdateTwist( const Eigen::MatrixXd X_result = ekf_.getLatestX(); DEBUG_PRINT_MAT(X_result.transpose()); DEBUG_PRINT_MAT((X_result - X_curr).transpose()); + + return true; } /* @@ -607,6 +660,45 @@ void EKFLocalizer::publishEstimateResult() pub_debug_->publish(msg); } +void EKFLocalizer::publishDiagnostics() +{ + std::vector diag_status_array; + + diag_status_array.push_back(checkProcessActivated(is_activated_)); + + if (is_activated_) { + diag_status_array.push_back(checkMeasurementUpdated( + "pose", pose_no_update_count_, params_.pose_no_update_count_threshold_warn, + params_.pose_no_update_count_threshold_error)); + diag_status_array.push_back(checkMeasurementQueueSize("pose", pose_queue_size_)); + diag_status_array.push_back(checkMeasurementDelayGate( + "pose", pose_is_passed_delay_gate_, pose_delay_time_, pose_delay_time_threshold_)); + diag_status_array.push_back(checkMeasurementMahalanobisGate( + "pose", pose_is_passed_mahalanobis_gate_, pose_mahalanobis_distance_, + params_.pose_gate_dist)); + + diag_status_array.push_back(checkMeasurementUpdated( + "twist", twist_no_update_count_, params_.twist_no_update_count_threshold_warn, + params_.twist_no_update_count_threshold_error)); + diag_status_array.push_back(checkMeasurementQueueSize("twist", twist_queue_size_)); + diag_status_array.push_back(checkMeasurementDelayGate( + "twist", twist_is_passed_delay_gate_, twist_delay_time_, twist_delay_time_threshold_)); + diag_status_array.push_back(checkMeasurementMahalanobisGate( + "twist", twist_is_passed_mahalanobis_gate_, twist_mahalanobis_distance_, + params_.twist_gate_dist)); + } + + diagnostic_msgs::msg::DiagnosticStatus diag_merged_status; + diag_merged_status = mergeDiagnosticStatus(diag_status_array); + diag_merged_status.name = "localization: " + std::string(this->get_name()); + diag_merged_status.hardware_id = this->get_name(); + + diagnostic_msgs::msg::DiagnosticArray diag_msg; + diag_msg.header.stamp = this->now(); + diag_msg.status.push_back(diag_merged_status); + pub_diag_->publish(diag_msg); +} + void EKFLocalizer::updateSimple1DFilters( const geometry_msgs::msg::PoseWithCovarianceStamped & pose, const size_t smoothing_step) { diff --git a/localization/ekf_localizer/test/src/test_ekf_localizer.cpp b/localization/ekf_localizer/test/src/test_ekf_localizer.cpp deleted file mode 100644 index f249dffef6034..0000000000000 --- a/localization/ekf_localizer/test/src/test_ekf_localizer.cpp +++ /dev/null @@ -1,305 +0,0 @@ -// Copyright 2018-2019 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/ekf_localizer.hpp" - -#include -#include -#include - -#include - -#include -#include - -#include -#include -#include -#include - -using std::placeholders::_1; - -class EKFLocalizerTestSuite : public ::testing::Test -{ -protected: - void SetUp() { rclcpp::init(0, nullptr); } - void TearDown() { (void)rclcpp::shutdown(); } -}; // sanity_check - -class TestEKFLocalizerNode : public EKFLocalizer -{ -public: - TestEKFLocalizerNode(const std::string & node_name, const rclcpp::NodeOptions & node_options) - : EKFLocalizer(node_name, node_options) - { - sub_twist = this->create_subscription( - "/ekf_twist", 1, std::bind(&TestEKFLocalizerNode::testCallbackTwist, this, _1)); - sub_pose = this->create_subscription( - "/ekf_pose", 1, std::bind(&TestEKFLocalizerNode::testCallbackPose, this, _1)); - - using std::chrono_literals::operator""ms; - test_timer_ = rclcpp::create_timer( - this, get_clock(), 100ms, std::bind(&TestEKFLocalizerNode::testTimerCallback, this)); - } - ~TestEKFLocalizerNode() {} - - std::string frame_id_a_ = "world"; - std::string frame_id_b_ = "base_link"; - - rclcpp::Subscription::SharedPtr sub_twist; - rclcpp::Subscription::SharedPtr sub_pose; - - rclcpp::TimerBase::SharedPtr test_timer_; - - geometry_msgs::msg::PoseStamped::SharedPtr test_current_pose_ptr_; - geometry_msgs::msg::TwistStamped::SharedPtr test_current_twist_ptr_; - - void testTimerCallback() - { - /* !!! this should be defined before sendTransform() !!! */ - static std::shared_ptr br = - std::make_shared(shared_from_this()); - geometry_msgs::msg::TransformStamped sent; - - rclcpp::Time current_time = this->now(); - - sent.header.stamp = current_time; - sent.header.frame_id = frame_id_a_; - sent.child_frame_id = frame_id_b_; - sent.transform.translation.x = -7.11; - sent.transform.translation.y = 0.0; - sent.transform.translation.z = 0.0; - tf2::Quaternion q; - q.setRPY(0, 0, 0.5); - sent.transform.rotation.x = q.x(); - sent.transform.rotation.y = q.y(); - sent.transform.rotation.z = q.z(); - sent.transform.rotation.w = q.w(); - - br->sendTransform(sent); - } - - void testCallbackPose(geometry_msgs::msg::PoseStamped::SharedPtr pose) - { - test_current_pose_ptr_ = std::make_shared(*pose); - } - - void testCallbackTwist(geometry_msgs::msg::TwistStamped::SharedPtr twist) - { - test_current_twist_ptr_ = std::make_shared(*twist); - } - - void resetCurrentPoseAndTwist() - { - test_current_pose_ptr_ = nullptr; - test_current_twist_ptr_ = nullptr; - } -}; - -TEST_F(EKFLocalizerTestSuite, measurementUpdatePose) -{ - rclcpp::NodeOptions node_options; - auto ekf = std::make_shared("EKFLocalizerTestSuite", node_options); - - auto pub_pose = ekf->create_publisher("/in_pose", 1); - - geometry_msgs::msg::PoseStamped in_pose; - in_pose.header.frame_id = "world"; - in_pose.pose.position.x = 1.0; - in_pose.pose.position.y = 2.0; - in_pose.pose.position.z = 3.0; - in_pose.pose.orientation.x = 0.0; - in_pose.pose.orientation.y = 0.0; - in_pose.pose.orientation.z = 0.0; - in_pose.pose.orientation.w = 1.0; - - /* test for valid value */ - const double pos_x = 12.3; - in_pose.pose.position.x = pos_x; // for valid value - - for (int i = 0; i < 20; ++i) { - in_pose.header.stamp = ekf->now(); - pub_pose->publish(in_pose); - rclcpp::spin_some(ekf); - rclcpp::sleep_for(std::chrono::milliseconds(100)); - } - - ASSERT_NE(ekf->test_current_pose_ptr_, nullptr); - ASSERT_NE(ekf->test_current_twist_ptr_, nullptr); - - double ekf_x = ekf->test_current_pose_ptr_->pose.position.x; - bool is_succeeded = !(std::isnan(ekf_x) || std::isinf(ekf_x)); - ASSERT_EQ(true, is_succeeded) << "ekf result includes invalid value."; - - ASSERT_TRUE(std::fabs(ekf_x - pos_x) < 0.1) - << "ekf pos x: " << ekf_x << " should be close to " << pos_x; - - /* test for invalid value */ - in_pose.pose.position.x = NAN; // check for invalid values - for (int i = 0; i < 10; ++i) { - in_pose.header.stamp = ekf->now(); - pub_pose->publish(in_pose); - rclcpp::spin_some(ekf); - rclcpp::sleep_for(std::chrono::milliseconds(100)); - } - is_succeeded = !(std::isnan(ekf_x) || std::isinf(ekf_x)); - ASSERT_EQ(true, is_succeeded) << "ekf result includes invalid value."; - - ekf->resetCurrentPoseAndTwist(); -} - -TEST_F(EKFLocalizerTestSuite, measurementUpdateTwist) -{ - rclcpp::NodeOptions node_options; - auto ekf = std::make_shared("EKFLocalizerTestSuite", node_options); - - auto pub_twist = ekf->create_publisher("/in_twist", 1); - geometry_msgs::msg::TwistStamped in_twist; - in_twist.header.frame_id = "base_link"; - - /* test for valid value */ - const double vx = 12.3; - in_twist.twist.linear.x = vx; // for valid value - for (int i = 0; i < 20; ++i) { - in_twist.header.stamp = ekf->now(); - pub_twist->publish(in_twist); - rclcpp::spin_some(ekf); - rclcpp::sleep_for(std::chrono::milliseconds(100)); - } - - ASSERT_FALSE(ekf->test_current_pose_ptr_ == nullptr); - ASSERT_FALSE(ekf->test_current_twist_ptr_ == nullptr); - - double ekf_vx = ekf->test_current_twist_ptr_->twist.linear.x; - bool is_succeeded = !(std::isnan(ekf_vx) || std::isinf(ekf_vx)); - ASSERT_EQ(true, is_succeeded) << "ekf result includes invalid value."; - ASSERT_TRUE(std::fabs(ekf_vx - vx) < 0.1) - << "ekf vel x: " << ekf_vx << ", should be close to " << vx; - - /* test for invalid value */ - in_twist.twist.linear.x = NAN; // check for invalid values - for (int i = 0; i < 10; ++i) { - in_twist.header.stamp = ekf->now(); - pub_twist->publish(in_twist); - rclcpp::spin_some(ekf); - rclcpp::sleep_for(std::chrono::milliseconds(100)); - } - - ekf_vx = ekf->test_current_twist_ptr_->twist.linear.x; - is_succeeded = !(std::isnan(ekf_vx) || std::isinf(ekf_vx)); - ASSERT_EQ(true, is_succeeded) << "ekf result includes invalid value."; - - ekf->resetCurrentPoseAndTwist(); -} - -TEST_F(EKFLocalizerTestSuite, measurementUpdatePoseWithCovariance) -{ - rclcpp::NodeOptions node_options; - node_options.append_parameter_override("use_pose_with_covariance", true); - rclcpp::sleep_for(std::chrono::milliseconds(200)); - auto ekf = std::make_shared("EKFLocalizerTestSuite", node_options); - - auto pub_pose = ekf->create_publisher( - "/in_pose_with_covariance", 1); - geometry_msgs::msg::PoseWithCovarianceStamped in_pose; - in_pose.header.frame_id = "world"; - in_pose.pose.pose.position.x = 1.0; - in_pose.pose.pose.position.y = 2.0; - in_pose.pose.pose.position.z = 3.0; - in_pose.pose.pose.orientation.x = 0.0; - in_pose.pose.pose.orientation.y = 0.0; - in_pose.pose.pose.orientation.z = 0.0; - in_pose.pose.pose.orientation.w = 1.0; - for (int i = 0; i < 36; ++i) { - in_pose.pose.covariance[i] = 0.1; - } - - /* test for valid value */ - const double pos_x = 99.3; - in_pose.pose.pose.position.x = pos_x; // for valid value - - for (int i = 0; i < 20; ++i) { - in_pose.header.stamp = ekf->now(); - pub_pose->publish(in_pose); - rclcpp::spin_some(ekf); - rclcpp::sleep_for(std::chrono::milliseconds(100)); - } - - ASSERT_FALSE(ekf->test_current_pose_ptr_ == nullptr); - ASSERT_FALSE(ekf->test_current_twist_ptr_ == nullptr); - - double ekf_x = ekf->test_current_pose_ptr_->pose.position.x; - bool is_succeeded = !(std::isnan(ekf_x) || std::isinf(ekf_x)); - ASSERT_EQ(true, is_succeeded) << "ekf result includes invalid value."; - ASSERT_TRUE(std::fabs(ekf_x - pos_x) < 0.1) - << "ekf pos x: " << ekf_x << " should be close to " << pos_x; - - /* test for invalid value */ - in_pose.pose.pose.position.x = NAN; // check for invalid values - for (int i = 0; i < 10; ++i) { - in_pose.header.stamp = ekf->now(); - pub_pose->publish(in_pose); - rclcpp::spin_some(ekf); - rclcpp::sleep_for(std::chrono::milliseconds(100)); - } - is_succeeded = !(std::isnan(ekf_x) || std::isinf(ekf_x)); - ASSERT_EQ(true, is_succeeded) << "ekf result includes invalid value."; - - ekf->resetCurrentPoseAndTwist(); -} - -TEST_F(EKFLocalizerTestSuite, measurementUpdateTwistWithCovariance) -{ - rclcpp::NodeOptions node_options; - auto ekf = std::make_shared("EKFLocalizerTestSuite", node_options); - - auto pub_twist = ekf->create_publisher( - "/in_twist_with_covariance", 1); - geometry_msgs::msg::TwistWithCovarianceStamped in_twist; - in_twist.header.frame_id = "base_link"; - - /* test for valid value */ - const double vx = 12.3; - in_twist.twist.twist.linear.x = vx; // for valid value - for (int i = 0; i < 36; ++i) { - in_twist.twist.covariance[i] = 0.1; - } - for (int i = 0; i < 10; ++i) { - in_twist.header.stamp = ekf->now(); - pub_twist->publish(in_twist); - rclcpp::spin_some(ekf); - rclcpp::sleep_for(std::chrono::milliseconds(100)); - } - - ASSERT_FALSE(ekf->test_current_pose_ptr_ == nullptr); - ASSERT_FALSE(ekf->test_current_twist_ptr_ == nullptr); - - double ekf_vx = ekf->test_current_twist_ptr_->twist.linear.x; - bool is_succeeded = !(std::isnan(ekf_vx) || std::isinf(ekf_vx)); - ASSERT_EQ(true, is_succeeded) << "ekf result includes invalid value."; - ASSERT_TRUE((ekf_vx - vx) < 0.1) << "vel x should be close to " << vx; - - /* test for invalid value */ - in_twist.twist.twist.linear.x = NAN; // check for invalid values - for (int i = 0; i < 10; ++i) { - in_twist.header.stamp = ekf->now(); - pub_twist->publish(in_twist); - rclcpp::spin_some(ekf); - rclcpp::sleep_for(std::chrono::milliseconds(100)); - } - - ekf_vx = ekf->test_current_twist_ptr_->twist.linear.x; - is_succeeded = !(std::isnan(ekf_vx) || std::isinf(ekf_vx)); - ASSERT_EQ(true, is_succeeded) << "ekf result includes invalid value."; -} diff --git a/localization/ekf_localizer/test/test_covariance.cpp b/localization/ekf_localizer/test/test_covariance.cpp index fdc732ea6d34c..23458fb18a838 100644 --- a/localization/ekf_localizer/test/test_covariance.cpp +++ b/localization/ekf_localizer/test/test_covariance.cpp @@ -31,15 +31,15 @@ TEST(EKFCovarianceToPoseMessageCovariance, SmokeTest) P(2, 2) = 9.; std::array covariance = ekfCovarianceToPoseMessageCovariance(P); - EXPECT_EQ(covariance(0), 1.); - EXPECT_EQ(covariance(1), 2.); - EXPECT_EQ(covariance(5), 3.); - EXPECT_EQ(covariance(6), 4.); - EXPECT_EQ(covariance(7), 5.); - EXPECT_EQ(covariance(11), 6.); - EXPECT_EQ(covariance(30), 7.); - EXPECT_EQ(covariance(31), 8.); - EXPECT_EQ(covariance(35), 9.); + EXPECT_EQ(covariance[0], 1.); + EXPECT_EQ(covariance[1], 2.); + EXPECT_EQ(covariance[5], 3.); + EXPECT_EQ(covariance[6], 4.); + EXPECT_EQ(covariance[7], 5.); + EXPECT_EQ(covariance[11], 6.); + EXPECT_EQ(covariance[30], 7.); + EXPECT_EQ(covariance[31], 8.); + EXPECT_EQ(covariance[35], 9.); } // ensure other elements are zero @@ -62,10 +62,10 @@ TEST(EKFCovarianceToTwistMessageCovariance, SmokeTest) P(5, 5) = 4.; std::array covariance = ekfCovarianceToTwistMessageCovariance(P); - EXPECT_EQ(covariance(0), 1.); - EXPECT_EQ(covariance(5), 2.); - EXPECT_EQ(covariance(30), 3.); - EXPECT_EQ(covariance(35), 4.); + EXPECT_EQ(covariance[0], 1.); + EXPECT_EQ(covariance[5], 2.); + EXPECT_EQ(covariance[30], 3.); + EXPECT_EQ(covariance[35], 4.); } // ensure other elements are zero diff --git a/localization/ekf_localizer/test/test_diagnostics.cpp b/localization/ekf_localizer/test/test_diagnostics.cpp new file mode 100644 index 0000000000000..f506dca1cb230 --- /dev/null +++ b/localization/ekf_localizer/test/test_diagnostics.cpp @@ -0,0 +1,192 @@ +// Copyright 2023 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/diagnostics.hpp" + +#include + +TEST(TestEkfDiagnostics, CheckProcessActivated) +{ + diagnostic_msgs::msg::DiagnosticStatus stat; + + bool is_activated = true; + stat = checkProcessActivated(is_activated); + EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK); + + is_activated = false; + stat = checkProcessActivated(is_activated); + EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN); +} + +TEST(TestEkfDiagnostics, checkMeasurementUpdated) +{ + diagnostic_msgs::msg::DiagnosticStatus stat; + + const std::string measurement_type = "pose"; // not effect for stat.level + const size_t no_update_count_threshold_warn = 50; + const size_t no_update_count_threshold_error = 250; + + size_t no_update_count = 0; + stat = checkMeasurementUpdated( + measurement_type, no_update_count, no_update_count_threshold_warn, + no_update_count_threshold_error); + EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK); + + no_update_count = 1; + stat = checkMeasurementUpdated( + measurement_type, no_update_count, no_update_count_threshold_warn, + no_update_count_threshold_error); + EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK); + + no_update_count = 49; + stat = checkMeasurementUpdated( + measurement_type, no_update_count, no_update_count_threshold_warn, + no_update_count_threshold_error); + EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK); + + no_update_count = 50; + stat = checkMeasurementUpdated( + measurement_type, no_update_count, no_update_count_threshold_warn, + no_update_count_threshold_error); + EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN); + + no_update_count = 249; + stat = checkMeasurementUpdated( + measurement_type, no_update_count, no_update_count_threshold_warn, + no_update_count_threshold_error); + EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN); + + no_update_count = 250; + stat = checkMeasurementUpdated( + measurement_type, no_update_count, no_update_count_threshold_warn, + no_update_count_threshold_error); + EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::ERROR); +} + +TEST(TestEkfDiagnostics, CheckMeasurementQueueSize) +{ + diagnostic_msgs::msg::DiagnosticStatus stat; + + const std::string measurement_type = "pose"; // not effect for stat.level + + size_t queue_size = 0; // not effect for stat.level + stat = checkMeasurementQueueSize(measurement_type, queue_size); + EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK); + + queue_size = 1; // not effect for stat.level + stat = checkMeasurementQueueSize(measurement_type, queue_size); + EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK); +} + +TEST(TestEkfDiagnostics, CheckMeasurementDelayGate) +{ + diagnostic_msgs::msg::DiagnosticStatus stat; + + const std::string measurement_type = "pose"; // not effect for stat.level + const double delay_time = 0.1; // not effect for stat.level + const double delay_time_threshold = 1.0; // not effect for stat.level + + bool is_passed_delay_gate = true; + stat = checkMeasurementDelayGate( + measurement_type, is_passed_delay_gate, delay_time, delay_time_threshold); + EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK); + + is_passed_delay_gate = false; + stat = checkMeasurementDelayGate( + measurement_type, is_passed_delay_gate, delay_time, delay_time_threshold); + EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN); +} + +TEST(TestEkfDiagnostics, CheckMeasurementMahalanobisGate) +{ + diagnostic_msgs::msg::DiagnosticStatus stat; + + const std::string measurement_type = "pose"; // not effect for stat.level + const double mahalanobis_distance = 0.1; // not effect for stat.level + const double mahalanobis_distance_threshold = 1.0; // not effect for stat.level + + bool is_passed_mahalanobis_gate = true; + stat = checkMeasurementMahalanobisGate( + measurement_type, is_passed_mahalanobis_gate, mahalanobis_distance, + mahalanobis_distance_threshold); + EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK); + + is_passed_mahalanobis_gate = false; + stat = checkMeasurementMahalanobisGate( + measurement_type, is_passed_mahalanobis_gate, mahalanobis_distance, + mahalanobis_distance_threshold); + EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN); +} + +TEST(TestLocalizationErrorMonitorDiagnostics, MergeDiagnosticStatus) +{ + diagnostic_msgs::msg::DiagnosticStatus merged_stat; + std::vector stat_array(2); + + stat_array.at(0).level = diagnostic_msgs::msg::DiagnosticStatus::OK; + stat_array.at(0).message = "OK"; + stat_array.at(1).level = diagnostic_msgs::msg::DiagnosticStatus::OK; + stat_array.at(1).message = "OK"; + merged_stat = mergeDiagnosticStatus(stat_array); + EXPECT_EQ(merged_stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK); + EXPECT_EQ(merged_stat.message, "OK"); + + stat_array.at(0).level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + stat_array.at(0).message = "WARN0"; + stat_array.at(1).level = diagnostic_msgs::msg::DiagnosticStatus::OK; + stat_array.at(1).message = "OK"; + merged_stat = mergeDiagnosticStatus(stat_array); + EXPECT_EQ(merged_stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN); + EXPECT_EQ(merged_stat.message, "WARN0"); + + stat_array.at(0).level = diagnostic_msgs::msg::DiagnosticStatus::OK; + stat_array.at(0).message = "OK"; + stat_array.at(1).level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + stat_array.at(1).message = "WARN1"; + merged_stat = mergeDiagnosticStatus(stat_array); + EXPECT_EQ(merged_stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN); + EXPECT_EQ(merged_stat.message, "WARN1"); + + stat_array.at(0).level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + stat_array.at(0).message = "WARN0"; + stat_array.at(1).level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + stat_array.at(1).message = "WARN1"; + merged_stat = mergeDiagnosticStatus(stat_array); + EXPECT_EQ(merged_stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN); + EXPECT_EQ(merged_stat.message, "WARN0; WARN1"); + + stat_array.at(0).level = diagnostic_msgs::msg::DiagnosticStatus::OK; + stat_array.at(0).message = "OK"; + stat_array.at(1).level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + stat_array.at(1).message = "ERROR1"; + merged_stat = mergeDiagnosticStatus(stat_array); + EXPECT_EQ(merged_stat.level, diagnostic_msgs::msg::DiagnosticStatus::ERROR); + EXPECT_EQ(merged_stat.message, "ERROR1"); + + stat_array.at(0).level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + stat_array.at(0).message = "WARN0"; + stat_array.at(1).level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + stat_array.at(1).message = "ERROR1"; + merged_stat = mergeDiagnosticStatus(stat_array); + EXPECT_EQ(merged_stat.level, diagnostic_msgs::msg::DiagnosticStatus::ERROR); + EXPECT_EQ(merged_stat.message, "WARN0; ERROR1"); + + stat_array.at(0).level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + stat_array.at(0).message = "ERROR0"; + stat_array.at(1).level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + stat_array.at(1).message = "ERROR1"; + merged_stat = mergeDiagnosticStatus(stat_array); + EXPECT_EQ(merged_stat.level, diagnostic_msgs::msg::DiagnosticStatus::ERROR); + EXPECT_EQ(merged_stat.message, "ERROR0; ERROR1"); +} diff --git a/localization/ekf_localizer/test/test_ekf_localizer.test b/localization/ekf_localizer/test/test_ekf_localizer.test deleted file mode 100644 index 4eecfa6e094b7..0000000000000 --- a/localization/ekf_localizer/test/test_ekf_localizer.test +++ /dev/null @@ -1,5 +0,0 @@ - - - - - diff --git a/localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp b/localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp index b0c7a5c4471c6..97d4a02472f2d 100644 --- a/localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp +++ b/localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp @@ -15,8 +15,8 @@ #ifndef GYRO_ODOMETER__GYRO_ODOMETER_CORE_HPP_ #define GYRO_ODOMETER__GYRO_ODOMETER_CORE_HPP_ +#include "tier4_autoware_utils/ros/msg_covariance.hpp" #include "tier4_autoware_utils/ros/transform_listener.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" #include diff --git a/localization/gyro_odometer/package.xml b/localization/gyro_odometer/package.xml index cbcaadebddc2e..ad4a865a2014a 100644 --- a/localization/gyro_odometer/package.xml +++ b/localization/gyro_odometer/package.xml @@ -15,10 +15,8 @@ fmt geometry_msgs sensor_msgs - std_msgs tf2 tf2_geometry_msgs - tf2_ros tier4_autoware_utils rclcpp diff --git a/localization/landmark_based_localizer/README.md b/localization/landmark_based_localizer/README.md new file mode 100644 index 0000000000000..a8ebdc0c8f1e5 --- /dev/null +++ b/localization/landmark_based_localizer/README.md @@ -0,0 +1,123 @@ +# Landmark Based Localizer + +This directory contains packages for landmark-based localization. + +Landmarks are, for example + +- AR tags detected by camera +- Boards characterized by intensity detected by LiDAR + +etc. + +Since these landmarks are easy to detect and estimate pose, the ego pose can be calculated from the pose of the detected landmark if the pose of the landmark is written on the map in advance. + +Currently, landmarks are assumed to be flat. + +The following figure shows the principle of localization in the case of `ar_tag_based_localizer`. + +![principle](./doc_image/principle.png) + +This calculated ego pose is passed to the EKF, where it is fused with the twist information and used to estimate a more accurate ego pose. + +## Node diagram + +![node diagram](./doc_image/node_diagram.drawio.svg) + +### `landmark_tf_caster` node + +The definitions of the landmarks written to the map are introduced in the next section. See `Map Specifications`. + +The `landmark_tf_caster` node publishes the TF from the map to the landmark. + +- Translation : The center of the four vertices of the landmark +- Rotation : Let the vertex numbers be 1, 2, 3, 4 counterclockwise as shown in the next section. Direction is defined as the cross product of the vector from 1 to 2 and the vector from 2 to 3. + +Users can define landmarks as Lanelet2 4-vertex polygons. +In this case, it is possible to define an arrangement in which the four vertices cannot be considered to be on the same plane. The direction of the landmark in that case is difficult to calculate. +So, if the 4 vertices are considered as forming a tetrahedron and its volume exceeds the `volume_threshold` parameter, the landmark will not publish tf_static. + +### Landmark based localizer packages + +- ar_tag_based_localizer +- etc. + +## Inputs / Outputs + +### `landmark_tf_caster` node + +#### Input + +| Name | Type | Description | +| :--------------------- | :------------------------------------------- | :--------------- | +| `~/input/lanelet2_map` | `autoware_auto_mapping_msgs::msg::HADMapBin` | Data of lanelet2 | + +#### Output + +| Name | Type | Description | +| :---------- | :------------------------------------- | :----------------- | +| `tf_static` | `geometry_msgs::msg::TransformStamped` | TF from map to tag | + +## Map specifications + +For this package to work correctly, the poses of the landmarks must be specified in the Lanelet2 map format that `map_loader` and `landmark_tf_caster` can interpret. + +The four vertices of a landmark are defined counterclockwise. + +The order of the four vertices is defined as follows. In the coordinate system of a landmark, + +- the x-axis is parallel to the vector from the first vertex to the second vertex +- the y-axis is parallel to the vector from the second vertex to the third vertex + +![lanelet2 data structure](./doc_image/lanelet2_data_structure.drawio.svg) + +### Example of `lanelet2_map.osm` + +The values provided below are placeholders. +Ensure to input the correct coordinates corresponding to the actual location where the landmark is placed, such as `lat`, `lon`, `mgrs_code`, `local_x`, `local_y`. + +For example, when using the AR tag, it would look like this. + +```xml +... + + + + + + + + + + + + + + + + + + + + + + + + + + +... + + + + + + + + + + + + +... + +``` diff --git a/localization/ar_tag_based_localizer/CMakeLists.txt b/localization/landmark_based_localizer/ar_tag_based_localizer/CMakeLists.txt similarity index 74% rename from localization/ar_tag_based_localizer/CMakeLists.txt rename to localization/landmark_based_localizer/ar_tag_based_localizer/CMakeLists.txt index 89337b9bb966c..15aecd1f8ded9 100644 --- a/localization/ar_tag_based_localizer/CMakeLists.txt +++ b/localization/landmark_based_localizer/ar_tag_based_localizer/CMakeLists.txt @@ -14,13 +14,9 @@ ament_auto_find_build_dependencies() find_package(OpenCV REQUIRED) -# -# ar_tag_based_localizer -# ament_auto_add_executable(ar_tag_based_localizer src/ar_tag_based_localizer_node.cpp src/ar_tag_based_localizer_core.cpp - src/utils.cpp ) target_include_directories(ar_tag_based_localizer SYSTEM PUBLIC @@ -28,18 +24,6 @@ target_include_directories(ar_tag_based_localizer ) target_link_libraries(ar_tag_based_localizer ${OpenCV_LIBRARIES}) -# -# tag_tf_caster -# -ament_auto_add_executable(tag_tf_caster - src/tag_tf_caster_node.cpp - src/tag_tf_caster_core.cpp -) -target_include_directories(tag_tf_caster - SYSTEM PUBLIC - ${OpenCV_INCLUDE_DIRS} -) - ament_auto_package( INSTALL_TO_SHARE launch diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/README.md b/localization/landmark_based_localizer/ar_tag_based_localizer/README.md new file mode 100644 index 0000000000000..b212711f38a57 --- /dev/null +++ b/localization/landmark_based_localizer/ar_tag_based_localizer/README.md @@ -0,0 +1,74 @@ +# AR Tag Based Localizer + +**ArTagBasedLocalizer** is a vision-based localization node. + + + +This node uses [the ArUco library](https://index.ros.org/p/aruco/) to detect AR-Tags from camera images and calculates and publishes the pose of the ego vehicle based on these detections. +The positions and orientations of the AR-Tags are assumed to be written in the Lanelet2 format. + +## Inputs / Outputs + +### `ar_tag_based_localizer` node + +#### Input + +| Name | Type | Description | +| :-------------------- | :---------------------------------------------- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------ | +| `~/input/image` | `sensor_msgs::msg::Image` | Camera Image | +| `~/input/camera_info` | `sensor_msgs::msg::CameraInfo` | Camera Info | +| `~/input/ekf_pose` | `geometry_msgs::msg::PoseWithCovarianceStamped` | EKF Pose without IMU correction. It is used to validate detected AR tags by filtering out False Positives. Only if the EKF Pose and the AR tag-detected Pose are within a certain temporal and spatial range, the AR tag-detected Pose is considered valid and published. | + +#### Output + +| Name | Type | Description | +| :------------------------------ | :---------------------------------------------- | :---------------------------------------------------------------------------------------- | +| `~/output/pose_with_covariance` | `geometry_msgs::msg::PoseWithCovarianceStamped` | Estimated Pose | +| `~/debug/result` | `sensor_msgs::msg::Image` | [debug topic] Image in which marker detection results are superimposed on the input image | +| `/tf` | `geometry_msgs::msg::TransformStamped` | [debug topic] TF from camera to detected tag | +| `/diagnostics` | `diagnostic_msgs::msg::DiagnosticArray` | Diagnostics outputs | + +## How to launch + +When launching Autoware, set `artag` for `pose_source`. + +```bash +ros2 launch autoware_launch ... \ + pose_source:=artag \ + ... +``` + +### Rosbag + +#### [Sample rosbag and map (AWSIM data)](https://drive.google.com/file/d/1uMVwQQFcfs8JOqfoA1FqfH_fLPwQ71jK/view) + +This data is simulated data created by [AWSIM](https://tier4.github.io/AWSIM/). +Essentially, AR tag-based self-localization is not intended for such public road driving, but for driving in a smaller area, so the max driving speed is set at 15 km/h. + +It is a known problem that the timing of when each AR tag begins to be detected can cause significant changes in estimation. + +![sample_result_in_awsim](./doc_image/sample_result_in_awsim.png) + +#### [Sample rosbag and map (Real world data)](https://drive.google.com/file/d/1wiCQjyjRnYbb0dg8G6mRecdSGh8tv3zR/view) + +Please remap the topic names and play it. + +```bash +ros2 bag play /path/to/ar_tag_based_localizer_sample_bag/ -r 0.5 -s sqlite3 \ + --remap /sensing/camera/front/image:=/sensing/camera/traffic_light/image_raw \ + /sensing/camera/front/image/info:=/sensing/camera/traffic_light/camera_info +``` + +This dataset contains issues such as missing IMU data, and overall the accuracy is low. Even when running AR tag-based self-localization, significant difference from the true trajectory can be observed. + +The image below shows the trajectory when the sample is executed and plotted. + +![sample_result](./doc_image/sample_result.png) + +The pull request video below should also be helpful. + + + +## Principle + +![principle](../doc_image/principle.png) diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/config/ar_tag_based_localizer.param.yaml b/localization/landmark_based_localizer/ar_tag_based_localizer/config/ar_tag_based_localizer.param.yaml new file mode 100644 index 0000000000000..29260e27cd43c --- /dev/null +++ b/localization/landmark_based_localizer/ar_tag_based_localizer/config/ar_tag_based_localizer.param.yaml @@ -0,0 +1,37 @@ +/**: + ros__parameters: + # marker_size + marker_size: 0.6 + + # target_tag_ids + target_tag_ids: ['0','1','2','3','4','5','6'] + + # base_covariance + # This value is dynamically scaled according to the distance at which AR tags are detected. + base_covariance: [0.2, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.2, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.2, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.02, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.02, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.02] + + # Detect AR-Tags within this range and publish the pose of ego vehicle + distance_threshold: 13.0 # [m] + + # Detector parameters + # See https://github.com/pal-robotics/aruco_ros/blob/7787a6794d30c248bc546d1582e65dd47bc40c12/aruco/include/aruco/markerdetector.h#L106-L126 + detection_mode: "DM_NORMAL" # select from [DM_NORMAL, DM_FAST, DM_VIDEO_FAST] + min_marker_size: 0.02 + + # Parameters for comparison with EKF Pose + # If the difference between the EKF pose and the current pose is within the range of values set below, the current pose is published. + # [How to determine the value] + # * ekf_time_tolerance: Since it is abnormal if the data comes too old from EKF, the tentative tolerance value is set at 5 seconds. + # This value is assumed to be unaffected even if it is increased or decreased by some amount. + # * ekf_position_tolerance: Since it is possible that multiple AR tags with the same ID could be placed, the tolerance should be as small as possible. + # And if the vehicle is running only on odometry in a section without AR tags, + # it is possible that self-position estimation could be off by a few meters. + # it should be fixed by AR tag detection, so tolerance should not be smaller than 10 meters. + # Therefore, the tolerance is set at 10 meters. + ekf_time_tolerance: 5.0 # [s] + ekf_position_tolerance: 10.0 # [m] diff --git a/localization/ar_tag_based_localizer/doc_image/ar_tag_image.png b/localization/landmark_based_localizer/ar_tag_based_localizer/doc_image/ar_tag_image.png similarity index 100% rename from localization/ar_tag_based_localizer/doc_image/ar_tag_image.png rename to localization/landmark_based_localizer/ar_tag_based_localizer/doc_image/ar_tag_image.png diff --git a/localization/ar_tag_based_localizer/doc_image/sample_result.png b/localization/landmark_based_localizer/ar_tag_based_localizer/doc_image/sample_result.png similarity index 100% rename from localization/ar_tag_based_localizer/doc_image/sample_result.png rename to localization/landmark_based_localizer/ar_tag_based_localizer/doc_image/sample_result.png diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/doc_image/sample_result_in_awsim.png b/localization/landmark_based_localizer/ar_tag_based_localizer/doc_image/sample_result_in_awsim.png new file mode 100644 index 0000000000000..875005214bf5e Binary files /dev/null and b/localization/landmark_based_localizer/ar_tag_based_localizer/doc_image/sample_result_in_awsim.png differ diff --git a/localization/ar_tag_based_localizer/include/ar_tag_based_localizer/ar_tag_based_localizer_core.hpp b/localization/landmark_based_localizer/ar_tag_based_localizer/include/ar_tag_based_localizer/ar_tag_based_localizer_core.hpp similarity index 83% rename from localization/ar_tag_based_localizer/include/ar_tag_based_localizer/ar_tag_based_localizer_core.hpp rename to localization/landmark_based_localizer/ar_tag_based_localizer/include/ar_tag_based_localizer/ar_tag_based_localizer_core.hpp index 39373d92add25..37725dd06c34e 100644 --- a/localization/ar_tag_based_localizer/include/ar_tag_based_localizer/ar_tag_based_localizer_core.hpp +++ b/localization/landmark_based_localizer/ar_tag_based_localizer/include/ar_tag_based_localizer/ar_tag_based_localizer_core.hpp @@ -48,6 +48,7 @@ #include #include +#include #include #include @@ -69,14 +70,18 @@ class ArTagBasedLocalizer : public rclcpp::Node private: void image_callback(const sensor_msgs::msg::Image::ConstSharedPtr & msg); void cam_info_callback(const sensor_msgs::msg::CameraInfo & msg); - void publish_pose_as_base_link(const geometry_msgs::msg::PoseStamped & msg); + void ekf_pose_callback(const geometry_msgs::msg::PoseWithCovarianceStamped & msg); + void publish_pose_as_base_link( + const geometry_msgs::msg::PoseStamped & msg, const std::string & camera_frame_id); + static tf2::Transform aruco_marker_to_tf2(const aruco::Marker & marker); // Parameters float marker_size_{}; std::vector target_tag_ids_; - std::vector covariance_; + std::vector base_covariance_; double distance_threshold_squared_{}; - std::string camera_frame_; + double ekf_time_tolerance_{}; + double ekf_position_tolerance_{}; // tf std::unique_ptr tf_buffer_; @@ -87,17 +92,20 @@ class ArTagBasedLocalizer : public rclcpp::Node std::unique_ptr it_; // Subscribers - image_transport::Subscriber image_sub_; + rclcpp::Subscription::SharedPtr image_sub_; rclcpp::Subscription::SharedPtr cam_info_sub_; + rclcpp::Subscription::SharedPtr ekf_pose_sub_; // Publishers image_transport::Publisher image_pub_; rclcpp::Publisher::SharedPtr pose_pub_; + rclcpp::Publisher::SharedPtr diag_pub_; // Others aruco::MarkerDetector detector_; aruco::CameraParameters cam_param_; bool cam_info_received_; + geometry_msgs::msg::PoseWithCovarianceStamped latest_ekf_pose_{}; }; #endif // AR_TAG_BASED_LOCALIZER__AR_TAG_BASED_LOCALIZER_CORE_HPP_ diff --git a/localization/ar_tag_based_localizer/launch/ar_tag_based_localizer.launch.xml b/localization/landmark_based_localizer/ar_tag_based_localizer/launch/ar_tag_based_localizer.launch.xml similarity index 88% rename from localization/ar_tag_based_localizer/launch/ar_tag_based_localizer.launch.xml rename to localization/landmark_based_localizer/ar_tag_based_localizer/launch/ar_tag_based_localizer.launch.xml index 940f6ed53d8b8..f6c10050b1826 100644 --- a/localization/ar_tag_based_localizer/launch/ar_tag_based_localizer.launch.xml +++ b/localization/landmark_based_localizer/ar_tag_based_localizer/launch/ar_tag_based_localizer.launch.xml @@ -5,6 +5,7 @@ + @@ -12,6 +13,7 @@ + diff --git a/localization/ar_tag_based_localizer/package.xml b/localization/landmark_based_localizer/ar_tag_based_localizer/package.xml similarity index 86% rename from localization/ar_tag_based_localizer/package.xml rename to localization/landmark_based_localizer/ar_tag_based_localizer/package.xml index 4527d2ad9b086..216fa21bc951a 100644 --- a/localization/ar_tag_based_localizer/package.xml +++ b/localization/landmark_based_localizer/ar_tag_based_localizer/package.xml @@ -12,13 +12,10 @@ autoware_cmake aruco - autoware_auto_mapping_msgs cv_bridge - geometry_msgs + diagnostic_msgs image_transport - lanelet2_extension rclcpp - sensor_msgs tf2_eigen tf2_geometry_msgs tf2_ros diff --git a/localization/ar_tag_based_localizer/src/ar_tag_based_localizer_core.cpp b/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer_core.cpp similarity index 60% rename from localization/ar_tag_based_localizer/src/ar_tag_based_localizer_core.cpp rename to localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer_core.cpp index ca3bfe2e4a6b0..afa82ab3e0677 100644 --- a/localization/ar_tag_based_localizer/src/ar_tag_based_localizer_core.cpp +++ b/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer_core.cpp @@ -44,12 +44,12 @@ #include "ar_tag_based_localizer/ar_tag_based_localizer_core.hpp" -#include "ar_tag_based_localizer/utils.hpp" - #include #include +#include #include +#include #ifdef ROS_DISTRO_GALACTIC #include @@ -69,9 +69,10 @@ bool ArTagBasedLocalizer::setup() */ marker_size_ = static_cast(this->declare_parameter("marker_size")); target_tag_ids_ = this->declare_parameter>("target_tag_ids"); - covariance_ = this->declare_parameter>("covariance"); + base_covariance_ = this->declare_parameter>("base_covariance"); distance_threshold_squared_ = std::pow(this->declare_parameter("distance_threshold"), 2); - camera_frame_ = this->declare_parameter("camera_frame"); + ekf_time_tolerance_ = this->declare_parameter("ekf_time_tolerance"); + ekf_position_tolerance_ = this->declare_parameter("ekf_position_tolerance"); std::string detection_mode = this->declare_parameter("detection_mode"); float min_marker_size = static_cast(this->declare_parameter("min_marker_size")); if (detection_mode == "DM_NORMAL") { @@ -109,20 +110,27 @@ bool ArTagBasedLocalizer::setup() /* Subscribers */ - image_sub_ = it_->subscribe("~/input/image", 1, &ArTagBasedLocalizer::image_callback, this); + rclcpp::QoS qos_sub(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_default)); + qos_sub.best_effort(); + image_sub_ = this->create_subscription( + "~/input/image", qos_sub, + std::bind(&ArTagBasedLocalizer::image_callback, this, std::placeholders::_1)); cam_info_sub_ = this->create_subscription( - "~/input/camera_info", 1, + "~/input/camera_info", qos_sub, std::bind(&ArTagBasedLocalizer::cam_info_callback, this, std::placeholders::_1)); + ekf_pose_sub_ = this->create_subscription( + "~/input/ekf_pose", qos_sub, + std::bind(&ArTagBasedLocalizer::ekf_pose_callback, this, std::placeholders::_1)); /* Publishers */ - auto qos = rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_default)); - qos.reliable(); - qos.transient_local(); + rclcpp::QoS qos_pub(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_default)); image_pub_ = it_->advertise("~/debug/result", 1); pose_pub_ = this->create_publisher( - "~/output/pose_with_covariance", qos); + "~/output/pose_with_covariance", qos_pub); + diag_pub_ = + this->create_publisher("/diagnostics", qos_pub); RCLCPP_INFO(this->get_logger(), "Setup of ar_tag_based_localizer node is successful!"); return true; @@ -164,7 +172,7 @@ void ArTagBasedLocalizer::image_callback(const sensor_msgs::msg::Image::ConstSha geometry_msgs::msg::TransformStamped tf_cam_to_marker_stamped; tf2::toMsg(tf_cam_to_marker, tf_cam_to_marker_stamped.transform); tf_cam_to_marker_stamped.header.stamp = curr_stamp; - tf_cam_to_marker_stamped.header.frame_id = camera_frame_; + tf_cam_to_marker_stamped.header.frame_id = msg->header.frame_id; tf_cam_to_marker_stamped.child_frame_id = "detected_marker_" + std::to_string(marker.id); tf_broadcaster_->sendTransform(tf_cam_to_marker_stamped); @@ -172,7 +180,7 @@ void ArTagBasedLocalizer::image_callback(const sensor_msgs::msg::Image::ConstSha tf2::toMsg(tf_cam_to_marker, pose_cam_to_marker.pose); pose_cam_to_marker.header.stamp = curr_stamp; pose_cam_to_marker.header.frame_id = std::to_string(marker.id); - publish_pose_as_base_link(pose_cam_to_marker); + publish_pose_as_base_link(pose_cam_to_marker, msg->header.frame_id); // drawing the detected markers marker.draw(in_image, cv::Scalar(0, 0, 255), 2); @@ -193,6 +201,32 @@ void ArTagBasedLocalizer::image_callback(const sensor_msgs::msg::Image::ConstSha out_msg.image = in_image; image_pub_.publish(out_msg.toImageMsg()); } + + const int detected_tags = markers.size(); + + diagnostic_msgs::msg::DiagnosticStatus diag_status; + + if (detected_tags > 0) { + diag_status.level = diagnostic_msgs::msg::DiagnosticStatus::OK; + diag_status.message = "AR tags detected. The number of tags: " + std::to_string(detected_tags); + } else { + diag_status.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + diag_status.message = "No AR tags detected."; + } + + diag_status.name = "localization: " + std::string(this->get_name()); + diag_status.hardware_id = this->get_name(); + + diagnostic_msgs::msg::KeyValue key_value; + key_value.key = "Number of Detected AR Tags"; + key_value.value = std::to_string(detected_tags); + diag_status.values.push_back(key_value); + + diagnostic_msgs::msg::DiagnosticArray diag_msg; + diag_msg.header.stamp = this->now(); + diag_msg.status.push_back(diag_status); + + diag_pub_->publish(diag_msg); } // wait for one camera info, then shut down that subscriber @@ -202,11 +236,41 @@ void ArTagBasedLocalizer::cam_info_callback(const sensor_msgs::msg::CameraInfo & return; } - cam_param_ = ros_camera_info_to_aruco_cam_params(msg, true); + cv::Mat camera_matrix(3, 4, CV_64FC1, 0.0); + cv::Mat distortion_coeff(4, 1, CV_64FC1); + cv::Size size(static_cast(msg.width), static_cast(msg.height)); + + camera_matrix.setTo(0); + camera_matrix.at(0, 0) = msg.p[0]; + camera_matrix.at(0, 1) = msg.p[1]; + camera_matrix.at(0, 2) = msg.p[2]; + camera_matrix.at(0, 3) = msg.p[3]; + camera_matrix.at(1, 0) = msg.p[4]; + camera_matrix.at(1, 1) = msg.p[5]; + camera_matrix.at(1, 2) = msg.p[6]; + camera_matrix.at(1, 3) = msg.p[7]; + camera_matrix.at(2, 0) = msg.p[8]; + camera_matrix.at(2, 1) = msg.p[9]; + camera_matrix.at(2, 2) = msg.p[10]; + camera_matrix.at(2, 3) = msg.p[11]; + + for (int i = 0; i < 4; ++i) { + distortion_coeff.at(i, 0) = 0; + } + + cam_param_ = aruco::CameraParameters(camera_matrix, distortion_coeff, size); + cam_info_received_ = true; } -void ArTagBasedLocalizer::publish_pose_as_base_link(const geometry_msgs::msg::PoseStamped & msg) +void ArTagBasedLocalizer::ekf_pose_callback( + const geometry_msgs::msg::PoseWithCovarianceStamped & msg) +{ + latest_ekf_pose_ = msg; +} + +void ArTagBasedLocalizer::publish_pose_as_base_link( + const geometry_msgs::msg::PoseStamped & msg, const std::string & camera_frame_id) { // Check if frame_id is in target_tag_ids if ( @@ -241,7 +305,7 @@ void ArTagBasedLocalizer::publish_pose_as_base_link(const geometry_msgs::msg::Po geometry_msgs::msg::TransformStamped camera_to_base_link_tf; try { camera_to_base_link_tf = - tf_buffer_->lookupTransform(camera_frame_, "base_link", tf2::TimePointZero); + tf_buffer_->lookupTransform(camera_frame_id, "base_link", tf2::TimePointZero); } catch (tf2::TransformException & ex) { RCLCPP_INFO(this->get_logger(), "Could not transform base_link to camera: %s", ex.what()); return; @@ -265,8 +329,68 @@ void ArTagBasedLocalizer::publish_pose_as_base_link(const geometry_msgs::msg::Po pose_with_covariance_stamped.header.stamp = msg.header.stamp; pose_with_covariance_stamped.header.frame_id = "map"; pose_with_covariance_stamped.pose.pose = tf2::toMsg(map_to_base_link); - std::copy( - covariance_.begin(), covariance_.end(), pose_with_covariance_stamped.pose.covariance.begin()); + + // If latest_ekf_pose_ is older than seconds compared to current frame, it + // will not be published. + const rclcpp::Duration tolerance{ + static_cast(ekf_time_tolerance_), + static_cast((ekf_time_tolerance_ - std::floor(ekf_time_tolerance_)) * 1e9)}; + if (rclcpp::Time(latest_ekf_pose_.header.stamp) + tolerance < rclcpp::Time(msg.header.stamp)) { + RCLCPP_INFO( + this->get_logger(), + "latest_ekf_pose_ is older than %f seconds compared to current frame. " + "latest_ekf_pose_.header.stamp: %d.%d, msg.header.stamp: %d.%d", + ekf_time_tolerance_, latest_ekf_pose_.header.stamp.sec, latest_ekf_pose_.header.stamp.nanosec, + msg.header.stamp.sec, msg.header.stamp.nanosec); + return; + } + + // If curr_pose differs from latest_ekf_pose_ by more than , it will not + // be published. + const geometry_msgs::msg::Pose curr_pose = pose_with_covariance_stamped.pose.pose; + const geometry_msgs::msg::Pose latest_ekf_pose = latest_ekf_pose_.pose.pose; + const double diff_x = curr_pose.position.x - latest_ekf_pose.position.x; + const double diff_y = curr_pose.position.y - latest_ekf_pose.position.y; + const double diff_z = curr_pose.position.z - latest_ekf_pose.position.z; + const double diff_distance_squared = diff_x * diff_x + diff_y * diff_y + diff_z * diff_z; + const double threshold = ekf_position_tolerance_ * ekf_position_tolerance_; + if (threshold < diff_distance_squared) { + RCLCPP_INFO( + this->get_logger(), + "curr_pose differs from latest_ekf_pose_ by more than %f m. " + "curr_pose: (%f, %f, %f), latest_ekf_pose: (%f, %f, %f)", + ekf_position_tolerance_, curr_pose.position.x, curr_pose.position.y, curr_pose.position.z, + latest_ekf_pose.position.x, latest_ekf_pose.position.y, latest_ekf_pose.position.z); + return; + } + + // ~5[m]: base_covariance + // 5~[m]: scaling base_covariance by std::pow(distance/5, 3) + const double distance = std::sqrt(distance_squared); + const double scale = distance / 5; + const double coeff = std::max(1.0, std::pow(scale, 3)); + for (int i = 0; i < 36; i++) { + pose_with_covariance_stamped.pose.covariance[i] = coeff * base_covariance_[i]; + } pose_pub_->publish(pose_with_covariance_stamped); } + +tf2::Transform ArTagBasedLocalizer::aruco_marker_to_tf2(const aruco::Marker & marker) +{ + cv::Mat rot(3, 3, CV_64FC1); + cv::Mat r_vec64; + marker.Rvec.convertTo(r_vec64, CV_64FC1); + cv::Rodrigues(r_vec64, rot); + cv::Mat tran64; + marker.Tvec.convertTo(tran64, CV_64FC1); + + tf2::Matrix3x3 tf_rot( + rot.at(0, 0), rot.at(0, 1), rot.at(0, 2), rot.at(1, 0), + rot.at(1, 1), rot.at(1, 2), rot.at(2, 0), rot.at(2, 1), + rot.at(2, 2)); + + tf2::Vector3 tf_orig(tran64.at(0, 0), tran64.at(1, 0), tran64.at(2, 0)); + + return tf2::Transform(tf_rot, tf_orig); +} diff --git a/localization/ar_tag_based_localizer/src/ar_tag_based_localizer_node.cpp b/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer_node.cpp similarity index 100% rename from localization/ar_tag_based_localizer/src/ar_tag_based_localizer_node.cpp rename to localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer_node.cpp diff --git a/localization/ar_tag_based_localizer/doc_image/lanelet2_data_structure.drawio.svg b/localization/landmark_based_localizer/doc_image/lanelet2_data_structure.drawio.svg similarity index 91% rename from localization/ar_tag_based_localizer/doc_image/lanelet2_data_structure.drawio.svg rename to localization/landmark_based_localizer/doc_image/lanelet2_data_structure.drawio.svg index bc8ba297558bb..5895d8ef65073 100644 --- a/localization/ar_tag_based_localizer/doc_image/lanelet2_data_structure.drawio.svg +++ b/localization/landmark_based_localizer/doc_image/lanelet2_data_structure.drawio.svg @@ -6,7 +6,7 @@ width="336px" height="336px" viewBox="-0.5 -0.5 336 336" - content="<mxfile><diagram id="z_GJx55S16-dNHi1224P" name="ページ1">5ZhNj5swEIZ/DVJ7wzYBcsymu+2lUtVU6tmCCVg1ODLOhuyvr1kMBDsROdBk1eQQ4ddfzDPj8SQeWRf1V0l3+XeRAvewn9Ye+eJhHC1i/d0Ix1Ygy6gVMsnSVkKDsGFvYETfqHuWQjUaqITgiu3GYiLKEhI10qiU4jAethV8vOuOZuAIm4RyV/3NUpW3aoyjQf8GLMu7nVG4bHsK2g02llQ5TcXhRCLPHllLIVT7VNRr4A27jks77+VCb/9iEkp1zYRg0c54pXxvjDMvpo6dtVLsyxSaCb5Hng45U7DZ0aTpPWj3ai1XBdctpB/NciAV1BffCfWW6ggBUYCSRz2kmxAaOCY6UAfrMLDux+SnnDuRGv9m/doDAv1gKFwgEjpEkINET9GxBtM4aLVrA3DL6gbhLHyCMR8cu3zwGTx4DjqRQ8cNmLvSIfiOdGKHDvnYdJB/QzpLh07wsejYJ+uWdBb+dCaGMl0115duJZxWFUvOJV9IndtrEsCJgYszBnaaBE4Vex0vf85qs8MPwfTGPd/YOpq+xa0Se5mAmXR6a1nrBP7EQorKDJSz0LsPequvcwty3FI7ftERp8aeqJQUf2AtuJBaKUXZRPSWcW5JlLOsbNypHQRaf2ril+lKY2U6Cpam/NJxGN/MW1EqUyuhcKZ0EVkHInIPBDkTL2SOA4Ed8sfHId/Xuscu5G8Hnvz/mQhbmZ7YpeO1qcheKPh3mSi4wi33vD2xXVvcsPJauFX7jHBO04enz9j7Zy5okQUtcqCFLrNwBmRuKf/2OPkVLS3uIbldgnV/Jqx+6vYvmn16kdrWz4/jh3jymkNoHjfo5vC/Spt/hz+nyPNf</diagram></mxfile>" + content="<mxfile><diagram id="z_GJx55S16-dNHi1224P" name="ページ1">5ZhNb6MwEIZ/DdLuDWwC9Nhm2+5hV1ophz1beAJWDY6Mk5D++jXFfNmJkgNNqk0OEbz+wPPMeDzg4WVRv0qyyX8LCtxDPq09/MNDCCeJ/m+EQytEKGyFTDLaSsEgrNg7GNE36pZRqCYdlRBcsc1UTEVZQqomGpFS7Kfd1oJPn7ohGTjCKiXcVf8yqvJWTVA86D+BZXn35CB6aFsK0nU2llQ5oWI/kvCzh5dSCNVeFfUSeMOu49KOeznR2i9MQqkuGRAu2hE7wrfGOLMwdeislWJbUmgG+B5+2udMwWpD0qZ1r92rtVwVXN8F+tJdgFnTDqSCeiSZBb2CKEDJg+7StUYGjomOoIO1H1j3ffIx504kxr9ZP/eAQF8YCieIRA6RwEGih+hYg/M4SLVpA3DN6gbhLHzCKR+UuHzQETxoDjqxQ8cNmJvSweiGdBKHDv7adAL/inQeHDrh16Jj76xr0ln45zMxlPSxOb70XcpJVbH0wuQLdHKguQBGBi6OGNhpEjhRbDfNi8esNk/4I5heSc83sbamb3GrxFamYAaNTy1rntA/M5EiMgPlTPThg97qy9wSOG6pHb/o8FJTT1RKijdYCi6kVkpRNhG9ZpxbEuEsKxt3ao+B1p+aYGW60ng0DQWjlJ/aDtOTeS1KZWqlIJopXcTWhojdDYGPxAueY0Mgh/zhfsj3te6hC/nrgcf/fyZCVqbHdul4aSqyJwo/LxOFF7jllqcnsmuLK1ZeC7dqnxHOOH14eo99/OaCFlvQYgda5DKLZkDmlvLv95NfgweLe4Svl2Dd14RfpKQFkW/fXqS29fv9+CE5e8wFwTxu0LfDd5U2/w4fp/DzPw==</diagram></mxfile>" > @@ -138,12 +138,12 @@
- AR Tag(Front) + Landmark(Front)
- AR Tag(Front) + Landmark(Front) diff --git a/localization/ar_tag_based_localizer/doc_image/node_diagram.drawio.svg b/localization/landmark_based_localizer/doc_image/node_diagram.drawio.svg similarity index 81% rename from localization/ar_tag_based_localizer/doc_image/node_diagram.drawio.svg rename to localization/landmark_based_localizer/doc_image/node_diagram.drawio.svg index a339951663b29..120a1f2aa3391 100644 --- a/localization/ar_tag_based_localizer/doc_image/node_diagram.drawio.svg +++ b/localization/landmark_based_localizer/doc_image/node_diagram.drawio.svg @@ -6,20 +6,20 @@ width="721px" height="331px" viewBox="-0.5 -0.5 721 331" - content="<mxfile><diagram id="z_GJx55S16-dNHi1224P" name="ページ1">1Zhdk5sgFIZ/TS6bEdF8XDYm2V5spzvdnWl75RBFZdaIg+Srv74QMcZAdt3EbKZeJHIAhec9nAP2oLfcPjCUJ99piNOebYXbHpz2bBtYliv+pGVXWhwwKA0xI6FqVBueyV9c9VTWFQlx0WjIKU05yZvGgGYZDnjDhhijm2aziKbNt+YoxprhOUCpbv1FQp6U1pE9rO3fMImT6s1gMC5rlqhqrGZSJCikmyMTnPWgxyjl5d1y6+FUwqu4lP3mZ2oPA2M442062GWHNUpXam5qXHxXTZbRVRZi2d7qwUlEM64EAbYqezSlbN8YWvtL2PWBqLGtMeN4e2RSA3vAdIk524kmqtZ2FSTlJVAVNzXyg0ckR7gHyoaUyvHhyTUIcaNYmLk4GpevP0X5BcXid4IKwcMWPguBnOsjFa4hmLD32JE0PWLleTN3LsYzKTijr/ioZhCM8CK6LV04aNJ1dLpjA1ynA7iuBvcHTyQ96+uK042Y1RHcHAWvYj0VH2U7n3ueie3IXsDB4Maeez+2cGhY0XO+IQX3N4QnfkDXiBGUBVgjikMR5VSRMp7QmGYondXWiUDCdr8V8n3hjyz0h25Vnm6Pa6c7VToLlSMWY2WCyibH8SZmhlPEyboZjU3UVNcnSsRrz7o+dEbNRxR0xQKsep2wPwyjlRxVnG/KQZYywbyFP6PZW7ztTniD4efwdqDb4G0PTxy9HJTGW3uQbTWFsyG8mXAjk3A5LfCHllGl45bwvYx9V5WkjkDd1xrKwu5Y0E61L+k0tb9o/Sk5vlh9IK6y07U+Mmz6CIQf9ZGqIY2iAl8rPzCGUcR8jmJ/IfO/n16Z9c/lH303sL+6yUzO6Qqy9dQEXENucrvITcAEFb9Gl7NUWf4uLAen+1PwmSyN8SlAS8yQT7KIXpNeuknnn5VetKzgtkvnhjzVfBAYua1i0AXxxYEG+d4XrFVoh2NdC6fzrVVrRx1rM31EGU4x12eMirw8rkdkK9f8JMeMiDdiuXrFY8X5Hj/VpomIGXEmqgKBY284HKelk4aoSA6RoxDnCJLFLzQXBigM5R7s8D8lS3G0m6dkIeN8IBH4IWFiOFTOdx4ijmTcL/rFOu4mfoxOvM2Wu4PTAGKKH874elkc/RAmzwmRX3Chf9DGGQ98QVvPrA44t40SrREY9+YywQsMASr4/3WgP03tAIwN/nSzhOTon0w6i2iOIbtU6t3Bb4zbmFQFNX+JcrGfQeHFm5n7fLI49R5jNOrKe0Sx/sZZps/6SzGc/QM=</diagram></mxfile>" + content="<mxfile><diagram id="z_GJx55S16-dNHi1224P" name="ページ1">1VnLjpswFP2aLBsFG0iybF7TxVQdaSq1XSEHDFgDGBnn1a+vHQwE8ExIQiYqm+BjA/Y51/fhDOA83j8xlIbfqYejARh5+wFcDAAwgA3Ej0QOOWIDMwcCRjw1qAJeyV+swJFCN8TDWW0gpzTiJK2DLk0S7PIahhiju/own0b1r6YowC3g1UVRG/1FPB7m6ASMK/wbJkFYfNmwp3lPjIrBaiVZiDy6O4HgcgDnjFKe38X7OY4keQUv+XOrd3rLiTGc8C4PKCG2KNqotal58UOxWEY3iYfl+NEAznyacCWIAVR7TiPKjoPh6HgJvD0RNbctZhzvTyA1sSdMY8zZQQxRvcBSJCkrgaq5qygvsfCEblthSKkclG+uiBA3igs9L2aLl2eUeDFibwKdoUzwAWyxXkOu9ZkK0xCcsHPckSg64Wo+X1orMZ9Zxhl9wyc9tjvBa/++7MJxnV2zza4x0rBr9sCu1WL3Bw8lfaOvG053Ylkn7KbIfRMbKruU3NVqPteROwFraNt3Nl37ceTCsWZPr/iOZNzZER46Lt0iRlDi4hal2BN+TjUp4yENaIKiZYXOBCfs8Ftxfmz8kY3h2Crai/1p7+KgWu+yyhELsIKgwuQ8PuSZ4Qhxsq37Yx1r6tEXSsRnK+Nv6APNSf0VGd0wF6unGtyX0+gkR+Hp63KQWIaYj+hPaPIR36AXvo3x5/BtQqvGNxg3DD2fVIvv1ovAqC4cgPBuwk10wqU0wxdto0LHPeFHGYeWakkdDXVfaSgbh1NBe9U+Z6eu/VX7T8nxZTQ0xJU/dKuNjOs2AuGlNlIMpL6f4VvlN7RuFDGHo8BZywzAiW6M++8FoHY+cLz6CU1mcwcBTWyyNLHJ6iM2GTpS8Zt/PZcqzD+ES7uZoRqfyaXWP7koxgw5JPHpLeGln3D+WeGlFRWsbuFcE6fqLzImVicfdIV/MaFGvvOCdXLtcNrWwuw9tepsqNPWSkUxhSPM2ytGWZoX7D7Zyz0/SzEj4otY7l7xWlHh45cKmgmfESSiyxV0HIGyoJZG6qEsLD1HJgoJkgQ/aSoAKIA8Byt/FyQOxFIispZ+3pUUOB5hYjpUrnflIY6k38+G2Tbox39MGtYGZHbQdCA6/2FOb5fFbFdhsk7wnYwL/d0uxljya3S1zKLAua+X6EyBNjePVKXvCC5clPH/q65vxnfDmGqM6m5RyWyfnPTm1kxNiCkkfIDxaHOZSHk2J0apSGqQd3VG85iDi6b1aF3S/azH1nEq88OL664zxxdGPd85l+7oy7eyYtOWb91C9QPTpuYRILw2bbKbxynWeNhX4iSa1XF4Prz6UwEu/wE=</diagram></mxfile>" > - - + + - AR Tag Based - Localizer + Landmark Based + Localizer - + - Other Autoware - packages + Other Autoware + packages @@ -74,7 +74,7 @@ - /tag_tf_caster + /landmark_tf_caster @@ -82,5 +82,11 @@ /lanelet2_map_loader + + + + + /ekf_pose_with_covariance + diff --git a/localization/ar_tag_based_localizer/doc_image/principle.png b/localization/landmark_based_localizer/doc_image/principle.png similarity index 100% rename from localization/ar_tag_based_localizer/doc_image/principle.png rename to localization/landmark_based_localizer/doc_image/principle.png diff --git a/localization/landmark_based_localizer/landmark_tf_caster/CMakeLists.txt b/localization/landmark_based_localizer/landmark_tf_caster/CMakeLists.txt new file mode 100644 index 0000000000000..8d8cb546b6162 --- /dev/null +++ b/localization/landmark_based_localizer/landmark_tf_caster/CMakeLists.txt @@ -0,0 +1,24 @@ +cmake_minimum_required(VERSION 3.14) +project(landmark_tf_caster) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +ament_auto_add_executable(landmark_tf_caster + src/landmark_tf_caster_node.cpp + src/landmark_tf_caster_core.cpp +) + +ament_auto_package( + INSTALL_TO_SHARE + launch + config +) diff --git a/localization/ar_tag_based_localizer/config/tag_tf_caster.param.yaml b/localization/landmark_based_localizer/landmark_tf_caster/config/landmark_tf_caster.param.yaml similarity index 100% rename from localization/ar_tag_based_localizer/config/tag_tf_caster.param.yaml rename to localization/landmark_based_localizer/landmark_tf_caster/config/landmark_tf_caster.param.yaml diff --git a/localization/ar_tag_based_localizer/include/ar_tag_based_localizer/tag_tf_caster_core.hpp b/localization/landmark_based_localizer/landmark_tf_caster/include/landmark_tf_caster/landmark_tf_caster_core.hpp similarity index 84% rename from localization/ar_tag_based_localizer/include/ar_tag_based_localizer/tag_tf_caster_core.hpp rename to localization/landmark_based_localizer/landmark_tf_caster/include/landmark_tf_caster/landmark_tf_caster_core.hpp index 5b10fe5b1dc0f..0eb6706a0b5b0 100644 --- a/localization/ar_tag_based_localizer/include/ar_tag_based_localizer/tag_tf_caster_core.hpp +++ b/localization/landmark_based_localizer/landmark_tf_caster/include/landmark_tf_caster/landmark_tf_caster_core.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AR_TAG_BASED_LOCALIZER__TAG_TF_CASTER_CORE_HPP_ -#define AR_TAG_BASED_LOCALIZER__TAG_TF_CASTER_CORE_HPP_ +#ifndef LANDMARK_TF_CASTER__LANDMARK_TF_CASTER_CORE_HPP_ +#define LANDMARK_TF_CASTER__LANDMARK_TF_CASTER_CORE_HPP_ #include @@ -26,10 +26,10 @@ #include -class TagTfCaster : public rclcpp::Node +class LandmarkTfCaster : public rclcpp::Node { public: - TagTfCaster(); + LandmarkTfCaster(); private: void map_bin_callback(const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr & msg); @@ -46,4 +46,4 @@ class TagTfCaster : public rclcpp::Node rclcpp::Subscription::SharedPtr map_bin_sub_; }; -#endif // AR_TAG_BASED_LOCALIZER__TAG_TF_CASTER_CORE_HPP_ +#endif // LANDMARK_TF_CASTER__LANDMARK_TF_CASTER_CORE_HPP_ diff --git a/localization/landmark_based_localizer/landmark_tf_caster/launch/landmark_tf_caster.launch.xml b/localization/landmark_based_localizer/landmark_tf_caster/launch/landmark_tf_caster.launch.xml new file mode 100644 index 0000000000000..eeaba555cb6f5 --- /dev/null +++ b/localization/landmark_based_localizer/landmark_tf_caster/launch/landmark_tf_caster.launch.xml @@ -0,0 +1,12 @@ + + + + + + + + + + + + diff --git a/localization/landmark_based_localizer/landmark_tf_caster/package.xml b/localization/landmark_based_localizer/landmark_tf_caster/package.xml new file mode 100644 index 0000000000000..75f42b91f502a --- /dev/null +++ b/localization/landmark_based_localizer/landmark_tf_caster/package.xml @@ -0,0 +1,24 @@ + + + + landmark_tf_caster + 0.0.0 + The landmark_tf_caster package + Shintaro Sakoda + Masahiro Sakamoto + Apache License 2.0 + + ament_cmake + autoware_cmake + + autoware_auto_mapping_msgs + lanelet2_extension + rclcpp + tf2_eigen + tf2_geometry_msgs + tf2_ros + + + ament_cmake + + diff --git a/localization/ar_tag_based_localizer/src/tag_tf_caster_core.cpp b/localization/landmark_based_localizer/landmark_tf_caster/src/landmark_tf_caster_core.cpp similarity index 92% rename from localization/ar_tag_based_localizer/src/tag_tf_caster_core.cpp rename to localization/landmark_based_localizer/landmark_tf_caster/src/landmark_tf_caster_core.cpp index d0279caf88dda..801e036415326 100644 --- a/localization/ar_tag_based_localizer/src/tag_tf_caster_core.cpp +++ b/localization/landmark_based_localizer/landmark_tf_caster/src/landmark_tf_caster_core.cpp @@ -12,20 +12,21 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "ar_tag_based_localizer/tag_tf_caster_core.hpp" +#include "landmark_tf_caster/landmark_tf_caster_core.hpp" #include "lanelet2_extension/utility/message_conversion.hpp" #include #include +#include #ifdef ROS_DISTRO_GALACTIC #include #else #include #endif -TagTfCaster::TagTfCaster() : Node("tag_tf_caster") +LandmarkTfCaster::LandmarkTfCaster() : Node("landmark_tf_caster") { // Parameters volume_threshold_ = this->declare_parameter("volume_threshold", 1e-5); @@ -37,11 +38,11 @@ TagTfCaster::TagTfCaster() : Node("tag_tf_caster") // Subscribers map_bin_sub_ = this->create_subscription( "~/input/lanelet2_map", rclcpp::QoS(10).durability(rclcpp::DurabilityPolicy::TransientLocal), - std::bind(&TagTfCaster::map_bin_callback, this, std::placeholders::_1)); + std::bind(&LandmarkTfCaster::map_bin_callback, this, std::placeholders::_1)); RCLCPP_INFO(this->get_logger(), "finish setup"); } -void TagTfCaster::map_bin_callback( +void LandmarkTfCaster::map_bin_callback( const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr & msg) { RCLCPP_INFO_STREAM(this->get_logger(), "msg->format_version: " << msg->format_version); @@ -59,7 +60,7 @@ void TagTfCaster::map_bin_callback( } } -void TagTfCaster::publish_tf(const lanelet::Polygon3d & poly) +void LandmarkTfCaster::publish_tf(const lanelet::Polygon3d & poly) { // Get marker_id const std::string marker_id = poly.attributeOr("marker_id", "none"); diff --git a/localization/ar_tag_based_localizer/src/tag_tf_caster_node.cpp b/localization/landmark_based_localizer/landmark_tf_caster/src/landmark_tf_caster_node.cpp similarity index 85% rename from localization/ar_tag_based_localizer/src/tag_tf_caster_node.cpp rename to localization/landmark_based_localizer/landmark_tf_caster/src/landmark_tf_caster_node.cpp index ce83c572d6a6e..4c34e593e7552 100644 --- a/localization/ar_tag_based_localizer/src/tag_tf_caster_node.cpp +++ b/localization/landmark_based_localizer/landmark_tf_caster/src/landmark_tf_caster_node.cpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "ar_tag_based_localizer/tag_tf_caster_core.hpp" +#include "landmark_tf_caster/landmark_tf_caster_core.hpp" int main(int argc, char ** argv) { rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); + rclcpp::spin(std::make_shared()); rclcpp::shutdown(); } diff --git a/localization/localization_error_monitor/CMakeLists.txt b/localization/localization_error_monitor/CMakeLists.txt index 258e942169556..3528367486350 100644 --- a/localization/localization_error_monitor/CMakeLists.txt +++ b/localization/localization_error_monitor/CMakeLists.txt @@ -4,10 +4,31 @@ project(localization_error_monitor) find_package(autoware_cmake REQUIRED) autoware_package() +ament_auto_add_library(localization_error_monitor_module SHARED + src/diagnostics.cpp +) + ament_auto_add_executable(localization_error_monitor src/main.cpp src/node.cpp ) +target_link_libraries(localization_error_monitor localization_error_monitor_module) + +if(BUILD_TESTING) + function(add_testcase filepath) + get_filename_component(filename ${filepath} NAME) + string(REGEX REPLACE ".cpp" "" test_name ${filename}) + ament_add_gtest(${test_name} ${filepath}) + target_include_directories(${test_name} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/include) + target_link_libraries(${test_name} localization_error_monitor_module) + ament_target_dependencies(${test_name} ${${PROJECT_NAME}_FOUND_BUILD_DEPENDS}) + endfunction() + + + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() + add_testcase(test/test_diagnostics.cpp) +endif() ament_auto_package(INSTALL_TO_SHARE config diff --git a/localization/localization_error_monitor/README.md b/localization/localization_error_monitor/README.md index bf005d6a00bbb..2ddf41e4eac70 100644 --- a/localization/localization_error_monitor/README.md +++ b/localization/localization_error_monitor/README.md @@ -2,6 +2,10 @@ ## Purpose +

+ +

+ localization_error_monitor is a package for diagnosing localization errors by monitoring uncertainty of the localization results. The package monitors the following two values: @@ -25,10 +29,10 @@ The package monitors the following two values: ## Parameters -| Name | Type | Description | -| -------------------------------------- | ------ | ------------------------------------------------------------------------------------------- | -| `scale` | double | scale factor for monitored values (default: 3.0) | -| `error_ellipse_size` | double | error threshold for long radius of confidence ellipse [m] (default: 1.0) | -| `warn_ellipse_size` | double | warning threshold for long radius of confidence ellipse [m] (default: 0.8) | -| `error_ellipse_size_lateral_direction` | double | error threshold for size of confidence ellipse along lateral direction [m] (default: 0.3) | -| `warn_ellipse_size_lateral_direction` | double | warning threshold for size of confidence ellipse along lateral direction [m] (default: 0.2) | +| Name | Type | Description | +| -------------------------------------- | ------ | -------------------------------------------------------------------------------------------- | +| `scale` | double | scale factor for monitored values (default: 3.0) | +| `error_ellipse_size` | double | error threshold for long radius of confidence ellipse [m] (default: 1.0) | +| `warn_ellipse_size` | double | warning threshold for long radius of confidence ellipse [m] (default: 0.8) | +| `error_ellipse_size_lateral_direction` | double | error threshold for size of confidence ellipse along lateral direction [m] (default: 0.3) | +| `warn_ellipse_size_lateral_direction` | double | warning threshold for size of confidence ellipse along lateral direction [m] (default: 0.25) | diff --git a/localization/localization_error_monitor/config/localization_error_monitor.param.yaml b/localization/localization_error_monitor/config/localization_error_monitor.param.yaml index 026daf0532d33..def5c80164642 100644 --- a/localization/localization_error_monitor/config/localization_error_monitor.param.yaml +++ b/localization/localization_error_monitor/config/localization_error_monitor.param.yaml @@ -1,7 +1,7 @@ /**: ros__parameters: scale: 3.0 - error_ellipse_size: 1.0 - warn_ellipse_size: 0.8 + error_ellipse_size: 1.5 + warn_ellipse_size: 1.2 error_ellipse_size_lateral_direction: 0.3 - warn_ellipse_size_lateral_direction: 0.2 + warn_ellipse_size_lateral_direction: 0.25 diff --git a/localization/localization_error_monitor/include/localization_error_monitor/diagnostics.hpp b/localization/localization_error_monitor/include/localization_error_monitor/diagnostics.hpp new file mode 100644 index 0000000000000..4fe4e5a5aac14 --- /dev/null +++ b/localization/localization_error_monitor/include/localization_error_monitor/diagnostics.hpp @@ -0,0 +1,31 @@ +// Copyright 2023 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 LOCALIZATION_ERROR_MONITOR__DIAGNOSTICS_HPP_ +#define LOCALIZATION_ERROR_MONITOR__DIAGNOSTICS_HPP_ + +#include + +#include +#include + +diagnostic_msgs::msg::DiagnosticStatus checkLocalizationAccuracy( + const double ellipse_size, const double warn_ellipse_size, const double error_ellipse_size); +diagnostic_msgs::msg::DiagnosticStatus checkLocalizationAccuracyLateralDirection( + const double ellipse_size, const double warn_ellipse_size, const double error_ellipse_size); + +diagnostic_msgs::msg::DiagnosticStatus mergeDiagnosticStatus( + const std::vector & stat_array); + +#endif // LOCALIZATION_ERROR_MONITOR__DIAGNOSTICS_HPP_ diff --git a/localization/localization_error_monitor/include/localization_error_monitor/node.hpp b/localization/localization_error_monitor/include/localization_error_monitor/node.hpp index 83b2ddef9215a..7bdc87d5d47f2 100644 --- a/localization/localization_error_monitor/include/localization_error_monitor/node.hpp +++ b/localization/localization_error_monitor/include/localization_error_monitor/node.hpp @@ -16,7 +16,6 @@ #define LOCALIZATION_ERROR_MONITOR__NODE_HPP_ #include -#include #include #include @@ -37,6 +36,7 @@ class LocalizationErrorMonitor : public rclcpp::Node private: rclcpp::Subscription::SharedPtr odom_sub_; rclcpp::Publisher::SharedPtr ellipse_marker_pub_; + rclcpp::Publisher::SharedPtr diag_pub_; rclcpp::TimerBase::SharedPtr timer_; double scale_; @@ -45,16 +45,11 @@ class LocalizationErrorMonitor : public rclcpp::Node double error_ellipse_size_lateral_direction_; double warn_ellipse_size_lateral_direction_; Ellipse ellipse_; - diagnostic_updater::Updater updater_; - void checkLocalizationAccuracy(diagnostic_updater::DiagnosticStatusWrapper & stat); - void checkLocalizationAccuracyLateralDirection( - diagnostic_updater::DiagnosticStatusWrapper & stat); void onOdom(nav_msgs::msg::Odometry::ConstSharedPtr input_msg); visualization_msgs::msg::Marker createEllipseMarker( const Ellipse & ellipse, nav_msgs::msg::Odometry::ConstSharedPtr odom); double measureSizeEllipseAlongBodyFrame(const Eigen::Matrix2d & Pinv, double theta); - void onTimer(); public: LocalizationErrorMonitor(); diff --git a/localization/localization_error_monitor/media/diagnostics.png b/localization/localization_error_monitor/media/diagnostics.png new file mode 100644 index 0000000000000..74aacb68a2b7c Binary files /dev/null and b/localization/localization_error_monitor/media/diagnostics.png differ diff --git a/localization/localization_error_monitor/package.xml b/localization/localization_error_monitor/package.xml index 23e71f2a844cd..4cc5599fd81a9 100644 --- a/localization/localization_error_monitor/package.xml +++ b/localization/localization_error_monitor/package.xml @@ -5,6 +5,7 @@ 0.1.0 ros node for monitoring localization error Yamato Ando + Koji Minoda Masahiro Sakamoto Apache License 2.0 @@ -12,15 +13,16 @@ ament_cmake_auto autoware_cmake + eigen diagnostic_msgs diagnostic_updater - eigen nav_msgs tf2 tf2_geometry_msgs visualization_msgs + ament_cmake_ros ament_lint_auto autoware_lint_common diff --git a/localization/localization_error_monitor/src/diagnostics.cpp b/localization/localization_error_monitor/src/diagnostics.cpp new file mode 100644 index 0000000000000..0c5a4a7800639 --- /dev/null +++ b/localization/localization_error_monitor/src/diagnostics.cpp @@ -0,0 +1,94 @@ +// Copyright 2023 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 + +#include +#include + +diagnostic_msgs::msg::DiagnosticStatus checkLocalizationAccuracy( + const double ellipse_size, const double warn_ellipse_size, const double error_ellipse_size) +{ + diagnostic_msgs::msg::DiagnosticStatus stat; + + diagnostic_msgs::msg::KeyValue key_value; + key_value.key = "localization_accuracy"; + key_value.value = std::to_string(ellipse_size); + stat.values.push_back(key_value); + + stat.level = diagnostic_msgs::msg::DiagnosticStatus::OK; + stat.message = "OK"; + if (ellipse_size >= warn_ellipse_size) { + stat.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + stat.message = "ellipse size is too large"; + } + if (ellipse_size >= error_ellipse_size) { + stat.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + stat.message = "ellipse size is over the expected range"; + } + + return stat; +} + +diagnostic_msgs::msg::DiagnosticStatus checkLocalizationAccuracyLateralDirection( + const double ellipse_size, const double warn_ellipse_size, const double error_ellipse_size) +{ + diagnostic_msgs::msg::DiagnosticStatus stat; + + diagnostic_msgs::msg::KeyValue key_value; + key_value.key = "localization_accuracy_lateral_direction"; + key_value.value = std::to_string(ellipse_size); + stat.values.push_back(key_value); + + stat.level = diagnostic_msgs::msg::DiagnosticStatus::OK; + stat.message = "OK"; + if (ellipse_size >= warn_ellipse_size) { + stat.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + stat.message = "ellipse size along lateral direction is too large"; + } + if (ellipse_size >= error_ellipse_size) { + stat.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + stat.message = "ellipse size along lateral direction is over the expected range"; + } + + return stat; +} + +// The highest level within the stat_array will be reflected in the merged_stat. +diagnostic_msgs::msg::DiagnosticStatus mergeDiagnosticStatus( + const std::vector & stat_array) +{ + diagnostic_msgs::msg::DiagnosticStatus merged_stat; + + for (const auto & stat : stat_array) { + if ((stat.level > diagnostic_msgs::msg::DiagnosticStatus::OK)) { + if (!merged_stat.message.empty()) { + merged_stat.message += "; "; + } + merged_stat.message += stat.message; + } + if (stat.level > merged_stat.level) { + merged_stat.level = stat.level; + } + for (const auto & value : stat.values) { + merged_stat.values.push_back(value); + } + } + + if (merged_stat.level == diagnostic_msgs::msg::DiagnosticStatus::OK) { + merged_stat.message = "OK"; + } + + return merged_stat; +} diff --git a/localization/localization_error_monitor/src/node.cpp b/localization/localization_error_monitor/src/node.cpp index 75858c1f4be4d..43795d4036d42 100644 --- a/localization/localization_error_monitor/src/node.cpp +++ b/localization/localization_error_monitor/src/node.cpp @@ -14,6 +14,8 @@ #include "localization_error_monitor/node.hpp" +#include "localization_error_monitor/diagnostics.hpp" + #include #include @@ -30,17 +32,16 @@ #include #include -LocalizationErrorMonitor::LocalizationErrorMonitor() -: Node("localization_error_monitor"), updater_(this) +LocalizationErrorMonitor::LocalizationErrorMonitor() : Node("localization_error_monitor") { - scale_ = this->declare_parameter("scale", 3.0); - error_ellipse_size_ = this->declare_parameter("error_ellipse_size", 1.0); - warn_ellipse_size_ = this->declare_parameter("warn_ellipse_size", 0.8); + scale_ = this->declare_parameter("scale"); + error_ellipse_size_ = this->declare_parameter("error_ellipse_size"); + warn_ellipse_size_ = this->declare_parameter("warn_ellipse_size"); error_ellipse_size_lateral_direction_ = - this->declare_parameter("error_ellipse_size_lateral_direction", 0.3); + this->declare_parameter("error_ellipse_size_lateral_direction"); warn_ellipse_size_lateral_direction_ = - this->declare_parameter("warn_ellipse_size_lateral_direction", 0.2); + this->declare_parameter("warn_ellipse_size_lateral_direction"); odom_sub_ = this->create_subscription( "input/odom", 1, std::bind(&LocalizationErrorMonitor::onOdom, this, std::placeholders::_1)); @@ -51,55 +52,7 @@ LocalizationErrorMonitor::LocalizationErrorMonitor() ellipse_marker_pub_ = this->create_publisher("debug/ellipse_marker", durable_qos); - updater_.setHardwareID("localization_error_monitor"); - updater_.add("localization_accuracy", this, &LocalizationErrorMonitor::checkLocalizationAccuracy); - updater_.add( - "localization_accuracy_lateral_direction", this, - &LocalizationErrorMonitor::checkLocalizationAccuracyLateralDirection); - - // Set timer - using std::chrono_literals::operator""ms; - timer_ = rclcpp::create_timer( - this, get_clock(), 100ms, std::bind(&LocalizationErrorMonitor::onTimer, this)); -} - -void LocalizationErrorMonitor::onTimer() -{ - updater_.force_update(); -} - -void LocalizationErrorMonitor::checkLocalizationAccuracy( - diagnostic_updater::DiagnosticStatusWrapper & stat) -{ - stat.add("localization_accuracy", ellipse_.long_radius); - int8_t diag_level = diagnostic_msgs::msg::DiagnosticStatus::OK; - std::string diag_message = "ellipse size is within the expected range"; - if (warn_ellipse_size_ <= ellipse_.long_radius) { - diag_level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - diag_message = "ellipse size is too large"; - } - if (error_ellipse_size_ <= ellipse_.long_radius) { - diag_level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; - diag_message = "ellipse size is over the expected range"; - } - stat.summary(diag_level, diag_message); -} - -void LocalizationErrorMonitor::checkLocalizationAccuracyLateralDirection( - diagnostic_updater::DiagnosticStatusWrapper & stat) -{ - stat.add("localization_accuracy_lateral_direction", ellipse_.size_lateral_direction); - int8_t diag_level = diagnostic_msgs::msg::DiagnosticStatus::OK; - std::string diag_message = "ellipse size along lateral direction is within the expected range"; - if (warn_ellipse_size_lateral_direction_ <= ellipse_.size_lateral_direction) { - diag_level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - diag_message = "ellipse size along lateral direction is too large"; - } - if (error_ellipse_size_lateral_direction_ <= ellipse_.size_lateral_direction) { - diag_level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; - diag_message = "ellipse size along lateral direction is over the expected range"; - } - stat.summary(diag_level, diag_message); + diag_pub_ = this->create_publisher("/diagnostics", 10); } visualization_msgs::msg::Marker LocalizationErrorMonitor::createEllipseMarker( @@ -158,6 +111,24 @@ void LocalizationErrorMonitor::onOdom(nav_msgs::msg::Odometry::ConstSharedPtr in const auto ellipse_marker = createEllipseMarker(ellipse_, input_msg); ellipse_marker_pub_->publish(ellipse_marker); + + // diagnostics + std::vector diag_status_array; + diag_status_array.push_back( + checkLocalizationAccuracy(ellipse_.long_radius, warn_ellipse_size_, error_ellipse_size_)); + diag_status_array.push_back(checkLocalizationAccuracyLateralDirection( + ellipse_.size_lateral_direction, warn_ellipse_size_lateral_direction_, + error_ellipse_size_lateral_direction_)); + + diagnostic_msgs::msg::DiagnosticStatus diag_merged_status; + diag_merged_status = mergeDiagnosticStatus(diag_status_array); + diag_merged_status.name = "localization: " + std::string(this->get_name()); + diag_merged_status.hardware_id = this->get_name(); + + diagnostic_msgs::msg::DiagnosticArray diag_msg; + diag_msg.header.stamp = this->now(); + diag_msg.status.push_back(diag_merged_status); + diag_pub_->publish(diag_msg); } double LocalizationErrorMonitor::measureSizeEllipseAlongBodyFrame( diff --git a/localization/localization_error_monitor/test/test_diagnostics.cpp b/localization/localization_error_monitor/test/test_diagnostics.cpp new file mode 100644 index 0000000000000..7b7e8aaab4ed9 --- /dev/null +++ b/localization/localization_error_monitor/test/test_diagnostics.cpp @@ -0,0 +1,140 @@ +// Copyright 2023 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 "localization_error_monitor/diagnostics.hpp" + +#include + +TEST(TestLocalizationErrorMonitorDiagnostics, CheckLocalizationAccuracy) +{ + diagnostic_msgs::msg::DiagnosticStatus stat; + + const double warn_ellipse_size = 0.8; + const double error_ellipse_size = 1.0; + + double ellipse_size = 0.0; + stat = checkLocalizationAccuracy(ellipse_size, warn_ellipse_size, error_ellipse_size); + EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK); + + ellipse_size = 0.7; + stat = checkLocalizationAccuracy(ellipse_size, warn_ellipse_size, error_ellipse_size); + EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK); + + ellipse_size = 0.8; + stat = checkLocalizationAccuracy(ellipse_size, warn_ellipse_size, error_ellipse_size); + EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN); + + ellipse_size = 0.9; + stat = checkLocalizationAccuracy(ellipse_size, warn_ellipse_size, error_ellipse_size); + EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN); + + ellipse_size = 1.0; + stat = checkLocalizationAccuracy(ellipse_size, warn_ellipse_size, error_ellipse_size); + EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::ERROR); +} + +TEST(TestLocalizationErrorMonitorDiagnostics, CheckLocalizationAccuracyLateralDirection) +{ + diagnostic_msgs::msg::DiagnosticStatus stat; + + const double warn_ellipse_size = 0.25; + const double error_ellipse_size = 0.3; + + double ellipse_size = 0.0; + stat = + checkLocalizationAccuracyLateralDirection(ellipse_size, warn_ellipse_size, error_ellipse_size); + EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK); + + ellipse_size = 0.24; + stat = + checkLocalizationAccuracyLateralDirection(ellipse_size, warn_ellipse_size, error_ellipse_size); + EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK); + + ellipse_size = 0.25; + stat = + checkLocalizationAccuracyLateralDirection(ellipse_size, warn_ellipse_size, error_ellipse_size); + EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN); + + ellipse_size = 0.29; + stat = + checkLocalizationAccuracyLateralDirection(ellipse_size, warn_ellipse_size, error_ellipse_size); + EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN); + + ellipse_size = 0.3; + stat = + checkLocalizationAccuracyLateralDirection(ellipse_size, warn_ellipse_size, error_ellipse_size); + EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::ERROR); +} + +TEST(TestLocalizationErrorMonitorDiagnostics, MergeDiagnosticStatus) +{ + diagnostic_msgs::msg::DiagnosticStatus merged_stat; + std::vector stat_array(2); + + stat_array.at(0).level = diagnostic_msgs::msg::DiagnosticStatus::OK; + stat_array.at(0).message = "OK"; + stat_array.at(1).level = diagnostic_msgs::msg::DiagnosticStatus::OK; + stat_array.at(1).message = "OK"; + merged_stat = mergeDiagnosticStatus(stat_array); + EXPECT_EQ(merged_stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK); + EXPECT_EQ(merged_stat.message, "OK"); + + stat_array.at(0).level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + stat_array.at(0).message = "WARN0"; + stat_array.at(1).level = diagnostic_msgs::msg::DiagnosticStatus::OK; + stat_array.at(1).message = "OK"; + merged_stat = mergeDiagnosticStatus(stat_array); + EXPECT_EQ(merged_stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN); + EXPECT_EQ(merged_stat.message, "WARN0"); + + stat_array.at(0).level = diagnostic_msgs::msg::DiagnosticStatus::OK; + stat_array.at(0).message = "OK"; + stat_array.at(1).level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + stat_array.at(1).message = "WARN1"; + merged_stat = mergeDiagnosticStatus(stat_array); + EXPECT_EQ(merged_stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN); + EXPECT_EQ(merged_stat.message, "WARN1"); + + stat_array.at(0).level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + stat_array.at(0).message = "WARN0"; + stat_array.at(1).level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + stat_array.at(1).message = "WARN1"; + merged_stat = mergeDiagnosticStatus(stat_array); + EXPECT_EQ(merged_stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN); + EXPECT_EQ(merged_stat.message, "WARN0; WARN1"); + + stat_array.at(0).level = diagnostic_msgs::msg::DiagnosticStatus::OK; + stat_array.at(0).message = "OK"; + stat_array.at(1).level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + stat_array.at(1).message = "ERROR1"; + merged_stat = mergeDiagnosticStatus(stat_array); + EXPECT_EQ(merged_stat.level, diagnostic_msgs::msg::DiagnosticStatus::ERROR); + EXPECT_EQ(merged_stat.message, "ERROR1"); + + stat_array.at(0).level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + stat_array.at(0).message = "WARN0"; + stat_array.at(1).level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + stat_array.at(1).message = "ERROR1"; + merged_stat = mergeDiagnosticStatus(stat_array); + EXPECT_EQ(merged_stat.level, diagnostic_msgs::msg::DiagnosticStatus::ERROR); + EXPECT_EQ(merged_stat.message, "WARN0; ERROR1"); + + stat_array.at(0).level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + stat_array.at(0).message = "ERROR0"; + stat_array.at(1).level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + stat_array.at(1).message = "ERROR1"; + merged_stat = mergeDiagnosticStatus(stat_array); + EXPECT_EQ(merged_stat.level, diagnostic_msgs::msg::DiagnosticStatus::ERROR); + EXPECT_EQ(merged_stat.message, "ERROR0; ERROR1"); +} diff --git a/localization/localization_util/CMakeLists.txt b/localization/localization_util/CMakeLists.txt new file mode 100644 index 0000000000000..9490ffb67723b --- /dev/null +++ b/localization/localization_util/CMakeLists.txt @@ -0,0 +1,15 @@ +cmake_minimum_required(VERSION 3.14) +project(localization_util) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +ament_auto_add_library(localization_util SHARED + src/util_func.cpp + src/pose_array_interpolator.cpp + src/tf2_listener_module.cpp +) + +ament_auto_package( + INSTALL_TO_SHARE +) diff --git a/localization/localization_util/README.md b/localization/localization_util/README.md new file mode 100644 index 0000000000000..a3e980464d0c6 --- /dev/null +++ b/localization/localization_util/README.md @@ -0,0 +1,5 @@ +# localization_util + +`localization_util`` is a localization utility package. + +This package does not have a node, it is just a library. diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/matrix_type.hpp b/localization/localization_util/include/localization_util/matrix_type.hpp similarity index 84% rename from localization/ndt_scan_matcher/include/ndt_scan_matcher/matrix_type.hpp rename to localization/localization_util/include/localization_util/matrix_type.hpp index 038ed4549eb2f..d9ec9b369127a 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/matrix_type.hpp +++ b/localization/localization_util/include/localization_util/matrix_type.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef NDT_SCAN_MATCHER__MATRIX_TYPE_HPP_ -#define NDT_SCAN_MATCHER__MATRIX_TYPE_HPP_ +#ifndef LOCALIZATION_UTIL__MATRIX_TYPE_HPP_ +#define LOCALIZATION_UTIL__MATRIX_TYPE_HPP_ #include using Matrix6d = Eigen::Matrix; using RowMatrixXd = Eigen::Matrix; -#endif // NDT_SCAN_MATCHER__MATRIX_TYPE_HPP_ +#endif // LOCALIZATION_UTIL__MATRIX_TYPE_HPP_ diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/pose_array_interpolator.hpp b/localization/localization_util/include/localization_util/pose_array_interpolator.hpp similarity index 90% rename from localization/ndt_scan_matcher/include/ndt_scan_matcher/pose_array_interpolator.hpp rename to localization/localization_util/include/localization_util/pose_array_interpolator.hpp index f1e7dfb3f544b..998d8465733f5 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/pose_array_interpolator.hpp +++ b/localization/localization_util/include/localization_util/pose_array_interpolator.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef NDT_SCAN_MATCHER__POSE_ARRAY_INTERPOLATOR_HPP_ -#define NDT_SCAN_MATCHER__POSE_ARRAY_INTERPOLATOR_HPP_ +#ifndef LOCALIZATION_UTIL__POSE_ARRAY_INTERPOLATOR_HPP_ +#define LOCALIZATION_UTIL__POSE_ARRAY_INTERPOLATOR_HPP_ -#include "ndt_scan_matcher/util_func.hpp" +#include "localization_util/util_func.hpp" #include @@ -59,4 +59,4 @@ class PoseArrayInterpolator const geometry_msgs::msg::Point & reference_point, const double distance_tolerance_m_) const; }; -#endif // NDT_SCAN_MATCHER__POSE_ARRAY_INTERPOLATOR_HPP_ +#endif // LOCALIZATION_UTIL__POSE_ARRAY_INTERPOLATOR_HPP_ diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/tf2_listener_module.hpp b/localization/localization_util/include/localization_util/tf2_listener_module.hpp similarity index 88% rename from localization/ndt_scan_matcher/include/ndt_scan_matcher/tf2_listener_module.hpp rename to localization/localization_util/include/localization_util/tf2_listener_module.hpp index 159acbd75ce3d..b332f9effe0e0 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/tf2_listener_module.hpp +++ b/localization/localization_util/include/localization_util/tf2_listener_module.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef NDT_SCAN_MATCHER__TF2_LISTENER_MODULE_HPP_ -#define NDT_SCAN_MATCHER__TF2_LISTENER_MODULE_HPP_ +#ifndef LOCALIZATION_UTIL__TF2_LISTENER_MODULE_HPP_ +#define LOCALIZATION_UTIL__TF2_LISTENER_MODULE_HPP_ #include @@ -40,4 +40,4 @@ class Tf2ListenerModule tf2_ros::TransformListener tf2_listener_; }; -#endif // NDT_SCAN_MATCHER__TF2_LISTENER_MODULE_HPP_ +#endif // LOCALIZATION_UTIL__TF2_LISTENER_MODULE_HPP_ diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/util_func.hpp b/localization/localization_util/include/localization_util/util_func.hpp similarity index 91% rename from localization/ndt_scan_matcher/include/ndt_scan_matcher/util_func.hpp rename to localization/localization_util/include/localization_util/util_func.hpp index 933375d5796f2..bd9a2b9305e25 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/util_func.hpp +++ b/localization/localization_util/include/localization_util/util_func.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef NDT_SCAN_MATCHER__UTIL_FUNC_HPP_ -#define NDT_SCAN_MATCHER__UTIL_FUNC_HPP_ +#ifndef LOCALIZATION_UTIL__UTIL_FUNC_HPP_ +#define LOCALIZATION_UTIL__UTIL_FUNC_HPP_ #include #include @@ -33,6 +33,7 @@ #include #include #include +#include #include // ref by http://takacity.blog.fc2.com/blog-entry-69.html @@ -86,4 +87,8 @@ T transform(const T & input, const geometry_msgs::msg::TransformStamped & transf double norm(const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2); -#endif // NDT_SCAN_MATCHER__UTIL_FUNC_HPP_ +void output_pose_with_cov_to_log( + const rclcpp::Logger logger, const std::string & prefix, + const geometry_msgs::msg::PoseWithCovarianceStamped & pose_with_cov); + +#endif // LOCALIZATION_UTIL__UTIL_FUNC_HPP_ diff --git a/localization/localization_util/package.xml b/localization/localization_util/package.xml new file mode 100644 index 0000000000000..20f9b160212b5 --- /dev/null +++ b/localization/localization_util/package.xml @@ -0,0 +1,40 @@ + + + + localization_util + 0.1.0 + The localization_util package + Yamato Ando + Masahiro Sakamoto + Shintaro Sakoda + Kento Yabuuchi + Apache License 2.0 + Yamato Ando + + ament_cmake_auto + autoware_cmake + + fmt + geometry_msgs + nav_msgs + rclcpp + sensor_msgs + std_msgs + std_srvs + tf2 + tf2_eigen + tf2_geometry_msgs + tf2_ros + tf2_sensor_msgs + tier4_autoware_utils + tier4_debug_msgs + tier4_localization_msgs + visualization_msgs + + ament_cmake_cppcheck + ament_lint_auto + + + ament_cmake + + diff --git a/localization/ndt_scan_matcher/src/pose_array_interpolator.cpp b/localization/localization_util/src/pose_array_interpolator.cpp similarity index 98% rename from localization/ndt_scan_matcher/src/pose_array_interpolator.cpp rename to localization/localization_util/src/pose_array_interpolator.cpp index 59ecedc18cb5a..ebc904fcf8d38 100644 --- a/localization/ndt_scan_matcher/src/pose_array_interpolator.cpp +++ b/localization/localization_util/src/pose_array_interpolator.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "ndt_scan_matcher/pose_array_interpolator.hpp" +#include "localization_util/pose_array_interpolator.hpp" PoseArrayInterpolator::PoseArrayInterpolator( rclcpp::Node * node, const rclcpp::Time & target_ros_time, @@ -51,6 +51,7 @@ PoseArrayInterpolator::PoseArrayInterpolator( // all validations must be true if (!(is_old_pose_valid && is_new_pose_valid && is_pose_diff_valid)) { + success_ = false; RCLCPP_WARN(logger_, "Validation error."); } } diff --git a/localization/ndt_scan_matcher/src/tf2_listener_module.cpp b/localization/localization_util/src/tf2_listener_module.cpp similarity index 97% rename from localization/ndt_scan_matcher/src/tf2_listener_module.cpp rename to localization/localization_util/src/tf2_listener_module.cpp index 364952a631f06..8a19ceec9f30d 100644 --- a/localization/ndt_scan_matcher/src/tf2_listener_module.cpp +++ b/localization/localization_util/src/tf2_listener_module.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "ndt_scan_matcher/tf2_listener_module.hpp" +#include "localization_util/tf2_listener_module.hpp" #include diff --git a/localization/ndt_scan_matcher/src/util_func.cpp b/localization/localization_util/src/util_func.cpp similarity index 89% rename from localization/ndt_scan_matcher/src/util_func.cpp rename to localization/localization_util/src/util_func.cpp index d947c56dfe183..865cc02cff293 100644 --- a/localization/ndt_scan_matcher/src/util_func.cpp +++ b/localization/localization_util/src/util_func.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "ndt_scan_matcher/util_func.hpp" +#include "localization_util/util_func.hpp" -#include "ndt_scan_matcher/matrix_type.hpp" +#include "localization_util/matrix_type.hpp" static std::random_device seed_gen; @@ -287,3 +287,24 @@ double norm(const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Poin const double dz = p1.z - p2.z; return std::sqrt(dx * dx + dy * dy + dz * dz); } + +void output_pose_with_cov_to_log( + const rclcpp::Logger logger, const std::string & prefix, + const geometry_msgs::msg::PoseWithCovarianceStamped & pose_with_cov) +{ + const Eigen::Map covariance = + make_eigen_covariance(pose_with_cov.pose.covariance); + const geometry_msgs::msg::Pose pose = pose_with_cov.pose.pose; + geometry_msgs::msg::Vector3 rpy = get_rpy(pose); + rpy.x = rpy.x * 180.0 / M_PI; + rpy.y = rpy.y * 180.0 / M_PI; + rpy.z = rpy.z * 180.0 / M_PI; + + RCLCPP_INFO_STREAM( + logger, std::fixed << prefix << "," << pose.position.x << "," << pose.position.y << "," + << pose.position.z << "," << pose.orientation.x << "," << pose.orientation.y + << "," << pose.orientation.z << "," << pose.orientation.w << "," << rpy.x + << "," << rpy.y << "," << rpy.z << "," << covariance(0, 0) << "," + << covariance(1, 1) << "," << covariance(2, 2) << "," << covariance(3, 3) + << "," << covariance(4, 4) << "," << covariance(5, 5)); +} diff --git a/localization/ndt_scan_matcher/CMakeLists.txt b/localization/ndt_scan_matcher/CMakeLists.txt index 1d5a9d5ac5320..79491a85a0a66 100644 --- a/localization/ndt_scan_matcher/CMakeLists.txt +++ b/localization/ndt_scan_matcher/CMakeLists.txt @@ -29,9 +29,6 @@ ament_auto_add_executable(ndt_scan_matcher src/debug.cpp src/ndt_scan_matcher_node.cpp src/ndt_scan_matcher_core.cpp - src/util_func.cpp - src/pose_array_interpolator.cpp - src/tf2_listener_module.cpp src/map_module.cpp src/map_update_module.cpp ) @@ -39,6 +36,13 @@ ament_auto_add_executable(ndt_scan_matcher link_directories(${PCL_LIBRARY_DIRS}) target_link_libraries(ndt_scan_matcher ${PCL_LIBRARIES}) +if(BUILD_TESTING) + add_ros_test( + test/test_ndt_scan_matcher_launch.py + TIMEOUT "30" + ) +endif() + ament_auto_package( INSTALL_TO_SHARE launch diff --git a/localization/ndt_scan_matcher/README.md b/localization/ndt_scan_matcher/README.md index b30cbaca1e87a..e210d3a28796f 100644 --- a/localization/ndt_scan_matcher/README.md +++ b/localization/ndt_scan_matcher/README.md @@ -38,6 +38,7 @@ One optional function is regularization. Please see the regularization chapter i | `transform_probability` | `tier4_debug_msgs::msg::Float32Stamped` | [debug topic] score of scan matching | | `no_ground_transform_probability` | `tier4_debug_msgs::msg::Float32Stamped` | [debug topic] score of scan matching based on de-grounded LiDAR scan | | `iteration_num` | `tier4_debug_msgs::msg::Int32Stamped` | [debug topic] number of scan matching iterations | +| `initial_to_result_relative_pose` | `geometry_msgs::msg::PoseStamped` | [debug topic] relative pose between the initial point and the convergence point | | `initial_to_result_distance` | `tier4_debug_msgs::msg::Float32Stamped` | [debug topic] distance difference between the initial point and the convergence point [m] | | `initial_to_result_distance_old` | `tier4_debug_msgs::msg::Float32Stamped` | [debug topic] distance difference between the older of the two initial points used in linear interpolation and the convergence point [m] | | `initial_to_result_distance_new` | `tier4_debug_msgs::msg::Float32Stamped` | [debug topic] distance difference between the newer of the two initial points used in linear interpolation and the convergence point [m] | @@ -54,17 +55,26 @@ One optional function is regularization. Please see the regularization chapter i ### Core Parameters -| Name | Type | Description | -| --------------------------------------- | ------ | ----------------------------------------------------------------------------------------------- | -| `base_frame` | string | Vehicle reference frame | -| `input_sensor_points_queue_size` | int | Subscriber queue size | -| `trans_epsilon` | double | The maximum difference between two consecutive transformations in order to consider convergence | -| `step_size` | double | The newton line search maximum step length | -| `resolution` | double | The ND voxel grid resolution [m] | -| `max_iterations` | int | The number of iterations required to calculate alignment | -| `converged_param_type` | int | The type of indicators for scan matching score (0: TP, 1: NVTL) | -| `converged_param_transform_probability` | double | Threshold for deciding whether to trust the estimation result | -| `num_threads` | int | Number of threads used for parallel computing | +| Name | Type | Description | +| --------------------------------------------------------- | ---------------------- | -------------------------------------------------------------------------------------------------- | +| `base_frame` | string | Vehicle reference frame | +| `ndt_base_frame` | string | NDT reference frame | +| `map_frame` | string | map frame | +| `input_sensor_points_queue_size` | int | Subscriber queue size | +| `trans_epsilon` | double | The max difference between two consecutive transformations to consider convergence | +| `step_size` | double | The newton line search maximum step length | +| `resolution` | double | The ND voxel grid resolution [m] | +| `max_iterations` | int | The number of iterations required to calculate alignment | +| `converged_param_type` | int | The type of indicators for scan matching score (0: TP, 1: NVTL) | +| `converged_param_transform_probability` | double | TP threshold for deciding whether to trust the estimation result (when converged_param_type = 0) | +| `converged_param_nearest_voxel_transformation_likelihood` | double | NVTL threshold for deciding whether to trust the estimation result (when converged_param_type = 1) | +| `initial_estimate_particles_num` | int | The number of particles to estimate initial pose | +| `n_startup_trials` | int | The number of initial random trials in the TPE (Tree-Structured Parzen Estimator). | +| `lidar_topic_timeout_sec` | double | Tolerance of timestamp difference between current time and sensor pointcloud | +| `initial_pose_timeout_sec` | int | Tolerance of timestamp difference between initial_pose and sensor pointcloud. [sec] | +| `initial_pose_distance_tolerance_m` | double | Tolerance of distance difference between two initial poses used for linear interpolation. [m] | +| `num_threads` | int | Number of threads used for parallel computing | +| `output_pose_covariance` | std::array | The covariance of output pose | (TP: Transform Probability, NVTL: Nearest Voxel Transform Probability) @@ -236,7 +246,7 @@ Here is a split PCD map for `sample-map-rosbag` from Autoware tutorial: [`sample ### Abstract -This is a function that using de-grounded LiDAR scan estimate scan matching score.This score can more accurately reflect the current localization performance. +This is a function that uses de-grounded LiDAR scan to estimate the scan matching score. This score can reflect the current localization performance more accurately. [related issue](https://github.com/autowarefoundation/autoware.universe/issues/2044). ### Parameters diff --git a/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml b/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml index a5d8142b6616e..0ba1b1a2e15f4 100644 --- a/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml +++ b/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml @@ -6,6 +6,12 @@ # Vehicle reference frame base_frame: "base_link" + # NDT reference frame + ndt_base_frame: "ndt_base_link" + + # map frame + map_frame: "map" + # Subscriber queue size input_sensor_points_queue_size: 1 @@ -35,7 +41,15 @@ converged_param_nearest_voxel_transformation_likelihood: 2.3 # The number of particles to estimate initial pose - initial_estimate_particles_num: 100 + initial_estimate_particles_num: 200 + + # The number of initial random trials in the TPE (Tree-Structured Parzen Estimator). + # This value should be equal to or less than 'initial_estimate_particles_num' and more than 0. + # If it is equal to 'initial_estimate_particles_num', the search will be the same as a full random search. + n_startup_trials: 20 + + # Tolerance of timestamp difference between current time and sensor pointcloud. [sec] + lidar_topic_timeout_sec: 1.0 # Tolerance of timestamp difference between initial_pose and sensor pointcloud. [sec] initial_pose_timeout_sec: 1.0 @@ -47,7 +61,7 @@ num_threads: 4 # The covariance of output pose - # Do note that this covariance matrix is empirically derived + # Note that this covariance matrix is empirically derived output_pose_covariance: [ 0.0225, 0.0, 0.0, 0.0, 0.0, 0.0, @@ -61,7 +75,7 @@ # Regularization switch regularization_enabled: false - # regularization scale factor + # Regularization scale factor regularization_scale_factor: 0.01 # Dynamic map loading distance @@ -79,3 +93,6 @@ # If lidar_point.z - base_link.z <= this threshold , the point will be removed z_margin_for_ground_removal: 0.8 + + # The execution time which means probably NDT cannot matches scans properly. [ms] + critical_upper_bound_exe_time_ms: 100 diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp index 4f630bb8c5898..13721c03ca22e 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp @@ -15,10 +15,10 @@ #ifndef NDT_SCAN_MATCHER__MAP_UPDATE_MODULE_HPP_ #define NDT_SCAN_MATCHER__MAP_UPDATE_MODULE_HPP_ +#include "localization_util/tf2_listener_module.hpp" +#include "localization_util/util_func.hpp" #include "ndt_scan_matcher/debug.hpp" #include "ndt_scan_matcher/particle.hpp" -#include "ndt_scan_matcher/tf2_listener_module.hpp" -#include "ndt_scan_matcher/util_func.hpp" #include #include diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp index 8657e354c728a..e5f46b5022b51 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp @@ -17,9 +17,9 @@ #define FMT_HEADER_ONLY +#include "localization_util/tf2_listener_module.hpp" #include "ndt_scan_matcher/map_module.hpp" #include "ndt_scan_matcher/map_update_module.hpp" -#include "ndt_scan_matcher/tf2_listener_module.hpp" #include @@ -92,8 +92,7 @@ class NDTScanMatcher : public rclcpp::Node void callback_regularization_pose( geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr pose_conv_msg_ptr); - geometry_msgs::msg::PoseWithCovarianceStamped align_using_monte_carlo( - const std::shared_ptr & ndt_ptr, + geometry_msgs::msg::PoseWithCovarianceStamped align_pose( const geometry_msgs::msg::PoseWithCovarianceStamped & initial_pose_with_cov); void transform_sensor_measurement( @@ -111,7 +110,7 @@ class NDTScanMatcher : public rclcpp::Node const pcl::shared_ptr> & sensor_points_in_map_ptr); void publish_marker( const rclcpp::Time & sensor_ros_time, const std::vector & pose_array); - void publish_initial_to_result_distances( + void publish_initial_to_result( const rclcpp::Time & sensor_ros_time, const geometry_msgs::msg::Pose & result_pose_msg, const geometry_msgs::msg::PoseWithCovarianceStamped & initial_pose_cov_msg, const geometry_msgs::msg::PoseWithCovarianceStamped & initial_pose_old_msg, @@ -150,6 +149,8 @@ class NDTScanMatcher : public rclcpp::Node rclcpp::Publisher::SharedPtr no_ground_nearest_voxel_transformation_likelihood_pub_; rclcpp::Publisher::SharedPtr iteration_num_pub_; + rclcpp::Publisher::SharedPtr + initial_to_result_relative_pose_pub_; rclcpp::Publisher::SharedPtr initial_to_result_distance_pub_; rclcpp::Publisher::SharedPtr @@ -179,6 +180,8 @@ class NDTScanMatcher : public rclcpp::Node double converged_param_nearest_voxel_transformation_likelihood_; int initial_estimate_particles_num_; + int n_startup_trials_; + double lidar_topic_timeout_sec_; double initial_pose_timeout_sec_; double initial_pose_distance_tolerance_m_; float inversion_vector_threshold_; @@ -207,6 +210,9 @@ class NDTScanMatcher : public rclcpp::Node double z_margin_for_ground_removal_; bool use_dynamic_map_loading_; + + // The execution time which means probably NDT cannot matches scans properly + int critical_upper_bound_exe_time_ms_; }; #endif // NDT_SCAN_MATCHER__NDT_SCAN_MATCHER_CORE_HPP_ diff --git a/localization/ndt_scan_matcher/package.xml b/localization/ndt_scan_matcher/package.xml index 92c690a708492..a9083c4ae0ae4 100644 --- a/localization/ndt_scan_matcher/package.xml +++ b/localization/ndt_scan_matcher/package.xml @@ -19,6 +19,7 @@ fmt geometry_msgs libpcl-all-dev + localization_util nav_msgs ndt_omp pcl_conversions @@ -34,10 +35,12 @@ tier4_autoware_utils tier4_debug_msgs tier4_localization_msgs + tree_structured_parzen_estimator visualization_msgs ament_cmake_cppcheck ament_lint_auto + ros_testing ament_cmake diff --git a/localization/ndt_scan_matcher/src/debug.cpp b/localization/ndt_scan_matcher/src/debug.cpp index 9c82e06d89b80..941f682de5803 100644 --- a/localization/ndt_scan_matcher/src/debug.cpp +++ b/localization/ndt_scan_matcher/src/debug.cpp @@ -14,7 +14,7 @@ #include "ndt_scan_matcher/debug.hpp" -#include "ndt_scan_matcher/util_func.hpp" +#include "localization_util/util_func.hpp" visualization_msgs::msg::MarkerArray make_debug_markers( const builtin_interfaces::msg::Time & stamp, const std::string & map_frame_, diff --git a/localization/ndt_scan_matcher/src/map_update_module.cpp b/localization/ndt_scan_matcher/src/map_update_module.cpp index 9c94467dc94c0..7512ae2e77a21 100644 --- a/localization/ndt_scan_matcher/src/map_update_module.cpp +++ b/localization/ndt_scan_matcher/src/map_update_module.cpp @@ -55,12 +55,6 @@ MapUpdateModule::MapUpdateModule( pcd_loader_client_ = node->create_client("pcd_loader_service"); - while (!pcd_loader_client_->wait_for_service(std::chrono::seconds(1)) && rclcpp::ok()) { - RCLCPP_INFO( - logger_, - "Waiting for pcd loader service. Check if the enable_differential_load in " - "pointcloud_map_loader is set `true`."); - } double map_update_dt = 1.0; auto period_ns = std::chrono::duration_cast( @@ -117,7 +111,14 @@ void MapUpdateModule::update_map(const geometry_msgs::msg::Point & position) request->area.radius = static_cast(dynamic_map_loading_map_radius_); request->cached_ids = ndt_ptr_->getCurrentMapIDs(); - // // send a request to map_loader + while (!pcd_loader_client_->wait_for_service(std::chrono::seconds(1)) && rclcpp::ok()) { + RCLCPP_INFO( + logger_, + "Waiting for pcd loader service. Check if the enable_differential_load in " + "pointcloud_map_loader is set `true`."); + } + + // send a request to map_loader auto result{pcd_loader_client_->async_send_request( request, [](rclcpp::Client::SharedFuture) {})}; diff --git a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp index 667fd59196df3..b38e3f0826184 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -14,14 +14,17 @@ #include "ndt_scan_matcher/ndt_scan_matcher_core.hpp" -#include "ndt_scan_matcher/matrix_type.hpp" +#include "localization_util/matrix_type.hpp" +#include "localization_util/pose_array_interpolator.hpp" +#include "localization_util/util_func.hpp" #include "ndt_scan_matcher/particle.hpp" -#include "ndt_scan_matcher/pose_array_interpolator.hpp" -#include "ndt_scan_matcher/util_func.hpp" +#include "tree_structured_parzen_estimator/tree_structured_parzen_estimator.hpp" #include #include +#include + #include #ifdef ROS_DISTRO_GALACTIC @@ -84,43 +87,32 @@ NDTScanMatcher::NDTScanMatcher() tf2_broadcaster_(*this), ndt_ptr_(new NormalDistributionsTransform), state_ptr_(new std::map), - base_frame_("base_link"), - ndt_base_frame_("ndt_base_link"), - map_frame_("map"), - converged_param_type_(ConvergedParamType::TRANSFORM_PROBABILITY), - converged_param_transform_probability_(4.5), - converged_param_nearest_voxel_transformation_likelihood_(2.3), - initial_estimate_particles_num_(100), - initial_pose_timeout_sec_(1.0), - initial_pose_distance_tolerance_m_(10.0), - inversion_vector_threshold_(-0.9), - oscillation_threshold_(10), - output_pose_covariance_(), - regularization_enabled_(declare_parameter("regularization_enabled", false)), - estimate_scores_for_degrounded_scan_( - declare_parameter("estimate_scores_for_degrounded_scan", false)), - z_margin_for_ground_removal_(declare_parameter("z_margin_for_ground_removal", 0.8)) + inversion_vector_threshold_(-0.9), // Not necessary to extract to ndt_scan_matcher.param.yaml + oscillation_threshold_(10), // Not necessary to extract to ndt_scan_matcher.param.yaml + regularization_enabled_(declare_parameter("regularization_enabled")) { (*state_ptr_)["state"] = "Initializing"; is_activated_ = false; - int points_queue_size = - static_cast(this->declare_parameter("input_sensor_points_queue_size", 0)); + int points_queue_size = this->declare_parameter("input_sensor_points_queue_size"); points_queue_size = std::max(points_queue_size, 0); RCLCPP_INFO(get_logger(), "points_queue_size: %d", points_queue_size); - base_frame_ = this->declare_parameter("base_frame", base_frame_); + base_frame_ = this->declare_parameter("base_frame"); RCLCPP_INFO(get_logger(), "base_frame_id: %s", base_frame_.c_str()); - ndt_base_frame_ = this->declare_parameter("ndt_base_frame", ndt_base_frame_); + ndt_base_frame_ = this->declare_parameter("ndt_base_frame"); RCLCPP_INFO(get_logger(), "ndt_base_frame_id: %s", ndt_base_frame_.c_str()); + map_frame_ = this->declare_parameter("map_frame"); + RCLCPP_INFO(get_logger(), "map_frame_id: %s", map_frame_.c_str()); + pclomp::NdtParams ndt_params{}; ndt_params.trans_epsilon = this->declare_parameter("trans_epsilon"); ndt_params.step_size = this->declare_parameter("step_size"); ndt_params.resolution = this->declare_parameter("resolution"); - ndt_params.max_iterations = static_cast(this->declare_parameter("max_iterations")); - ndt_params.num_threads = static_cast(this->declare_parameter("num_threads")); + ndt_params.max_iterations = this->declare_parameter("max_iterations"); + ndt_params.num_threads = this->declare_parameter("num_threads"); ndt_params.num_threads = std::max(ndt_params.num_threads, 1); ndt_params.regularization_scale_factor = static_cast(this->declare_parameter("regularization_scale_factor")); @@ -131,21 +123,23 @@ NDTScanMatcher::NDTScanMatcher() ndt_params.trans_epsilon, ndt_params.step_size, ndt_params.resolution, ndt_params.max_iterations); - int converged_param_type_tmp = - static_cast(this->declare_parameter("converged_param_type", 0)); + int converged_param_type_tmp = this->declare_parameter("converged_param_type"); converged_param_type_ = static_cast(converged_param_type_tmp); - converged_param_transform_probability_ = this->declare_parameter( - "converged_param_transform_probability", converged_param_transform_probability_); - converged_param_nearest_voxel_transformation_likelihood_ = this->declare_parameter( - "converged_param_nearest_voxel_transformation_likelihood", - converged_param_nearest_voxel_transformation_likelihood_); + converged_param_transform_probability_ = + this->declare_parameter("converged_param_transform_probability"); + converged_param_nearest_voxel_transformation_likelihood_ = + this->declare_parameter("converged_param_nearest_voxel_transformation_likelihood"); + + lidar_topic_timeout_sec_ = this->declare_parameter("lidar_topic_timeout_sec"); - initial_pose_timeout_sec_ = - this->declare_parameter("initial_pose_timeout_sec", initial_pose_timeout_sec_); + critical_upper_bound_exe_time_ms_ = + this->declare_parameter("critical_upper_bound_exe_time_ms"); - initial_pose_distance_tolerance_m_ = this->declare_parameter( - "initial_pose_distance_tolerance_m", initial_pose_distance_tolerance_m_); + initial_pose_timeout_sec_ = this->declare_parameter("initial_pose_timeout_sec"); + + initial_pose_distance_tolerance_m_ = + this->declare_parameter("initial_pose_distance_tolerance_m"); std::vector output_pose_covariance = this->declare_parameter>("output_pose_covariance"); @@ -153,8 +147,13 @@ NDTScanMatcher::NDTScanMatcher() output_pose_covariance_[i] = output_pose_covariance[i]; } - initial_estimate_particles_num_ = static_cast( - this->declare_parameter("initial_estimate_particles_num", initial_estimate_particles_num_)); + initial_estimate_particles_num_ = this->declare_parameter("initial_estimate_particles_num"); + n_startup_trials_ = this->declare_parameter("n_startup_trials"); + + estimate_scores_for_degrounded_scan_ = + this->declare_parameter("estimate_scores_for_degrounded_scan"); + + z_margin_for_ground_removal_ = this->declare_parameter("z_margin_for_ground_removal"); rclcpp::CallbackGroup::SharedPtr initial_pose_callback_group; initial_pose_callback_group = @@ -206,6 +205,8 @@ NDTScanMatcher::NDTScanMatcher() "no_ground_nearest_voxel_transformation_likelihood", 10); iteration_num_pub_ = this->create_publisher("iteration_num", 10); + initial_to_result_relative_pose_pub_ = + this->create_publisher("initial_to_result_relative_pose", 10); initial_to_result_distance_pub_ = this->create_publisher("initial_to_result_distance", 10); initial_to_result_distance_old_pub_ = @@ -268,9 +269,16 @@ void NDTScanMatcher::timer_diagnostic() diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; diag_status_msg.message += "Initializing State. "; } + if ( + state_ptr_->count("lidar_topic_delay_time_sec") && + std::stod((*state_ptr_)["lidar_topic_delay_time_sec"]) > lidar_topic_timeout_sec_) { + diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + diag_status_msg.message += "lidar_topic_delay_time_sec exceed limit. "; + } if ( state_ptr_->count("skipping_publish_num") && - std::stoi((*state_ptr_)["skipping_publish_num"]) > 1) { + std::stoi((*state_ptr_)["skipping_publish_num"]) > 1 && + std::stoi((*state_ptr_)["skipping_publish_num"]) < 5) { diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; diag_status_msg.message += "skipping_publish_num > 1. "; } @@ -280,6 +288,20 @@ void NDTScanMatcher::timer_diagnostic() diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; diag_status_msg.message += "skipping_publish_num exceed limit. "; } + if ( + state_ptr_->count("nearest_voxel_transformation_likelihood") && + std::stod((*state_ptr_)["nearest_voxel_transformation_likelihood"]) < + converged_param_nearest_voxel_transformation_likelihood_) { + diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + diag_status_msg.message += "NDT score is unreliably low. "; + } + if ( + state_ptr_->count("execution_time") && + std::stod((*state_ptr_)["execution_time"]) >= critical_upper_bound_exe_time_ms_) { + diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + diag_status_msg.message += + "NDT exe time is too long. (took " + (*state_ptr_)["execution_time"] + " [ms])"; + } // Ignore local optimal solution if ( state_ptr_->count("is_local_optimal_solution_oscillation") && @@ -346,11 +368,29 @@ void NDTScanMatcher::callback_sensor_points( return; } + const rclcpp::Time sensor_ros_time = sensor_points_msg_in_sensor_frame->header.stamp; + const double lidar_topic_delay_time_sec = (this->now() - sensor_ros_time).seconds(); + (*state_ptr_)["lidar_topic_delay_time_sec"] = std::to_string(lidar_topic_delay_time_sec); + + if (lidar_topic_delay_time_sec > lidar_topic_timeout_sec_) { + RCLCPP_WARN( + this->get_logger(), + "The LiDAR topic is experiencing latency. The delay time is %lf[sec] (the tolerance is " + "%lf[sec])", + lidar_topic_delay_time_sec, lidar_topic_timeout_sec_); + + // If the delay time of the LiDAR topic exceeds the delay compensation time of ekf_localizer, + // even if further processing continues, the estimated result will be rejected by ekf_localizer. + // Therefore, it would be acceptable to exit the function here. + // However, for now, we will continue the processing as it is. + + // return; + } + // mutex ndt_ptr_ std::lock_guard lock(ndt_ptr_mtx_); const auto exe_start_time = std::chrono::system_clock::now(); - const rclcpp::Time sensor_ros_time = sensor_points_msg_in_sensor_frame->header.stamp; // preprocess input pointcloud pcl::shared_ptr> sensor_points_in_sensor_frame( @@ -408,20 +448,8 @@ void NDTScanMatcher::callback_sensor_points( } // perform several validations - /***************************************************************************** - The reason the add 2 to the ndt_ptr_->getMaximumIterations() is that there are bugs in - implementation of ndt. - 1. gradient descent method ends when the iteration is greater than max_iteration if it does not - converge (be careful it's 'greater than' instead of 'greater equal than'.) - https://github.com/tier4/autoware.iv/blob/2323e5baa0b680d43a9219f5fb3b7a11dd9edc82/localization/pose_estimator/ndt_scan_matcher/ndt_omp/include/ndt_omp/ndt_omp_impl.hpp#L212 - 2. iterate iteration count when end of gradient descent function. - https://github.com/tier4/autoware.iv/blob/2323e5baa0b680d43a9219f5fb3b7a11dd9edc82/localization/pose_estimator/ndt_scan_matcher/ndt_omp/include/ndt_omp/ndt_omp_impl.hpp#L217 - - These bugs are now resolved in original pcl implementation. - https://github.com/PointCloudLibrary/pcl/blob/424c1c6a0ca97d94ca63e5daff4b183a4db8aae4/registration/include/pcl/registration/impl/ndt.hpp#L73-L180 - *****************************************************************************/ bool is_ok_iteration_num = - validate_num_iteration(ndt_result.iteration_num, ndt_ptr_->getMaximumIterations() + 2); + validate_num_iteration(ndt_result.iteration_num, ndt_ptr_->getMaximumIterations()); bool is_local_optimal_solution_oscillation = false; if (!is_ok_iteration_num) { is_local_optimal_solution_oscillation = validate_local_optimal_solution_oscillation( @@ -449,7 +477,7 @@ void NDTScanMatcher::callback_sensor_points( publish_tf(sensor_ros_time, result_pose_msg); publish_pose(sensor_ros_time, result_pose_msg, is_converged); publish_marker(sensor_ros_time, transformation_msg_array); - publish_initial_to_result_distances( + publish_initial_to_result( sensor_ros_time, result_pose_msg, interpolator.get_current_pose(), interpolator.get_old_pose(), interpolator.get_new_pose()); @@ -498,6 +526,7 @@ void NDTScanMatcher::callback_sensor_points( } else { (*state_ptr_)["is_local_optimal_solution_oscillation"] = "0"; } + (*state_ptr_)["execution_time"] = std::to_string(exe_time); } void NDTScanMatcher::transform_sensor_measurement( @@ -589,12 +618,19 @@ void NDTScanMatcher::publish_marker( ndt_marker_pub_->publish(marker_array); } -void NDTScanMatcher::publish_initial_to_result_distances( +void NDTScanMatcher::publish_initial_to_result( const rclcpp::Time & sensor_ros_time, const geometry_msgs::msg::Pose & result_pose_msg, const geometry_msgs::msg::PoseWithCovarianceStamped & initial_pose_cov_msg, const geometry_msgs::msg::PoseWithCovarianceStamped & initial_pose_old_msg, const geometry_msgs::msg::PoseWithCovarianceStamped & initial_pose_new_msg) { + geometry_msgs::msg::PoseStamped initial_to_result_relative_pose_stamped; + initial_to_result_relative_pose_stamped.pose = + tier4_autoware_utils::inverseTransformPose(result_pose_msg, initial_pose_cov_msg.pose.pose); + initial_to_result_relative_pose_stamped.header.stamp = sensor_ros_time; + initial_to_result_relative_pose_stamped.header.frame_id = map_frame_; + initial_to_result_relative_pose_pub_->publish(initial_to_result_relative_pose_stamped); + const auto initial_to_result_distance = static_cast(norm(initial_pose_cov_msg.pose.pose.position, result_pose_msg.position)); initial_to_result_distance_pub_->publish( @@ -716,49 +752,95 @@ void NDTScanMatcher::service_ndt_align( map_update_module_->update_map(initial_pose_msg_in_map_frame.pose.pose.position); } + // mutex Map + std::lock_guard lock(ndt_ptr_mtx_); + if (ndt_ptr_->getInputTarget() == nullptr) { res->success = false; - RCLCPP_WARN(get_logger(), "No InputTarget"); + RCLCPP_WARN( + get_logger(), "No InputTarget. Please check the map file and the map_loader service"); return; } if (ndt_ptr_->getInputSource() == nullptr) { res->success = false; - RCLCPP_WARN(get_logger(), "No InputSource"); + RCLCPP_WARN(get_logger(), "No InputSource. Please check the input lidar topic"); return; } - // mutex Map - std::lock_guard lock(ndt_ptr_mtx_); - (*state_ptr_)["state"] = "Aligning"; - res->pose_with_covariance = align_using_monte_carlo(ndt_ptr_, initial_pose_msg_in_map_frame); + res->pose_with_covariance = align_pose(initial_pose_msg_in_map_frame); (*state_ptr_)["state"] = "Sleeping"; res->success = true; res->pose_with_covariance.pose.covariance = req->pose_with_covariance.pose.covariance; } -geometry_msgs::msg::PoseWithCovarianceStamped NDTScanMatcher::align_using_monte_carlo( - const std::shared_ptr & ndt_ptr, +geometry_msgs::msg::PoseWithCovarianceStamped NDTScanMatcher::align_pose( const geometry_msgs::msg::PoseWithCovarianceStamped & initial_pose_with_cov) { - if (ndt_ptr->getInputTarget() == nullptr || ndt_ptr->getInputSource() == nullptr) { - RCLCPP_WARN(get_logger(), "No Map or Sensor PointCloud"); - return geometry_msgs::msg::PoseWithCovarianceStamped(); - } - - // generateParticle - const auto initial_poses = - create_random_pose_array(initial_pose_with_cov, initial_estimate_particles_num_); + output_pose_with_cov_to_log(get_logger(), "align_pose_input", initial_pose_with_cov); + + const auto base_rpy = get_rpy(initial_pose_with_cov); + const Eigen::Map covariance = { + initial_pose_with_cov.pose.covariance.data(), 6, 6}; + const double stddev_x = std::sqrt(covariance(0, 0)); + const double stddev_y = std::sqrt(covariance(1, 1)); + const double stddev_z = std::sqrt(covariance(2, 2)); + const double stddev_roll = std::sqrt(covariance(3, 3)); + const double stddev_pitch = std::sqrt(covariance(4, 4)); + + // Let phi be the cumulative distribution function of the standard normal distribution. + // It has the following relationship with the error function (erf). + // phi(x) = 1/2 (1 + erf(x / sqrt(2))) + // so, 2 * phi(x) - 1 = erf(x / sqrt(2)). + // The range taken by 2 * phi(x) - 1 is [-1, 1], so it can be used as a uniform distribution in + // TPE. Let u = 2 * phi(x) - 1, then x = sqrt(2) * erf_inv(u). Computationally, it is not a good + // to give erf_inv -1 and 1, so it is rounded off at (-1 + eps, 1 - eps). + const double SQRT2 = std::sqrt(2); + auto uniform_to_normal = [&SQRT2](const double uniform) { + assert(-1.0 <= uniform && uniform <= 1.0); + constexpr double epsilon = 1.0e-6; + const double clamped = std::clamp(uniform, -1.0 + epsilon, 1.0 - epsilon); + return boost::math::erf_inv(clamped) * SQRT2; + }; + + auto normal_to_uniform = [&SQRT2](const double normal) { + return boost::math::erf(normal / SQRT2); + }; + + // Optimizing (x, y, z, roll, pitch, yaw) 6 dimensions. + // The last dimension (yaw) is a loop variable. + // Although roll and pitch are also angles, they are considered non-looping variables that follow + // a normal distribution with a small standard deviation. This assumes that the initial pose of + // the ego vehicle is aligned with the ground to some extent about roll and pitch. + const std::vector is_loop_variable = {false, false, false, false, false, true}; + TreeStructuredParzenEstimator tpe( + TreeStructuredParzenEstimator::Direction::MAXIMIZE, n_startup_trials_, is_loop_variable); std::vector particle_array; auto output_cloud = std::make_shared>(); - for (unsigned int i = 0; i < initial_poses.size(); i++) { - const auto & initial_pose = initial_poses[i]; + for (int i = 0; i < initial_estimate_particles_num_; i++) { + const TreeStructuredParzenEstimator::Input input = tpe.get_next_input(); + + geometry_msgs::msg::Pose initial_pose; + initial_pose.position.x = + initial_pose_with_cov.pose.pose.position.x + uniform_to_normal(input[0]) * stddev_x; + initial_pose.position.y = + initial_pose_with_cov.pose.pose.position.y + uniform_to_normal(input[1]) * stddev_y; + initial_pose.position.z = + initial_pose_with_cov.pose.pose.position.z + uniform_to_normal(input[2]) * stddev_z; + geometry_msgs::msg::Vector3 init_rpy; + init_rpy.x = base_rpy.x + uniform_to_normal(input[3]) * stddev_roll; + init_rpy.y = base_rpy.y + uniform_to_normal(input[4]) * stddev_pitch; + init_rpy.z = base_rpy.z + input[5] * M_PI; + tf2::Quaternion tf_quaternion; + tf_quaternion.setRPY(init_rpy.x, init_rpy.y, init_rpy.z); + initial_pose.orientation = tf2::toMsg(tf_quaternion); + const Eigen::Matrix4f initial_pose_matrix = pose_to_matrix4f(initial_pose); - ndt_ptr->align(*output_cloud, initial_pose_matrix); - const pclomp::NdtResult ndt_result = ndt_ptr->getResult(); + ndt_ptr_->align(*output_cloud, initial_pose_matrix); + const pclomp::NdtResult ndt_result = ndt_ptr_->getResult(); Particle particle( initial_pose, matrix4f_to_pose(ndt_result.pose), ndt_result.transform_probability, @@ -769,9 +851,30 @@ geometry_msgs::msg::PoseWithCovarianceStamped NDTScanMatcher::align_using_monte_ particle, i); ndt_monte_carlo_initial_pose_marker_pub_->publish(marker_array); + const geometry_msgs::msg::Pose pose = matrix4f_to_pose(ndt_result.pose); + const geometry_msgs::msg::Vector3 rpy = get_rpy(pose); + + const double diff_x = pose.position.x - initial_pose_with_cov.pose.pose.position.x; + const double diff_y = pose.position.y - initial_pose_with_cov.pose.pose.position.y; + const double diff_z = pose.position.z - initial_pose_with_cov.pose.pose.position.z; + const double diff_roll = rpy.x - base_rpy.x; + const double diff_pitch = rpy.y - base_rpy.y; + const double diff_yaw = rpy.z - base_rpy.z; + + // Only yaw is a loop_variable, so only simple normalization is performed. + // All other variables are converted from normal distribution to uniform distribution. + TreeStructuredParzenEstimator::Input result(is_loop_variable.size()); + result[0] = normal_to_uniform(diff_x / stddev_x); + result[1] = normal_to_uniform(diff_y / stddev_y); + result[2] = normal_to_uniform(diff_z / stddev_z); + result[3] = normal_to_uniform(diff_roll / stddev_roll); + result[4] = normal_to_uniform(diff_pitch / stddev_pitch); + result[5] = diff_yaw / M_PI; + tpe.add_trial(TreeStructuredParzenEstimator::Trial{result, ndt_result.transform_probability}); + auto sensor_points_in_map_ptr = std::make_shared>(); tier4_autoware_utils::transformPointCloud( - *ndt_ptr->getInputSource(), *sensor_points_in_map_ptr, ndt_result.pose); + *ndt_ptr_->getInputSource(), *sensor_points_in_map_ptr, ndt_result.pose); publish_point_cloud(initial_pose_with_cov.header.stamp, map_frame_, sensor_points_in_map_ptr); } @@ -784,5 +887,8 @@ geometry_msgs::msg::PoseWithCovarianceStamped NDTScanMatcher::align_using_monte_ result_pose_with_cov_msg.header.frame_id = map_frame_; result_pose_with_cov_msg.pose.pose = best_particle_ptr->result_pose; + output_pose_with_cov_to_log(get_logger(), "align_pose_output", result_pose_with_cov_msg); + RCLCPP_INFO_STREAM(get_logger(), "best_score," << best_particle_ptr->score); + return result_pose_with_cov_msg; } diff --git a/localization/ndt_scan_matcher/test/test_ndt_scan_matcher_launch.py b/localization/ndt_scan_matcher/test/test_ndt_scan_matcher_launch.py new file mode 100644 index 0000000000000..2dc4958c4704f --- /dev/null +++ b/localization/ndt_scan_matcher/test/test_ndt_scan_matcher_launch.py @@ -0,0 +1,101 @@ +#!/usr/bin/env python3 + +# Copyright 2023 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 os +import unittest + +from ament_index_python import get_package_share_directory +import launch +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import AnyLaunchDescriptionSource +from launch.logging import get_logger +import launch_testing +import pytest +import rclpy +from std_srvs.srv import SetBool + +logger = get_logger(__name__) + + +@pytest.mark.launch_test +def generate_test_description(): + test_ndt_scan_matcher_launch_file = os.path.join( + get_package_share_directory("ndt_scan_matcher"), + "launch", + "ndt_scan_matcher.launch.xml", + ) + ndt_scan_matcher = IncludeLaunchDescription( + AnyLaunchDescriptionSource(test_ndt_scan_matcher_launch_file), + ) + + return launch.LaunchDescription( + [ + ndt_scan_matcher, + # Start tests right away - no need to wait for anything + launch_testing.actions.ReadyToTest(), + ] + ) + + +class TestNDTScanMatcher(unittest.TestCase): + @classmethod + def setUpClass(cls): + # Initialize the ROS context for the test node + rclpy.init() + + @classmethod + def tearDownClass(cls): + # Shutdown the ROS context + rclpy.shutdown() + + def setUp(self): + # Create a ROS node for tests + self.test_node = rclpy.create_node("test_node") + + def tearDown(self): + self.test_node.destroy_node() + + @staticmethod + def print_message(stat): + logger.debug("===========================") + logger.debug(stat) + + def test_launch(self): + # Trigger ndt_scan_matcher to activate the node + cli_trigger = self.test_node.create_client(SetBool, "/trigger_node") + while not cli_trigger.wait_for_service(timeout_sec=1.0): + continue + + request = SetBool.Request() + request.data = True + future = cli_trigger.call_async(request) + rclpy.spin_until_future_complete(self.test_node, future) + + if future.result() is not None: + self.test_node.get_logger().info("Result of bool service: %s" % future.result().message) + self.assertTrue(future.result().success, "ndt_scan_matcher is not activated") + else: + self.test_node.get_logger().error( + "Exception while calling service: %r" % future.exception() + ) + raise self.failureException("service trigger failed") + + +@launch_testing.post_shutdown_test() +class TestProcessOutput(unittest.TestCase): + def test_exit_code(self, proc_info): + # Check that process exits with code 0: no error + launch_testing.asserts.assertExitCodes(proc_info) diff --git a/localization/stop_filter/CMakeLists.txt b/localization/stop_filter/CMakeLists.txt index 2d1867b8cd0bc..97a0443195ae5 100644 --- a/localization/stop_filter/CMakeLists.txt +++ b/localization/stop_filter/CMakeLists.txt @@ -13,4 +13,5 @@ ament_target_dependencies(stop_filter) ament_auto_package( INSTALL_TO_SHARE launch + config ) diff --git a/localization/stop_filter/config/stop_filter.param.yaml b/localization/stop_filter/config/stop_filter.param.yaml new file mode 100644 index 0000000000000..ded090b75b5bd --- /dev/null +++ b/localization/stop_filter/config/stop_filter.param.yaml @@ -0,0 +1,4 @@ +/**: + ros__parameters: + vx_threshold: 0.1 # [m/s] + wz_threshold: 0.02 # [rad/s] diff --git a/localization/stop_filter/launch/stop_filter.launch.xml b/localization/stop_filter/launch/stop_filter.launch.xml index 36a66a2c143c0..0ea92d26c9677 100644 --- a/localization/stop_filter/launch/stop_filter.launch.xml +++ b/localization/stop_filter/launch/stop_filter.launch.xml @@ -1,6 +1,5 @@ - - + @@ -10,7 +9,6 @@ - - + diff --git a/localization/stop_filter/src/stop_filter.cpp b/localization/stop_filter/src/stop_filter.cpp index 111d460be737e..ac0960b8cb67b 100644 --- a/localization/stop_filter/src/stop_filter.cpp +++ b/localization/stop_filter/src/stop_filter.cpp @@ -27,8 +27,8 @@ using std::placeholders::_1; StopFilter::StopFilter(const std::string & node_name, const rclcpp::NodeOptions & node_options) : rclcpp::Node(node_name, node_options) { - vx_threshold_ = declare_parameter("vx_threshold", 0.01); - wz_threshold_ = declare_parameter("wz_threshold", 0.01); + vx_threshold_ = declare_parameter("vx_threshold"); + wz_threshold_ = declare_parameter("wz_threshold"); sub_odom_ = create_subscription( "input/odom", 1, std::bind(&StopFilter::callbackOdometry, this, _1)); diff --git a/localization/tree_structured_parzen_estimator/CMakeLists.txt b/localization/tree_structured_parzen_estimator/CMakeLists.txt new file mode 100644 index 0000000000000..7b687a12a003c --- /dev/null +++ b/localization/tree_structured_parzen_estimator/CMakeLists.txt @@ -0,0 +1,23 @@ +cmake_minimum_required(VERSION 3.14) +project(tree_structured_parzen_estimator) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +ament_auto_add_library(tree_structured_parzen_estimator SHARED + src/tree_structured_parzen_estimator.cpp +) + +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + ament_add_gtest(test_tpe + test/test_tpe.cpp + src/tree_structured_parzen_estimator.cpp + ) + target_include_directories(test_tpe PRIVATE include) + target_link_libraries(test_tpe) +endif() + +ament_auto_package( + INSTALL_TO_SHARE +) diff --git a/localization/tree_structured_parzen_estimator/README.md b/localization/tree_structured_parzen_estimator/README.md new file mode 100644 index 0000000000000..2b21e65929485 --- /dev/null +++ b/localization/tree_structured_parzen_estimator/README.md @@ -0,0 +1,5 @@ +# tree_structured_parzen_estimator + +`tree_structured_parzen_estimator`` is a package for black-box optimization. + +This package does not have a node, it is just a library. diff --git a/localization/tree_structured_parzen_estimator/include/tree_structured_parzen_estimator/tree_structured_parzen_estimator.hpp b/localization/tree_structured_parzen_estimator/include/tree_structured_parzen_estimator/tree_structured_parzen_estimator.hpp new file mode 100644 index 0000000000000..b7b522b4e6b76 --- /dev/null +++ b/localization/tree_structured_parzen_estimator/include/tree_structured_parzen_estimator/tree_structured_parzen_estimator.hpp @@ -0,0 +1,80 @@ +// Copyright 2023 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 TREE_STRUCTURED_PARZEN_ESTIMATOR__TREE_STRUCTURED_PARZEN_ESTIMATOR_HPP_ +#define TREE_STRUCTURED_PARZEN_ESTIMATOR__TREE_STRUCTURED_PARZEN_ESTIMATOR_HPP_ + +/* +A implementation of tree-structured parzen estimator (TPE) +See below pdf for the TPE algorithm detail. +https://papers.nips.cc/paper_files/paper/2011/file/86e8f7ab32cfd12577bc2619bc635690-Paper.pdf + +Optuna is also used as a reference for implementation. +https://github.com/optuna/optuna +*/ + +#include +#include +#include + +class TreeStructuredParzenEstimator +{ +public: + using Input = std::vector; + using Score = double; + struct Trial + { + Input input; + Score score; + }; + + enum Direction { + MINIMIZE = 0, + MAXIMIZE = 1, + }; + + TreeStructuredParzenEstimator() = delete; + TreeStructuredParzenEstimator( + const Direction direction, const int64_t n_startup_trials, std::vector is_loop_variable); + void add_trial(const Trial & trial); + Input get_next_input() const; + +private: + static constexpr double BASE_STDDEV_COEFF = 0.2; + static constexpr double MAX_GOOD_RATE = 0.10; + static constexpr double MAX_VALUE = 1.0; + static constexpr double MIN_VALUE = -1.0; + static constexpr double VALUE_WIDTH = MAX_VALUE - MIN_VALUE; + static constexpr int64_t N_EI_CANDIDATES = 100; + static constexpr double PRIOR_WEIGHT = 0.0; + + static std::mt19937_64 engine; + static std::uniform_real_distribution dist_uniform; + static std::normal_distribution dist_normal; + + double compute_log_likelihood_ratio(const Input & input) const; + double log_gaussian_pdf(const Input & input, const Input & mu, const Input & sigma) const; + static std::vector get_weights(const int64_t n); + static double normalize_loop_variable(const double value); + + std::vector trials_; + int64_t above_num_; + const Direction direction_; + const int64_t n_startup_trials_; + const int64_t input_dimension_; + const std::vector is_loop_variable_; + const Input base_stddev_; +}; + +#endif // TREE_STRUCTURED_PARZEN_ESTIMATOR__TREE_STRUCTURED_PARZEN_ESTIMATOR_HPP_ diff --git a/localization/tree_structured_parzen_estimator/package.xml b/localization/tree_structured_parzen_estimator/package.xml new file mode 100644 index 0000000000000..b699d5c69e610 --- /dev/null +++ b/localization/tree_structured_parzen_estimator/package.xml @@ -0,0 +1,23 @@ + + + + tree_structured_parzen_estimator + 0.1.0 + The tree_structured_parzen_estimator package + Yamato Ando + Masahiro Sakamoto + Shintaro Sakoda + Kento Yabuuchi + Apache License 2.0 + Shintaro Sakoda + + ament_cmake_auto + autoware_cmake + + ament_cmake_cppcheck + ament_lint_auto + + + ament_cmake + + diff --git a/localization/tree_structured_parzen_estimator/src/tree_structured_parzen_estimator.cpp b/localization/tree_structured_parzen_estimator/src/tree_structured_parzen_estimator.cpp new file mode 100644 index 0000000000000..99c70a844f331 --- /dev/null +++ b/localization/tree_structured_parzen_estimator/src/tree_structured_parzen_estimator.cpp @@ -0,0 +1,217 @@ +// Copyright 2023 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 "tree_structured_parzen_estimator/tree_structured_parzen_estimator.hpp" + +#include +#include +#include +#include + +// random number generator +std::mt19937_64 TreeStructuredParzenEstimator::engine(std::random_device{}()); +std::uniform_real_distribution TreeStructuredParzenEstimator::dist_uniform( + TreeStructuredParzenEstimator::MIN_VALUE, TreeStructuredParzenEstimator::MAX_VALUE); +std::normal_distribution TreeStructuredParzenEstimator::dist_normal(0.0, 1.0); + +TreeStructuredParzenEstimator::TreeStructuredParzenEstimator( + const Direction direction, const int64_t n_startup_trials, std::vector is_loop_variable) +: above_num_(0), + direction_(direction), + n_startup_trials_(n_startup_trials), + input_dimension_(is_loop_variable.size()), + is_loop_variable_(is_loop_variable), + base_stddev_(input_dimension_, VALUE_WIDTH) +{ +} + +void TreeStructuredParzenEstimator::add_trial(const Trial & trial) +{ + trials_.push_back(trial); + std::sort(trials_.begin(), trials_.end(), [this](const Trial & lhs, const Trial & rhs) { + return (direction_ == Direction::MAXIMIZE ? lhs.score > rhs.score : lhs.score < rhs.score); + }); + above_num_ = + std::min(static_cast(25), static_cast(trials_.size() * MAX_GOOD_RATE)); +} + +TreeStructuredParzenEstimator::Input TreeStructuredParzenEstimator::get_next_input() const +{ + if (static_cast(trials_.size()) < n_startup_trials_ || above_num_ == 0) { + // Random sampling based on prior until the number of trials reaches `n_startup_trials_`. + Input input(input_dimension_); + for (int64_t j = 0; j < input_dimension_; j++) { + input[j] = dist_uniform(engine); + } + return input; + } + + Input best_input; + double best_log_likelihood_ratio = std::numeric_limits::lowest(); + const double coeff = BASE_STDDEV_COEFF * std::pow(above_num_, -1.0 / (4 + input_dimension_)); + std::vector weights = get_weights(above_num_); + weights.push_back(PRIOR_WEIGHT); + std::discrete_distribution dist(weights.begin(), weights.end()); + for (int64_t i = 0; i < N_EI_CANDIDATES; i++) { + Input mu, sigma; + const int64_t index = dist(engine); + if (index == above_num_) { + mu = Input(input_dimension_, 0.0); + sigma = base_stddev_; + } else { + mu = trials_[index].input; + sigma = base_stddev_; + for (int64_t j = 0; j < input_dimension_; j++) { + sigma[j] *= coeff; + } + } + // sample from the normal distribution + Input input(input_dimension_); + for (int64_t j = 0; j < input_dimension_; j++) { + input[j] = mu[j] + dist_normal(engine) * sigma[j]; + input[j] = + (is_loop_variable_[j] ? normalize_loop_variable(input[j]) + : std::clamp(input[j], MIN_VALUE, MAX_VALUE)); + } + const double log_likelihood_ratio = compute_log_likelihood_ratio(input); + if (log_likelihood_ratio > best_log_likelihood_ratio) { + best_log_likelihood_ratio = log_likelihood_ratio; + best_input = input; + } + } + return best_input; +} + +double TreeStructuredParzenEstimator::compute_log_likelihood_ratio(const Input & input) const +{ + const int64_t n = trials_.size(); + + // The above KDE and the below KDE are calculated respectively, and the ratio is the criteria to + // select best sample. + std::vector above_logs; + std::vector below_logs; + + // Scott's rule + const double coeff_above = + BASE_STDDEV_COEFF * std::pow(above_num_, -1.0 / (4 + input_dimension_)); + const double coeff_below = + BASE_STDDEV_COEFF * std::pow(n - above_num_, -1.0 / (4 + input_dimension_)); + Input sigma_above = base_stddev_; + Input sigma_below = base_stddev_; + for (int64_t j = 0; j < input_dimension_; j++) { + sigma_above[j] *= coeff_above; + sigma_below[j] *= coeff_below; + } + + std::vector above_weights = get_weights(above_num_); + std::vector below_weights = get_weights(n - above_num_); + std::reverse(below_weights.begin(), below_weights.end()); // below_weights is ascending order + + // calculate the sum of weights to normalize + double above_sum = std::accumulate(above_weights.begin(), above_weights.end(), 0.0); + double below_sum = std::accumulate(below_weights.begin(), below_weights.end(), 0.0); + + // above includes prior + above_sum += PRIOR_WEIGHT; + + for (int64_t i = 0; i < n; i++) { + if (i < above_num_) { + const double log_p = log_gaussian_pdf(input, trials_[i].input, sigma_above); + const double w = above_weights[i] / above_sum; + const double log_w = std::log(w); + above_logs.push_back(log_p + log_w); + } else { + const double log_p = log_gaussian_pdf(input, trials_[i].input, sigma_below); + const double w = below_weights[i - above_num_] / below_sum; + const double log_w = std::log(w); + below_logs.push_back(log_p + log_w); + } + } + + // prior + if (PRIOR_WEIGHT > 0.0) { + const double log_p = log_gaussian_pdf(input, Input(input_dimension_, 0.0), base_stddev_); + const double log_w = std::log(PRIOR_WEIGHT / above_sum); + above_logs.push_back(log_p + log_w); + } + + auto log_sum_exp = [](const std::vector & log_vec) { + const double max = *std::max_element(log_vec.begin(), log_vec.end()); + double sum = 0.0; + for (const double log_v : log_vec) { + sum += std::exp(log_v - max); + } + return max + std::log(sum); + }; + + const double above = log_sum_exp(above_logs); + const double below = log_sum_exp(below_logs); + const double r = above - below; + return r; +} + +double TreeStructuredParzenEstimator::log_gaussian_pdf( + const Input & input, const Input & mu, const Input & sigma) const +{ + const double log_2pi = std::log(2.0 * M_PI); + auto log_gaussian_pdf_1d = [&](const double diff, const double sigma) { + return -0.5 * log_2pi - std::log(sigma) - (diff * diff) / (2.0 * sigma * sigma); + }; + + const int64_t n = input.size(); + + double result = 0.0; + for (int64_t i = 0; i < n; i++) { + double diff = input[i] - mu[i]; + if (is_loop_variable_[i]) { + diff = normalize_loop_variable(diff); + } + result += log_gaussian_pdf_1d(diff, sigma[i]); + } + return result; +} + +std::vector TreeStructuredParzenEstimator::get_weights(const int64_t n) +{ + // See optuna + // https://github.com/optuna/optuna/blob/4bfab78e98bf786f6a2ce6e593a26e3f8403e08d/optuna/samplers/_tpe/sampler.py#L50-L58 + std::vector weights; + constexpr int64_t WEIGHT_ALPHA = 25; + if (n == 0) { + return weights; + } else if (n < WEIGHT_ALPHA) { + weights.resize(n, 1.0); + } else { + weights.resize(n); + const double unit = (1.0 - 1.0 / n) / (n - WEIGHT_ALPHA); + for (int64_t i = 0; i < n; i++) { + weights[i] = (i < WEIGHT_ALPHA ? 1.0 : 1.0 - unit * (i - WEIGHT_ALPHA)); + } + } + + return weights; +} + +double TreeStructuredParzenEstimator::normalize_loop_variable(const double value) +{ + // Normalize the loop variable to [-1, 1) + double result = value; + while (result >= MAX_VALUE) { + result -= VALUE_WIDTH; + } + while (result < MIN_VALUE) { + result += VALUE_WIDTH; + } + return result; +} diff --git a/localization/tree_structured_parzen_estimator/test/test_tpe.cpp b/localization/tree_structured_parzen_estimator/test/test_tpe.cpp new file mode 100644 index 0000000000000..32eb66e70fb16 --- /dev/null +++ b/localization/tree_structured_parzen_estimator/test/test_tpe.cpp @@ -0,0 +1,65 @@ +// Copyright 2023 The Autoware Contributors +// +// 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 "tree_structured_parzen_estimator/tree_structured_parzen_estimator.hpp" + +#include + +TEST(TreeStructuredParzenEstimatorTest, TPE_is_better_than_random_search_on_sphere_function) +{ + auto sphere_function = [](const TreeStructuredParzenEstimator::Input & input) { + double value = 0.0; + const int64_t n = input.size(); + for (int64_t i = 0; i < n; i++) { + const double v = input[i] * 10; + value += v * v; + } + return value; + }; + + constexpr int64_t kOuterTrialsNum = 10; + constexpr int64_t kInnerTrialsNum = 100; + std::cout << std::fixed; + std::vector mean_scores; + for (const int64_t n_startup_trials : {kInnerTrialsNum, kInnerTrialsNum / 10}) { + const std::string method = ((n_startup_trials == kInnerTrialsNum) ? "Random" : "TPE"); + + std::vector scores; + for (int64_t i = 0; i < kOuterTrialsNum; i++) { + double best_score = std::numeric_limits::lowest(); + const std::vector is_loop_variable(6, false); + TreeStructuredParzenEstimator estimator( + TreeStructuredParzenEstimator::Direction::MAXIMIZE, n_startup_trials, is_loop_variable); + for (int64_t trial = 0; trial < kInnerTrialsNum; trial++) { + const TreeStructuredParzenEstimator::Input input = estimator.get_next_input(); + const double score = -sphere_function(input); + estimator.add_trial({input, score}); + best_score = std::max(best_score, score); + } + scores.push_back(best_score); + } + + const double sum = std::accumulate(scores.begin(), scores.end(), 0.0); + const double mean = sum / scores.size(); + mean_scores.push_back(mean); + double sq_sum = 0.0; + for (const double score : scores) { + sq_sum += (score - mean) * (score - mean); + } + const double stddev = std::sqrt(sq_sum / scores.size()); + + std::cout << method << ", mean = " << mean << ", stddev = " << stddev << std::endl; + } + ASSERT_LT(mean_scores[0], mean_scores[1]); +} diff --git a/localization/yabloc/yabloc_common/package.xml b/localization/yabloc/yabloc_common/package.xml index 1c3ba4de307f6..d7e94ebefa24b 100644 --- a/localization/yabloc/yabloc_common/package.xml +++ b/localization/yabloc/yabloc_common/package.xml @@ -13,22 +13,17 @@ autoware_auto_mapping_msgs cv_bridge - geographiclib geometry_msgs lanelet2_core lanelet2_extension - lanelet2_io libgoogle-glog-dev pcl_conversions - rcl_interfaces rclcpp - rclcpp_components sensor_msgs signal_processing sophus std_msgs tf2_ros - ublox_msgs visualization_msgs ament_lint_auto diff --git a/localization/yabloc/yabloc_image_processing/README.md b/localization/yabloc/yabloc_image_processing/README.md index eb9ae658c39a7..85e1d408a1b4b 100644 --- a/localization/yabloc/yabloc_image_processing/README.md +++ b/localization/yabloc/yabloc_image_processing/README.md @@ -108,10 +108,15 @@ This node performs image resizing and undistortion at the same time. #### Input -| Name | Type | Description | -| ---------------------------- | ----------------------------------- | -------------------- | -| `input/camera_info` | `sensor_msgs::msg::CameraInfo` | raw compressed image | -| `input/image_raw/compressed` | `sensor_msgs::msg::CompressedImage` | raw sensor info | +| Name | Type | Description | +| ---------------------------- | ----------------------------------- | ----------------------- | +| `input/camera_info` | `sensor_msgs::msg::CameraInfo` | camera info | +| `input/image_raw` | `sensor_msgs::msg::Image` | raw camera image | +| `input/image_raw/compressed` | `sensor_msgs::msg::CompressedImage` | compressed camera image | + +This node subscribes to both compressed image and raw image topics. +If raw image is subscribed to even once, compressed image will no longer be subscribed to. +This is to avoid redundant decompression within Autoware. #### Output diff --git a/localization/yabloc/yabloc_image_processing/launch/image_processing.launch.xml b/localization/yabloc/yabloc_image_processing/launch/image_processing.launch.xml index fd55a8163123a..aa6fc10ee6667 100644 --- a/localization/yabloc/yabloc_image_processing/launch/image_processing.launch.xml +++ b/localization/yabloc/yabloc_image_processing/launch/image_processing.launch.xml @@ -1,5 +1,5 @@ - + @@ -9,6 +9,7 @@ + diff --git a/localization/yabloc/yabloc_image_processing/package.xml b/localization/yabloc/yabloc_image_processing/package.xml index 961ce574f7c7a..ffdcea25b4b6d 100644 --- a/localization/yabloc/yabloc_image_processing/package.xml +++ b/localization/yabloc/yabloc_image_processing/package.xml @@ -12,14 +12,10 @@ autoware_cmake cv_bridge - nav_msgs pcl_conversions rclcpp sensor_msgs - sophus std_msgs - tf2 - tf2_ros tier4_autoware_utils visualization_msgs yabloc_common diff --git a/localization/yabloc/yabloc_image_processing/src/undistort/undistort_node.cpp b/localization/yabloc/yabloc_image_processing/src/undistort/undistort_node.cpp index 7412db28e1550..7fc9ad785dbe2 100644 --- a/localization/yabloc/yabloc_image_processing/src/undistort/undistort_node.cpp +++ b/localization/yabloc/yabloc_image_processing/src/undistort/undistort_node.cpp @@ -50,9 +50,12 @@ class UndistortNode : public rclcpp::Node } auto on_image = std::bind(&UndistortNode::on_image, this, _1); + auto on_compressed_image = std::bind(&UndistortNode::on_compressed_image, this, _1); auto on_info = std::bind(&UndistortNode::on_info, this, _1); - sub_image_ = - create_subscription("~/input/image_raw", qos, std::move(on_image)); + sub_image_ = create_subscription("~/input/image_raw", qos, std::move(on_image)); + sub_compressed_image_ = create_subscription( + "~/input/image_raw/compressed", qos, std::move(on_compressed_image)); + sub_info_ = create_subscription("~/input/camera_info", qos, std::move(on_info)); pub_info_ = create_publisher("~/output/resized_info", 10); @@ -63,7 +66,8 @@ class UndistortNode : public rclcpp::Node const int OUTPUT_WIDTH; const std::string OVERRIDE_FRAME_ID; - rclcpp::Subscription::SharedPtr sub_image_; + rclcpp::Subscription::SharedPtr sub_image_; + rclcpp::Subscription::SharedPtr sub_compressed_image_; rclcpp::Subscription::SharedPtr sub_info_; rclcpp::Publisher::SharedPtr pub_image_; rclcpp::Publisher::SharedPtr pub_info_; @@ -99,14 +103,8 @@ class UndistortNode : public rclcpp::Node scaled_info_->height = new_size.height; } - void on_image(const CompressedImage & msg) + void remap_and_publish(const cv::Mat & image, const std_msgs::msg::Header & header) { - if (!info_.has_value()) return; - if (undistort_map_x.empty()) make_remap_lut(); - - tier4_autoware_utils::StopWatch stop_watch; - cv::Mat image = common::decompress_to_cv_mat(msg); - cv::Mat undistorted_image; cv::remap(image, undistorted_image, undistort_map_x, undistort_map_y, cv::INTER_LINEAR); @@ -120,16 +118,47 @@ class UndistortNode : public rclcpp::Node // Publish Image { cv_bridge::CvImage bridge; - bridge.header.stamp = msg.header.stamp; + bridge.header.stamp = header.stamp; if (OVERRIDE_FRAME_ID != "") bridge.header.frame_id = OVERRIDE_FRAME_ID; else - bridge.header.frame_id = msg.header.frame_id; + bridge.header.frame_id = header.frame_id; bridge.encoding = "bgr8"; bridge.image = undistorted_image; pub_image_->publish(*bridge.toImageMsg()); } + } + void on_image(const Image & msg) + { + if (!info_.has_value()) { + return; + } + if (undistort_map_x.empty()) { + make_remap_lut(); + } + + // To remove redundant decompression, deactivate compressed image subscriber + if (sub_compressed_image_) { + sub_compressed_image_.reset(); + } + + tier4_autoware_utils::StopWatch stop_watch; + remap_and_publish(common::decompress_to_cv_mat(msg), msg.header); + RCLCPP_INFO_STREAM(get_logger(), "image undistort: " << stop_watch.toc() << "[ms]"); + } + + void on_compressed_image(const CompressedImage & msg) + { + if (!info_.has_value()) { + return; + } + if (undistort_map_x.empty()) { + make_remap_lut(); + } + + tier4_autoware_utils::StopWatch stop_watch; + remap_and_publish(common::decompress_to_cv_mat(msg), msg.header); RCLCPP_INFO_STREAM(get_logger(), "image undistort: " << stop_watch.toc() << "[ms]"); } diff --git a/localization/yabloc/yabloc_particle_filter/package.xml b/localization/yabloc/yabloc_particle_filter/package.xml index 6d11b12d9c922..aabb6009c2148 100644 --- a/localization/yabloc/yabloc_particle_filter/package.xml +++ b/localization/yabloc/yabloc_particle_filter/package.xml @@ -13,7 +13,6 @@ rosidl_default_generators geometry_msgs - libgoogle-glog-dev rclcpp sensor_msgs sophus @@ -22,7 +21,6 @@ tf2 tf2_geometry_msgs tf2_ros - tf2_sensor_msgs tier4_autoware_utils visualization_msgs yabloc_common diff --git a/localization/yabloc/yabloc_pose_initializer/CMakeLists.txt b/localization/yabloc/yabloc_pose_initializer/CMakeLists.txt index cd54a3c5f2ef2..164f280becae8 100644 --- a/localization/yabloc/yabloc_pose_initializer/CMakeLists.txt +++ b/localization/yabloc/yabloc_pose_initializer/CMakeLists.txt @@ -14,24 +14,6 @@ find_package(PCL REQUIRED COMPONENTS common kdtree) # Sophus find_package(Sophus REQUIRED) -# =================================================== -# Download DNN model -include(download.cmake) - -# =================================================== -# Clear ${PYTHON_EXECUTABLE} defined by mrt_cmake_module so that rosidl_generate_interfaces can look for it properly -unset(PYTHON_EXECUTABLE) -message(STATUS "PYTHON_EXECUTABLE: ${PYTHON_EXECUTABLE}") - -# =================================================== -# Service -rosidl_generate_interfaces(${PROJECT_NAME} - "srv/SemanticSegmentation.srv" - DEPENDENCIES - std_msgs - sensor_msgs -) - # =================================================== # Executable # Camera @@ -40,26 +22,12 @@ ament_auto_add_executable(${TARGET} src/camera/lane_image.cpp src/camera/marker_module.cpp src/camera/projector_module.cpp + src/camera/semantic_segmentation.cpp src/camera/camera_pose_initializer_core.cpp src/camera/camera_pose_initializer_node.cpp) target_include_directories(${TARGET} PUBLIC include) target_include_directories(${TARGET} SYSTEM PRIVATE ${EIGEN3_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS}) target_link_libraries(${TARGET} ${PCL_LIBRARIES} Sophus::Sophus) -# ros2idl typesupport -if(${rosidl_cmake_VERSION} VERSION_LESS 2.5.0) - rosidl_target_interfaces(${TARGET} ${PROJECT_NAME} "rosidl_typesupport_cpp") -else() - rosidl_get_typesupport_target(cpp_typesupport_target ${PROJECT_NAME} "rosidl_typesupport_cpp") - target_link_libraries(${TARGET} "${cpp_typesupport_target}") -endif() - -# Semantic segmentation -install(PROGRAMS - src/semantic_segmentation/semantic_segmentation_core.py - src/semantic_segmentation/semantic_segmentation_server.py - DESTINATION lib/${PROJECT_NAME} -) - # =================================================== -ament_auto_package(INSTALL_TO_SHARE config data launch) +ament_auto_package(INSTALL_TO_SHARE config launch) diff --git a/localization/yabloc/yabloc_pose_initializer/README.md b/localization/yabloc/yabloc_pose_initializer/README.md index 4e3c0a0363402..907b79c1459ec 100644 --- a/localization/yabloc/yabloc_pose_initializer/README.md +++ b/localization/yabloc/yabloc_pose_initializer/README.md @@ -1,27 +1,24 @@ # yabloc_pose_initializer -This package contains some nodes related to initial pose estimation. +This package contains a node related to initial pose estimation. - [camera_pose_initializer](#camera_pose_initializer) -- [semantic_segmentation_server](#semantic_segmentation_server) -Ideally, this package downloads a pre-trained semantic segmentation model during the build and loads it at runtime for initialization. -However, to handle cases where network connectivity is not available at build time, **the default behavior is not to download the model during build.** -Even if the model is not downloaded, initialization will still complete, but the accuracy may be compromised. +This package requires the pre-trained semantic segmentation model for runtime. This model is usually downloaded by `ansible` during env preparation phase of the [installation](https://autowarefoundation.github.io/autoware-documentation/main/installation/autoware/source-installation/). +It is also possible to download it manually. Even if the model is not downloaded, initialization will still complete, but the accuracy may be compromised. - - -To download the model, please specify `--cmake-args -DDOWNLOAD_ARTIFACTS=ON` to the build command. +To download and extract the model manually: ```bash -colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release -DDOWNLOAD_ARTIFACTS=ON --packages-select yabloc_pose_initializer +$ mkdir -p ~/autoware_data/yabloc_pose_initializer/ +$ wget -P ~/autoware_data/yabloc_pose_initializer/ \ + https://s3.ap-northeast-2.wasabisys.com/pinto-model-zoo/136_road-segmentation-adas-0001/resources.tar.gz +$ tar xzf ~/autoware_data/yabloc_pose_initializer/resources.tar.gz -C ~/autoware_data/yabloc_pose_initializer/ ``` -For detailed information about the downloaded contents, please consult the `download.cmake` file in this package. - ## Note -This package makes use of external code. The trained files are provided by apollo. The trained files are automatically downloaded when you build. +This package makes use of external code. The trained files are provided by apollo. The trained files are automatically downloaded during env preparation. Original model URL @@ -71,21 +68,3 @@ Converted model URL | Name | Type | Description | | ------------------ | --------------------------------------------------------- | ------------------------------- | | `yabloc_align_srv` | `tier4_localization_msgs::srv::PoseWithCovarianceStamped` | initial pose estimation request | - -### Clients - -| Name | Type | Description | -| --------------------------- | ---------------------------------------------------- | ----------------------------- | -| `semantic_segmentation_srv` | `yabloc_pose_initializer::srv::SemanticSegmentation` | semantic segmentation request | - -## semantic_segmentation_server - -### Purpose - -- This node performs semantic segmentation. - -### Services - -| Name | Type | Description | -| --------------------------- | ---------------------------------------------------- | ----------------------------- | -| `semantic_segmentation_srv` | `yabloc_pose_initializer::srv::SemanticSegmentation` | semantic segmentation request | diff --git a/localization/yabloc/yabloc_pose_initializer/download.cmake b/localization/yabloc/yabloc_pose_initializer/download.cmake deleted file mode 100644 index 6ce997b3978d5..0000000000000 --- a/localization/yabloc/yabloc_pose_initializer/download.cmake +++ /dev/null @@ -1,67 +0,0 @@ -# Copyright 2023 the 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. - -set(DOWNLOAD_ARTIFACTS OFF CACHE BOOL "enable artifacts download") - -set(DATA_URL "https://s3.ap-northeast-2.wasabisys.com/pinto-model-zoo/136_road-segmentation-adas-0001/resources.tar.gz") -set(DATA_PATH "${CMAKE_CURRENT_SOURCE_DIR}/data") -set(FILE_HASH 146ed8af689a30b898dc5369870c40fb) -set(FILE_NAME "resources.tar.gz") - -function(download_and_extract) - message(STATUS "Checking and downloading ${FILE_NAME}") - set(FILE_PATH ${DATA_PATH}/${FILE_NAME}) - set(STATUS_CODE 0) - message(STATUS "start ${FILE_NAME}") - - if(EXISTS ${FILE_PATH}) - message(STATUS "found ${FILE_NAME}") - file(MD5 ${FILE_PATH} EXISTING_FILE_HASH) - - if(${FILE_HASH} STREQUAL ${EXISTING_FILE_HASH}) - message(STATUS "same ${FILE_NAME}") - message(STATUS "File already exists.") - else() - message(STATUS "diff ${FILE_NAME}") - message(STATUS "File hash changes. Downloading now ...") - file(DOWNLOAD ${DATA_URL} ${FILE_PATH} STATUS DOWNLOAD_STATUS TIMEOUT 3600) - list(GET DOWNLOAD_STATUS 0 STATUS_CODE) - list(GET DOWNLOAD_STATUS 1 ERROR_MESSAGE) - endif() - else() - if(DOWNLOAD_ARTIFACTS) - message(STATUS "not found ${FILE_NAME}") - message(STATUS "File doesn't exists. Downloading now ...") - file(DOWNLOAD ${DATA_URL} ${FILE_PATH} STATUS DOWNLOAD_STATUS TIMEOUT 3600) - list(GET DOWNLOAD_STATUS 0 STATUS_CODE) - list(GET DOWNLOAD_STATUS 1 ERROR_MESSAGE) - else() - message(WARNING "Skipped download for ${FILE_NAME} (enable by setting DOWNLOAD_ARTIFACTS)") - file(MAKE_DIRECTORY "${DATA_PATH}") - return() - endif() - endif() - - if(${STATUS_CODE} EQUAL 0) - message(STATUS "Download completed successfully!") - else() - message(FATAL_ERROR "Error occurred during download: ${ERROR_MESSAGE}") - endif() - - execute_process(COMMAND - ${CMAKE_COMMAND} -E - tar xzf "${DATA_PATH}/${FILE_NAME}" WORKING_DIRECTORY "${DATA_PATH}") -endfunction() - -download_and_extract() diff --git a/localization/yabloc/yabloc_pose_initializer/include/yabloc_pose_initializer/camera/camera_pose_initializer.hpp b/localization/yabloc/yabloc_pose_initializer/include/yabloc_pose_initializer/camera/camera_pose_initializer.hpp index ccb55e79b8ea9..917a6ecaaf291 100644 --- a/localization/yabloc/yabloc_pose_initializer/include/yabloc_pose_initializer/camera/camera_pose_initializer.hpp +++ b/localization/yabloc/yabloc_pose_initializer/include/yabloc_pose_initializer/camera/camera_pose_initializer.hpp @@ -18,10 +18,10 @@ #include "yabloc_pose_initializer/camera/lane_image.hpp" #include "yabloc_pose_initializer/camera/marker_module.hpp" #include "yabloc_pose_initializer/camera/projector_module.hpp" +#include "yabloc_pose_initializer/camera/semantic_segmentation.hpp" #include #include -#include #include #include @@ -42,7 +42,6 @@ class CameraPoseInitializer : public rclcpp::Node using Image = sensor_msgs::msg::Image; using HADMapBin = autoware_auto_mapping_msgs::msg::HADMapBin; using RequestPoseAlignment = tier4_localization_msgs::srv::PoseWithCovarianceStamped; - using SegmentationSrv = yabloc_pose_initializer::srv::SemanticSegmentation; CameraPoseInitializer(); @@ -57,12 +56,13 @@ class CameraPoseInitializer : public rclcpp::Node rclcpp::Subscription::SharedPtr sub_image_; rclcpp::Service::SharedPtr align_server_; - rclcpp::Client::SharedPtr segmentation_client_; rclcpp::CallbackGroup::SharedPtr service_callback_group_; std::optional latest_image_msg_{std::nullopt}; lanelet::ConstLanelets const_lanelets_; + std::unique_ptr semantic_segmentation_{nullptr}; + void on_map(const HADMapBin & msg); void on_service( const RequestPoseAlignment::Request::SharedPtr, diff --git a/localization/yabloc/yabloc_pose_initializer/include/yabloc_pose_initializer/camera/projector_module.hpp b/localization/yabloc/yabloc_pose_initializer/include/yabloc_pose_initializer/camera/projector_module.hpp index cc084990a0ff3..071d55482f564 100644 --- a/localization/yabloc/yabloc_pose_initializer/include/yabloc_pose_initializer/camera/projector_module.hpp +++ b/localization/yabloc/yabloc_pose_initializer/include/yabloc_pose_initializer/camera/projector_module.hpp @@ -33,7 +33,7 @@ class ProjectorModule bool define_project_func(); - cv::Mat project_image(const sensor_msgs::msg::Image & image_msg); + cv::Mat project_image(const cv::Mat & mask_image); private: common::CameraInfoSubscriber info_; diff --git a/localization/yabloc/yabloc_pose_initializer/include/yabloc_pose_initializer/camera/semantic_segmentation.hpp b/localization/yabloc/yabloc_pose_initializer/include/yabloc_pose_initializer/camera/semantic_segmentation.hpp new file mode 100644 index 0000000000000..061d0548c6f18 --- /dev/null +++ b/localization/yabloc/yabloc_pose_initializer/include/yabloc_pose_initializer/camera/semantic_segmentation.hpp @@ -0,0 +1,46 @@ +// Copyright 2023 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 YABLOC_POSE_INITIALIZER__CAMERA__SEMANTIC_SEGMENTATION_HPP_ +#define YABLOC_POSE_INITIALIZER__CAMERA__SEMANTIC_SEGMENTATION_HPP_ +#include +#include + +#include +#include +#include + +class SemanticSegmentation +{ +public: + explicit SemanticSegmentation(const std::string & model_path); + + cv::Mat inference(const cv::Mat & image, double score_threshold = 0.5); + + static void print_error_message(const rclcpp::Logger & logger); + +private: + cv::Mat make_blob(const cv::Mat & image); + + cv::Mat convert_blob_to_image(const cv::Mat & blob); + + cv::Mat normalize(const cv::Mat & mask, double score_threshold = 0.5); + + cv::Mat draw_overlay(const cv::Mat & image, const cv::Mat & segmentation); + + struct Impl; + std::shared_ptr impl_{nullptr}; +}; + +#endif // YABLOC_POSE_INITIALIZER__CAMERA__SEMANTIC_SEGMENTATION_HPP_ diff --git a/localization/yabloc/yabloc_pose_initializer/launch/yabloc_pose_initializer.launch.xml b/localization/yabloc/yabloc_pose_initializer/launch/yabloc_pose_initializer.launch.xml index a1d5d59b46094..83c2c8fe09287 100644 --- a/localization/yabloc/yabloc_pose_initializer/launch/yabloc_pose_initializer.launch.xml +++ b/localization/yabloc/yabloc_pose_initializer/launch/yabloc_pose_initializer.launch.xml @@ -1,17 +1,14 @@ + + + + - - - - - - - diff --git a/localization/yabloc/yabloc_pose_initializer/package.xml b/localization/yabloc/yabloc_pose_initializer/package.xml index f6e8aa83bafc7..4e3ca1c1e61d1 100644 --- a/localization/yabloc/yabloc_pose_initializer/package.xml +++ b/localization/yabloc/yabloc_pose_initializer/package.xml @@ -15,11 +15,7 @@ geometry_msgs lanelet2_extension rclcpp - rclpy sensor_msgs - std_msgs - std_srvs - tf2 tier4_localization_msgs visualization_msgs yabloc_common diff --git a/localization/yabloc/yabloc_pose_initializer/src/camera/camera_pose_initializer_core.cpp b/localization/yabloc/yabloc_pose_initializer/src/camera/camera_pose_initializer_core.cpp index 5fab9763328e3..19d3c8d88f260 100644 --- a/localization/yabloc/yabloc_pose_initializer/src/camera/camera_pose_initializer_core.cpp +++ b/localization/yabloc/yabloc_pose_initializer/src/camera/camera_pose_initializer_core.cpp @@ -20,6 +20,10 @@ #include #include +#include + +#include + namespace yabloc { CameraPoseInitializer::CameraPoseInitializer() @@ -39,17 +43,17 @@ CameraPoseInitializer::CameraPoseInitializer() sub_map_ = create_subscription("~/input/vector_map", map_qos, on_map); sub_image_ = create_subscription("~/input/image_raw", 10, on_image); - // Client - segmentation_client_ = create_client( - "~/semantic_segmentation_srv", rmw_qos_profile_services_default, service_callback_group_); - // Server auto on_service = std::bind(&CameraPoseInitializer::on_service, this, _1, _2); align_server_ = create_service("~/yabloc_align_srv", on_service); - using namespace std::literals::chrono_literals; - while (!segmentation_client_->wait_for_service(1s) && rclcpp::ok()) { - RCLCPP_INFO_STREAM(get_logger(), "Waiting for " << segmentation_client_->get_service_name()); + const std::string model_path = declare_parameter("model_path"); + RCLCPP_INFO_STREAM(get_logger(), "segmentation model path: " + model_path); + if (std::filesystem::exists(model_path)) { + semantic_segmentation_ = std::make_unique(model_path); + } else { + semantic_segmentation_ = nullptr; + SemanticSegmentation::print_error_message(get_logger()); } } @@ -97,31 +101,20 @@ std::optional CameraPoseInitializer::estimate_pose( return std::nullopt; } - Image segmented_image; + cv::Mat segmented_image; { - // Call semantic segmentation service - auto request = std::make_shared(); - request->src_image = *latest_image_msg_.value(); - auto result_future = segmentation_client_->async_send_request(request); - using namespace std::literals::chrono_literals; - std::future_status status = result_future.wait_for(1000ms); - if (status == std::future_status::ready) { - const auto response = result_future.get(); - if (response->success) { - segmented_image = response->dst_image; - } else { - RCLCPP_WARN_STREAM(get_logger(), "segmentation service failed unexpectedly"); - // NOTE: Even if the segmentation service fails, the function will still return the - // yaw_angle_rad as it is and complete the initialization. The service fails - // when the DNN model is not downloaded. Ideally, initialization should rely on - // segmentation, but this implementation allows initialization even in cases where network - // connectivity is not available. - return yaw_angle_rad; - } + if (semantic_segmentation_) { + const cv::Mat src_image = + cv_bridge::toCvCopy(*latest_image_msg_.value(), sensor_msgs::image_encodings::BGR8)->image; + segmented_image = semantic_segmentation_->inference(src_image); } else { - RCLCPP_ERROR_STREAM( - get_logger(), "segmentation service did not return within the expected time"); - return std::nullopt; + RCLCPP_WARN_STREAM(get_logger(), "segmentation service failed unexpectedly"); + // NOTE: Even if the segmentation service fails, the function will still return the + // yaw_angle_rad as it is and complete the initialization. The service fails + // when the DNN model is not downloaded. Ideally, initialization should rely on + // segmentation, but this implementation allows initialization even in cases where network + // connectivity is not available. + return yaw_angle_rad; } } diff --git a/localization/yabloc/yabloc_pose_initializer/src/camera/projector_module.cpp b/localization/yabloc/yabloc_pose_initializer/src/camera/projector_module.cpp index 00f2da473b7ed..86a403f68a957 100644 --- a/localization/yabloc/yabloc_pose_initializer/src/camera/projector_module.cpp +++ b/localization/yabloc/yabloc_pose_initializer/src/camera/projector_module.cpp @@ -36,10 +36,8 @@ cv::Point2i to_cv_point(const Eigen::Vector3f & v) return pt; } -cv::Mat ProjectorModule::project_image(const sensor_msgs::msg::Image & image_msg) +cv::Mat ProjectorModule::project_image(const cv::Mat & mask_image) { - cv::Mat mask_image = common::decompress_to_cv_mat(image_msg); - // project semantics on plane std::vector masks; cv::split(mask_image, masks); diff --git a/localization/yabloc/yabloc_pose_initializer/src/camera/semantic_segmentation.cpp b/localization/yabloc/yabloc_pose_initializer/src/camera/semantic_segmentation.cpp new file mode 100644 index 0000000000000..760ada6f8310a --- /dev/null +++ b/localization/yabloc/yabloc_pose_initializer/src/camera/semantic_segmentation.cpp @@ -0,0 +1,121 @@ +// Copyright 2023 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 "yabloc_pose_initializer/camera/semantic_segmentation.hpp" + +#include +#include +#include +#include + +#include +#include + +struct SemanticSegmentation::Impl +{ + cv::dnn::Net net; +}; + +SemanticSegmentation::SemanticSegmentation(const std::string & model_path) +{ + impl_ = std::make_shared(); + impl_->net = cv::dnn::readNet(model_path); + impl_->net.setPreferableBackend(cv::dnn::DNN_BACKEND_OPENCV); + impl_->net.setPreferableTarget(cv::dnn::DNN_TARGET_CPU); +} + +cv::Mat SemanticSegmentation::make_blob(const cv::Mat & image) +{ + double scale = 1.0; + cv::Size size = cv::Size(896, 512); + cv::Scalar mean = cv::Scalar(0, 0, 0); + bool swap = true; + bool crop = false; + return cv::dnn::blobFromImage(image, scale, size, mean, swap, crop, CV_32F); +} + +cv::Mat SemanticSegmentation::convert_blob_to_image(const cv::Mat & blob) +{ + if (blob.size.dims() != 4) { + throw std::runtime_error("blob has invalid size"); + } + const int channels = blob.size[1]; + const int height = blob.size[2]; + const int width = blob.size[3]; + cv::Mat image = cv::Mat(height, width, CV_32FC4); + + for (int h = 0; h < height; ++h) { + for (int w = 0; w < width; ++w) { + cv::Vec4f vec4f; + for (int c = 0; c < channels; ++c) { + vec4f[c] = blob.at(c * height * width + h * width + w); + } + image.at(h, w) = vec4f; + } + } + return image; +} + +cv::Mat SemanticSegmentation::inference(const cv::Mat & image, double score_threshold) +{ + cv::Mat blob = make_blob(image); + impl_->net.setInput(blob); + std::vector output_layers = impl_->net.getUnconnectedOutLayersNames(); + std::vector masks; + impl_->net.forward(masks, output_layers); + + cv::Mat mask = masks[0]; + cv::Mat output = convert_blob_to_image(mask); + + cv::resize(output, output, cv::Size(image.cols, image.rows), 0, 0, cv::INTER_LINEAR); + + return normalize(output, score_threshold); +} + +cv::Mat SemanticSegmentation::normalize(const cv::Mat & mask, double score_threshold) +{ + std::vector masks; + cv::split(mask, masks); + std::vector bin_masks; + + for (size_t i = 1; i < masks.size(); ++i) { + cv::Mat bin_mask; + cv::threshold(masks[i], bin_mask, score_threshold, 255, cv::THRESH_BINARY_INV); + bin_mask.convertTo(bin_mask, CV_8UC1); + bin_masks.push_back(255 - bin_mask); + } + + cv::Mat result; + cv::merge(bin_masks, result); + return result; +} + +cv::Mat SemanticSegmentation::draw_overlay(const cv::Mat & image, const cv::Mat & segmentation) +{ + cv::Mat overlay_image = image.clone(); + return overlay_image * 0.5 + segmentation * 0.5; +} + +void SemanticSegmentation::print_error_message(const rclcpp::Logger & logger) +{ + const std::string ERROR_MESSAGE = + R"(The yabloc_pose_initializer is not working correctly because the DNN model has not been downloaded correctly. +Please check the README of yabloc_pose_initializer to know how download models.)"; + + std::istringstream stream(ERROR_MESSAGE); + std::string line; + while (std::getline(stream, line)) { + RCLCPP_ERROR_STREAM(logger, line); + } +} diff --git a/localization/yabloc/yabloc_pose_initializer/src/semantic_segmentation/semantic_segmentation_core.py b/localization/yabloc/yabloc_pose_initializer/src/semantic_segmentation/semantic_segmentation_core.py deleted file mode 100755 index 70d116fe0b295..0000000000000 --- a/localization/yabloc/yabloc_pose_initializer/src/semantic_segmentation/semantic_segmentation_core.py +++ /dev/null @@ -1,69 +0,0 @@ -#!/usr/bin/env python3 - -# Copyright 2023 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 copy - -import cv2 -import numpy as np - - -class SemanticSegmentationCore: - def __init__(self, model_path): - self.net_ = cv2.dnn.readNet(model_path) - self.net_.setPreferableBackend(cv2.dnn.DNN_BACKEND_OPENCV) - self.net_.setPreferableTarget(cv2.dnn.DNN_TARGET_CPU) - - def makeBlob(self, image: np.ndarray) -> np.ndarray: - scale = 1.0 - size = (896, 512) - mean = (0, 0, 0) - swap = True - crop = False - depth = cv2.CV_32F - return cv2.dnn.blobFromImage(image, scale, size, mean, swap, crop, depth) - - def inference(self, image: np.ndarray, score_threshold=0.5) -> np.ndarray: - blob = self.makeBlob(image) - self.net_.setInput(blob) - output_layers = self.net_.getUnconnectedOutLayersNames() - mask = self.net_.forward(output_layers)[0] - mask = np.squeeze(mask).transpose(1, 2, 0) - - mask = cv2.resize( - mask, dsize=(image.shape[1], image.shape[0]), interpolation=cv2.INTER_LINEAR - ) - - return self.__normalize(mask, score_threshold) - - def __normalize(self, mask, score_threshold=0.5) -> np.ndarray: - masks = cv2.split(mask)[1:] - bin_masks = [] - for mask in masks: - bin_mask = np.where(mask > score_threshold, 0, 1).astype("uint8") - bin_masks.append(255 - 255 * bin_mask) - return cv2.merge(bin_masks) - - def drawOverlay(self, image, segmentation) -> np.ndarray: - overlay_image = copy.deepcopy(image) - return cv2.addWeighted(overlay_image, 0.5, segmentation, 0.5, 1.0) - - -def main(): - print("This script is not intended to be run alone.") - - -if __name__ == "__main__": - main() diff --git a/localization/yabloc/yabloc_pose_initializer/src/semantic_segmentation/semantic_segmentation_server.py b/localization/yabloc/yabloc_pose_initializer/src/semantic_segmentation/semantic_segmentation_server.py deleted file mode 100755 index 03de9de572910..0000000000000 --- a/localization/yabloc/yabloc_pose_initializer/src/semantic_segmentation/semantic_segmentation_server.py +++ /dev/null @@ -1,91 +0,0 @@ -#!/usr/bin/env python3 - -# Copyright 2023 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 os -import sys - -from cv_bridge import CvBridge -import rclpy -from rclpy.node import Node -import semantic_segmentation_core as core -from sensor_msgs.msg import Image -from yabloc_pose_initializer.srv import SemanticSegmentation - -# cspell: ignore DDOWNLOAD -ERROR_MESSAGE = """\ -The yabloc_pose_initializer is not working correctly because the DNN model has not been downloaded correctly. -To download models, "-DDOWNLOAD_ARTIFACTS=ON" is required at build time. -Please see the README of yabloc_pose_initializer for more information.""" - - -class SemanticSegmentationServer(Node): - def __init__(self): - super().__init__("segmentation_server_node") - - model_path = self.declare_parameter("model_path", "").value - - self.get_logger().info("model path: " + model_path) - self.bridge_ = CvBridge() - - if os.path.exists(model_path): - self.dnn_ = core.SemanticSegmentationCore(model_path) - else: - self.dnn_ = None - self.__print_error_message() - - self.srv = self.create_service( - SemanticSegmentation, "semantic_segmentation_srv", self.on_service - ) - - def __print_error_message(self): - messages = ERROR_MESSAGE.split("\n") - for message in messages: - self.get_logger().error(message) - - def on_service(self, request, response): - if self.dnn_: - response.dst_image = self.__inference(request.src_image) - response.success = True - else: - self.__print_error_message() - response.success = False - response.dst_image = request.src_image - - return response - - def __inference(self, msg: Image): - stamp = msg.header.stamp - self.get_logger().info("Subscribed image: " + str(stamp)) - src_image = self.bridge_.imgmsg_to_cv2(msg) - - mask = self.dnn_.inference(src_image) - dst_msg = self.bridge_.cv2_to_imgmsg(mask) - dst_msg.encoding = "bgr8" - - return dst_msg - - -def main(): - rclpy.init(args=sys.argv) - - server_node = SemanticSegmentationServer() - rclpy.spin(server_node) - server_node.destroy_node() - rclpy.shutdown() - - -if __name__ == "__main__": - main() diff --git a/localization/yabloc/yabloc_pose_initializer/srv/SemanticSegmentation.srv b/localization/yabloc/yabloc_pose_initializer/srv/SemanticSegmentation.srv deleted file mode 100644 index 3daa611fd18ce..0000000000000 --- a/localization/yabloc/yabloc_pose_initializer/srv/SemanticSegmentation.srv +++ /dev/null @@ -1,4 +0,0 @@ -sensor_msgs/Image src_image ---- -bool success -sensor_msgs/Image dst_image diff --git a/map/map_height_fitter/package.xml b/map/map_height_fitter/package.xml index 8f6ccf969c9a4..f50eba9218d67 100644 --- a/map/map_height_fitter/package.xml +++ b/map/map_height_fitter/package.xml @@ -19,7 +19,6 @@ pcl_conversions rclcpp sensor_msgs - tf2 tf2_geometry_msgs tf2_ros tier4_localization_msgs diff --git a/map/map_loader/CMakeLists.txt b/map/map_loader/CMakeLists.txt index 745edd0daca99..b94eaac7ee34d 100644 --- a/map/map_loader/CMakeLists.txt +++ b/map/map_loader/CMakeLists.txt @@ -4,7 +4,7 @@ project(map_loader) find_package(autoware_cmake REQUIRED) autoware_package() -find_package(PCL REQUIRED COMPONENTS io) +find_package(PCL REQUIRED COMPONENTS common io filters) ament_auto_add_library(pointcloud_map_loader_node SHARED src/pointcloud_map_loader/pointcloud_map_loader_node.cpp diff --git a/map/map_loader/README.md b/map/map_loader/README.md index 816cfc418f016..e39b6d66af86c 100644 --- a/map/map_loader/README.md +++ b/map/map_loader/README.md @@ -141,11 +141,7 @@ Please see [the description of `GetSelectedPointCloudMap.srv`](https://github.co lanelet2_map_loader loads Lanelet2 file and publishes the map data as autoware_auto_mapping_msgs/HADMapBin message. The node projects lan/lon coordinates into arbitrary coordinates defined in `/map/map_projector_info` from `map_projection_loader`. -The node supports the following three types of coordinate systems: - -- MGRS -- LocalCartesianUTM -- local +Please see [tier4_autoware_msgs/msg/MapProjectorInfo.msg](https://github.com/tier4/tier4_autoware_msgs/blob/tier4/universe/tier4_map_msgs/msg/MapProjectorInfo.msg) for supported projector types. ### How to run 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 d2e9f66ad047a..0adc7612e9f61 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 @@ -36,8 +36,8 @@ class Lanelet2MapLoaderNode : public rclcpp::Node explicit Lanelet2MapLoaderNode(const rclcpp::NodeOptions & options); static lanelet::LaneletMapPtr load_map( - const std::string & lanelet2_filename, const std::string & lanelet2_map_projector_type, - const double & map_origin_lat = 0.0, const double & map_origin_lon = 0.0); + const std::string & lanelet2_filename, + const tier4_map_msgs::msg::MapProjectorInfo & projector_info); static HADMapBin create_map_bin_msg( const lanelet::LaneletMapPtr map, const std::string & lanelet2_filename, const rclcpp::Time & now); diff --git a/map/map_loader/package.xml b/map/map_loader/package.xml index fbc2572d0bc6d..5230d4bd03214 100644 --- a/map/map_loader/package.xml +++ b/map/map_loader/package.xml @@ -19,17 +19,13 @@ component_interface_specs component_interface_utils fmt + geography_utils geometry_msgs lanelet2_extension libpcl-all-dev pcl_conversions - pcl_ros rclcpp rclcpp_components - std_msgs - tf2_geometry_msgs - tf2_ros - tier4_autoware_utils tier4_map_msgs visualization_msgs yaml-cpp 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 c443318a146ac..259c168edcc5c 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 @@ -34,6 +34,7 @@ #include "map_loader/lanelet2_map_loader_node.hpp" #include +#include #include #include #include @@ -69,8 +70,7 @@ void Lanelet2MapLoaderNode::on_map_projector_info( const auto center_line_resolution = get_parameter("center_line_resolution").as_double(); // load map from file - const auto map = - load_map(lanelet2_filename, msg->type, msg->map_origin.latitude, msg->map_origin.longitude); + const auto map = load_map(lanelet2_filename, *msg); if (!map) { return; } @@ -88,25 +88,18 @@ void Lanelet2MapLoaderNode::on_map_projector_info( } lanelet::LaneletMapPtr Lanelet2MapLoaderNode::load_map( - const std::string & lanelet2_filename, const std::string & lanelet2_map_projector_type, - const double & map_origin_lat, const double & map_origin_lon) + const std::string & lanelet2_filename, + const tier4_map_msgs::msg::MapProjectorInfo & projector_info) { lanelet::ErrorMessages errors{}; - if (lanelet2_map_projector_type == "MGRS") { - lanelet::projection::MGRSProjector projector{}; - const lanelet::LaneletMapPtr map = lanelet::load(lanelet2_filename, projector, &errors); - if (errors.empty()) { - return map; - } - } else if (lanelet2_map_projector_type == "LocalCartesianUTM") { - lanelet::GPSPoint position{map_origin_lat, map_origin_lon}; - lanelet::Origin origin{position}; - lanelet::projection::UtmProjector projector{origin}; - const lanelet::LaneletMapPtr map = lanelet::load(lanelet2_filename, projector, &errors); + if (projector_info.projector_type != tier4_map_msgs::msg::MapProjectorInfo::LOCAL) { + std::unique_ptr projector = + geography_utils::get_lanelet2_projector(projector_info); + const lanelet::LaneletMapPtr map = lanelet::load(lanelet2_filename, *projector, &errors); if (errors.empty()) { return map; } - } else if (lanelet2_map_projector_type == "local") { + } else { // Use MGRSProjector as parser lanelet::projection::MGRSProjector projector{}; const lanelet::LaneletMapPtr map = lanelet::load(lanelet2_filename, projector, &errors); @@ -131,9 +124,6 @@ lanelet::LaneletMapPtr Lanelet2MapLoaderNode::load_map( } return map; - } else { - RCLCPP_ERROR(rclcpp::get_logger("map_loader"), "lanelet2_map_projector_type is not supported"); - return nullptr; } for (const auto & error : errors) { diff --git a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp index 53794fb93b7c6..c325f183fd783 100644 --- a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp +++ b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp @@ -127,13 +127,14 @@ void Lanelet2MapVisualizationNode::onMapBin( lanelet::utils::query::getAllPolygonsByType(viz_lanelet_map, "hatched_road_markings"); std::vector no_parking_reg_elems = lanelet::utils::query::noParkingAreas(all_lanelets); + lanelet::ConstLineStrings3d curbstones = lanelet::utils::query::curbstones(viz_lanelet_map); std_msgs::msg::ColorRGBA cl_road, cl_shoulder, cl_cross, cl_partitions, cl_pedestrian_markings, cl_ll_borders, cl_shoulder_borders, cl_stoplines, cl_trafficlights, cl_detection_areas, cl_speed_bumps, cl_crosswalks, cl_parking_lots, cl_parking_spaces, cl_lanelet_id, cl_obstacle_polygons, cl_no_stopping_areas, cl_no_obstacle_segmentation_area, cl_no_obstacle_segmentation_area_for_run_out, cl_hatched_road_markings_area, - cl_hatched_road_markings_line, cl_no_parking_areas; + cl_hatched_road_markings_line, cl_no_parking_areas, cl_curbstones; setColor(&cl_road, 0.27, 0.27, 0.27, 0.999); setColor(&cl_shoulder, 0.15, 0.15, 0.15, 0.999); setColor(&cl_cross, 0.27, 0.3, 0.27, 0.5); @@ -156,6 +157,7 @@ void Lanelet2MapVisualizationNode::onMapBin( setColor(&cl_hatched_road_markings_area, 0.3, 0.3, 0.3, 0.5); setColor(&cl_hatched_road_markings_line, 0.5, 0.5, 0.5, 0.999); setColor(&cl_no_parking_areas, 0.42, 0.42, 0.42, 0.5); + setColor(&cl_curbstones, 0.1, 0.1, 0.2, 0.999); visualization_msgs::msg::MarkerArray map_marker_array; @@ -242,6 +244,10 @@ void Lanelet2MapVisualizationNode::onMapBin( &map_marker_array, lanelet::visualization::noParkingAreasAsMarkerArray(no_parking_reg_elems, cl_no_parking_areas)); + insertMarkerArray( + &map_marker_array, + lanelet::visualization::lineStringsAsMarkerArray(curbstones, "curbstone", cl_curbstones, 0.2)); + pub_marker_->publish(map_marker_array); } diff --git a/map/map_projection_loader/CMakeLists.txt b/map/map_projection_loader/CMakeLists.txt index 8658ba172fde2..700d468ed4431 100644 --- a/map/map_projection_loader/CMakeLists.txt +++ b/map/map_projection_loader/CMakeLists.txt @@ -42,6 +42,10 @@ if(BUILD_TESTING) test/test_node_load_local_cartesian_utm_from_yaml.test.py TIMEOUT "30" ) + add_launch_test( + test/test_node_load_transverse_mercator_from_yaml.test.py + TIMEOUT "30" + ) install(DIRECTORY test/data/ DESTINATION share/${PROJECT_NAME}/test/data/ diff --git a/map/map_projection_loader/README.md b/map/map_projection_loader/README.md index e31db7890cf47..94b142da3dcf9 100644 --- a/map/map_projection_loader/README.md +++ b/map/map_projection_loader/README.md @@ -25,7 +25,7 @@ sample-map-rosbag ```yaml # map_projector_info.yaml -type: "local" +projector_type: local ``` ### Using MGRS @@ -34,8 +34,9 @@ If you want to use MGRS, please specify the MGRS grid as well. ```yaml # map_projector_info.yaml -type: "MGRS" -mgrs_grid: "54SUE" +projector_type: MGRS +vertical_datum: WGS84 +mgrs_grid: 54SUE ``` ### Using LocalCartesianUTM @@ -44,10 +45,26 @@ If you want to use local cartesian UTM, please specify the map origin as well. ```yaml # map_projector_info.yaml -type: "LocalCartesianUTM" +projector_type: LocalCartesianUTM +vertical_datum: WGS84 map_origin: - latitude: 35.6092 - longitude: 139.7303 + latitude: 35.6762 # [deg] + longitude: 139.6503 # [deg] + altitude: 0.0 # [m] +``` + +### Using TransverseMercator + +If you want to use Transverse Mercator projection, please specify the map origin as well. + +```yaml +# map_projector_info.yaml +projector_type: TransverseMercator +vertical_datum: WGS84 +map_origin: + latitude: 35.6762 # [deg] + longitude: 139.6503 # [deg] + altitude: 0.0 # [m] ``` ## Published Topics diff --git a/map/map_projection_loader/package.xml b/map/map_projection_loader/package.xml index 823cea064515b..b128c2cf15e15 100644 --- a/map/map_projection_loader/package.xml +++ b/map/map_projection_loader/package.xml @@ -16,8 +16,8 @@ component_interface_utils lanelet2_extension rclcpp - rclcpp_components tier4_map_msgs + yaml-cpp ament_cmake_gmock ament_cmake_gtest diff --git a/map/map_projection_loader/src/load_info_from_lanelet2_map.cpp b/map/map_projection_loader/src/load_info_from_lanelet2_map.cpp index 77958c20a9e75..18306e8d39e25 100644 --- a/map/map_projection_loader/src/load_info_from_lanelet2_map.cpp +++ b/map/map_projection_loader/src/load_info_from_lanelet2_map.cpp @@ -46,10 +46,15 @@ tier4_map_msgs::msg::MapProjectorInfo load_info_from_lanelet2_map(const std::str tier4_map_msgs::msg::MapProjectorInfo msg; if (is_local) { - msg.type = "local"; + msg.projector_type = tier4_map_msgs::msg::MapProjectorInfo::LOCAL; } else { - msg.type = "MGRS"; + msg.projector_type = tier4_map_msgs::msg::MapProjectorInfo::MGRS; msg.mgrs_grid = projector.getProjectedMGRSGrid(); } + + // We assume that the vertical datum of the map is WGS84 when using lanelet2 map. + // However, do note that this is not always true, and may cause problems in the future. + // Thus, please consider using the map_projector_info.yaml instead of this deprecated function. + msg.vertical_datum = tier4_map_msgs::msg::MapProjectorInfo::WGS84; return msg; } diff --git a/map/map_projection_loader/src/map_projection_loader.cpp b/map/map_projection_loader/src/map_projection_loader.cpp index 2bd8a9cfa243a..9fdba288601cc 100644 --- a/map/map_projection_loader/src/map_projection_loader.cpp +++ b/map/map_projection_loader/src/map_projection_loader.cpp @@ -27,17 +27,30 @@ tier4_map_msgs::msg::MapProjectorInfo load_info_from_yaml(const std::string & fi YAML::Node data = YAML::LoadFile(filename); tier4_map_msgs::msg::MapProjectorInfo msg; - msg.type = data["type"].as(); - if (msg.type == "MGRS") { + msg.projector_type = data["projector_type"].as(); + if (msg.projector_type == tier4_map_msgs::msg::MapProjectorInfo::MGRS) { + msg.vertical_datum = data["vertical_datum"].as(); msg.mgrs_grid = data["mgrs_grid"].as(); - } else if (msg.type == "LocalCartesianUTM") { + + } else if (msg.projector_type == tier4_map_msgs::msg::MapProjectorInfo::LOCAL_CARTESIAN_UTM) { + msg.vertical_datum = data["vertical_datum"].as(); + msg.map_origin.latitude = data["map_origin"]["latitude"].as(); + msg.map_origin.longitude = data["map_origin"]["longitude"].as(); + msg.map_origin.altitude = data["map_origin"]["altitude"].as(); + + } else if (msg.projector_type == tier4_map_msgs::msg::MapProjectorInfo::TRANSVERSE_MERCATOR) { + msg.vertical_datum = data["vertical_datum"].as(); msg.map_origin.latitude = data["map_origin"]["latitude"].as(); msg.map_origin.longitude = data["map_origin"]["longitude"].as(); - } else if (msg.type == "local") { + msg.map_origin.altitude = data["map_origin"]["altitude"].as(); + + } else if (msg.projector_type == tier4_map_msgs::msg::MapProjectorInfo::LOCAL) { ; // do nothing + } else { throw std::runtime_error( - "Invalid map projector type. Currently supported types: MGRS, LocalCartesianUTM, and local"); + "Invalid map projector type. Currently supported types: MGRS, LocalCartesianUTM, " + "TransverseMercator, and local"); } return msg; } diff --git a/map/map_projection_loader/test/data/map_projector_info_local.yaml b/map/map_projection_loader/test/data/map_projector_info_local.yaml new file mode 100644 index 0000000000000..2e5d27c3e8143 --- /dev/null +++ b/map/map_projection_loader/test/data/map_projector_info_local.yaml @@ -0,0 +1 @@ +projector_type: local diff --git a/map/map_projection_loader/test/data/map_projector_info_local_cartesian_utm.yaml b/map/map_projection_loader/test/data/map_projector_info_local_cartesian_utm.yaml new file mode 100644 index 0000000000000..854a6ad7b5a21 --- /dev/null +++ b/map/map_projection_loader/test/data/map_projector_info_local_cartesian_utm.yaml @@ -0,0 +1,6 @@ +projector_type: LocalCartesianUTM +vertical_datum: WGS84 +map_origin: + latitude: 35.6762 # [deg] + longitude: 139.6503 # [deg] + altitude: 0.0 # [m] diff --git a/map/map_projection_loader/test/data/map_projector_info_mgrs.yaml b/map/map_projection_loader/test/data/map_projector_info_mgrs.yaml new file mode 100644 index 0000000000000..5446de4898900 --- /dev/null +++ b/map/map_projection_loader/test/data/map_projector_info_mgrs.yaml @@ -0,0 +1,3 @@ +projector_type: MGRS +vertical_datum: WGS84 +mgrs_grid: 54SUE diff --git a/map/map_projection_loader/test/data/map_projector_info_transverse_mercator.yaml b/map/map_projection_loader/test/data/map_projector_info_transverse_mercator.yaml new file mode 100644 index 0000000000000..de1febeb0a283 --- /dev/null +++ b/map/map_projection_loader/test/data/map_projector_info_transverse_mercator.yaml @@ -0,0 +1,6 @@ +projector_type: TransverseMercator +vertical_datum: WGS84 +map_origin: + latitude: 35.6762 # [deg] + longitude: 139.6503 # [deg] + altitude: 0.0 # [m] diff --git a/map/map_projection_loader/test/data/projection_info_local.yaml b/map/map_projection_loader/test/data/projection_info_local.yaml deleted file mode 100644 index cea4aaf31d439..0000000000000 --- a/map/map_projection_loader/test/data/projection_info_local.yaml +++ /dev/null @@ -1 +0,0 @@ -type: local diff --git a/map/map_projection_loader/test/data/projection_info_local_cartesian_utm.yaml b/map/map_projection_loader/test/data/projection_info_local_cartesian_utm.yaml deleted file mode 100644 index c7500d37bb35a..0000000000000 --- a/map/map_projection_loader/test/data/projection_info_local_cartesian_utm.yaml +++ /dev/null @@ -1,4 +0,0 @@ -type: LocalCartesianUTM -map_origin: - latitude: 35.6762 - longitude: 139.6503 diff --git a/map/map_projection_loader/test/data/projection_info_mgrs.yaml b/map/map_projection_loader/test/data/projection_info_mgrs.yaml deleted file mode 100644 index d7687521be5fa..0000000000000 --- a/map/map_projection_loader/test/data/projection_info_mgrs.yaml +++ /dev/null @@ -1,2 +0,0 @@ -type: MGRS -mgrs_grid: 54SUE diff --git a/map/map_projection_loader/test/test_load_info_from_lanelet2_map.cpp b/map/map_projection_loader/test/test_load_info_from_lanelet2_map.cpp index 206053fd2e8d2..fdfe96434fe0d 100644 --- a/map/map_projection_loader/test/test_load_info_from_lanelet2_map.cpp +++ b/map/map_projection_loader/test/test_load_info_from_lanelet2_map.cpp @@ -95,7 +95,7 @@ TEST(TestLoadFromLanelet2Map, LoadMGRSGrid) const auto projector_info = load_info_from_lanelet2_map(output_path); // Check the result - EXPECT_EQ(projector_info.type, "MGRS"); + EXPECT_EQ(projector_info.projector_type, "MGRS"); EXPECT_EQ(projector_info.mgrs_grid, mgrs_grid); } @@ -109,7 +109,7 @@ TEST(TestLoadFromLanelet2Map, LoadLocalGrid) const auto projector_info = load_info_from_lanelet2_map(output_path); // Check the result - EXPECT_EQ(projector_info.type, "local"); + EXPECT_EQ(projector_info.projector_type, "local"); } TEST(TestLoadFromLanelet2Map, LoadNoLocalGrid) @@ -122,7 +122,7 @@ TEST(TestLoadFromLanelet2Map, LoadNoLocalGrid) const auto projector_info = load_info_from_lanelet2_map(output_path); // Check the result - EXPECT_EQ(projector_info.type, "MGRS"); + EXPECT_EQ(projector_info.projector_type, "MGRS"); } int main(int argc, char ** argv) diff --git a/map/map_projection_loader/test/test_node_load_local_cartesian_utm_from_yaml.test.py b/map/map_projection_loader/test/test_node_load_local_cartesian_utm_from_yaml.test.py index 920f5e3be11a1..ec13df010c8c3 100644 --- a/map/map_projection_loader/test/test_node_load_local_cartesian_utm_from_yaml.test.py +++ b/map/map_projection_loader/test/test_node_load_local_cartesian_utm_from_yaml.test.py @@ -33,12 +33,12 @@ logger = get_logger(__name__) -YAML_FILE_PATH = "test/data/projection_info_local_cartesian_utm.yaml" +YAML_FILE_PATH = "test/data/map_projector_info_local_cartesian_utm.yaml" @pytest.mark.launch_test def generate_test_description(): - map_projection_info_path = os.path.join( + map_projector_info_path = os.path.join( get_package_share_directory("map_projection_loader"), YAML_FILE_PATH ) @@ -48,7 +48,7 @@ def generate_test_description(): output="screen", parameters=[ { - "map_projector_info_path": map_projection_info_path, + "map_projector_info_path": map_projector_info_path, "lanelet2_map_path": "", "use_local_projector": False, }, @@ -121,17 +121,18 @@ def test_node_link(self): ) # Load the yaml file directly - map_projection_info_path = os.path.join( + map_projector_info_path = os.path.join( get_package_share_directory("map_projection_loader"), YAML_FILE_PATH ) - with open(map_projection_info_path) as f: + with open(map_projector_info_path) as f: yaml_data = yaml.load(f, Loader=yaml.FullLoader) # Test if message received self.assertIsNotNone( self.received_message, "No message received on map_projector_info topic" ) - self.assertEqual(self.received_message.type, yaml_data["type"]) + self.assertEqual(self.received_message.projector_type, yaml_data["projector_type"]) + self.assertEqual(self.received_message.vertical_datum, yaml_data["vertical_datum"]) self.assertEqual( self.received_message.map_origin.latitude, yaml_data["map_origin"]["latitude"] ) diff --git a/map/map_projection_loader/test/test_node_load_local_from_yaml.test.py b/map/map_projection_loader/test/test_node_load_local_from_yaml.test.py index 8ab0db43e32a7..ccec68486b79c 100644 --- a/map/map_projection_loader/test/test_node_load_local_from_yaml.test.py +++ b/map/map_projection_loader/test/test_node_load_local_from_yaml.test.py @@ -33,12 +33,12 @@ logger = get_logger(__name__) -YAML_FILE_PATH = "test/data/projection_info_local.yaml" +YAML_FILE_PATH = "test/data/map_projector_info_local.yaml" @pytest.mark.launch_test def generate_test_description(): - map_projection_info_path = os.path.join( + map_projector_info_path = os.path.join( get_package_share_directory("map_projection_loader"), YAML_FILE_PATH ) @@ -48,7 +48,7 @@ def generate_test_description(): output="screen", parameters=[ { - "map_projector_info_path": map_projection_info_path, + "map_projector_info_path": map_projector_info_path, "lanelet2_map_path": "", "use_local_projector": False, }, @@ -121,17 +121,17 @@ def test_node_link(self): ) # Load the yaml file directly - map_projection_info_path = os.path.join( + map_projector_info_path = os.path.join( get_package_share_directory("map_projection_loader"), YAML_FILE_PATH ) - with open(map_projection_info_path) as f: + with open(map_projector_info_path) as f: yaml_data = yaml.load(f, Loader=yaml.FullLoader) # Test if message received self.assertIsNotNone( self.received_message, "No message received on map_projector_info topic" ) - self.assertEqual(self.received_message.type, yaml_data["type"]) + self.assertEqual(self.received_message.projector_type, yaml_data["projector_type"]) self.test_node.destroy_subscription(subscription) diff --git a/map/map_projection_loader/test/test_node_load_mgrs_from_yaml.test.py b/map/map_projection_loader/test/test_node_load_mgrs_from_yaml.test.py index f18f9b7c17e75..200504d0ea18f 100644 --- a/map/map_projection_loader/test/test_node_load_mgrs_from_yaml.test.py +++ b/map/map_projection_loader/test/test_node_load_mgrs_from_yaml.test.py @@ -33,12 +33,12 @@ logger = get_logger(__name__) -YAML_FILE_PATH = "test/data/projection_info_mgrs.yaml" +YAML_FILE_PATH = "test/data/map_projector_info_mgrs.yaml" @pytest.mark.launch_test def generate_test_description(): - map_projection_info_path = os.path.join( + map_projector_info_path = os.path.join( get_package_share_directory("map_projection_loader"), YAML_FILE_PATH ) @@ -48,7 +48,7 @@ def generate_test_description(): output="screen", parameters=[ { - "map_projector_info_path": map_projection_info_path, + "map_projector_info_path": map_projector_info_path, "lanelet2_map_path": "", "use_local_projector": False, }, @@ -121,17 +121,18 @@ def test_node_link(self): ) # Load the yaml file directly - map_projection_info_path = os.path.join( + map_projector_info_path = os.path.join( get_package_share_directory("map_projection_loader"), YAML_FILE_PATH ) - with open(map_projection_info_path) as f: + with open(map_projector_info_path) as f: yaml_data = yaml.load(f, Loader=yaml.FullLoader) # Test if message received self.assertIsNotNone( self.received_message, "No message received on map_projector_info topic" ) - self.assertEqual(self.received_message.type, yaml_data["type"]) + self.assertEqual(self.received_message.projector_type, yaml_data["projector_type"]) + self.assertEqual(self.received_message.vertical_datum, yaml_data["vertical_datum"]) self.assertEqual(self.received_message.mgrs_grid, yaml_data["mgrs_grid"]) self.test_node.destroy_subscription(subscription) diff --git a/map/map_projection_loader/test/test_node_load_transverse_mercator_from_yaml.test.py b/map/map_projection_loader/test/test_node_load_transverse_mercator_from_yaml.test.py new file mode 100644 index 0000000000000..7883ae6aa3c99 --- /dev/null +++ b/map/map_projection_loader/test/test_node_load_transverse_mercator_from_yaml.test.py @@ -0,0 +1,150 @@ +#!/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 os +import unittest + +from ament_index_python import get_package_share_directory +import launch +from launch import LaunchDescription +from launch.logging import get_logger +from launch_ros.actions import Node +import launch_testing +import pytest +import rclpy +from rclpy.qos import QoSDurabilityPolicy +from rclpy.qos import QoSHistoryPolicy +from rclpy.qos import QoSProfile +from tier4_map_msgs.msg import MapProjectorInfo +import yaml + +logger = get_logger(__name__) + +YAML_FILE_PATH = "test/data/map_projector_info_transverse_mercator.yaml" + + +@pytest.mark.launch_test +def generate_test_description(): + map_projector_info_path = os.path.join( + get_package_share_directory("map_projection_loader"), YAML_FILE_PATH + ) + + map_projection_loader_node = Node( + package="map_projection_loader", + executable="map_projection_loader", + output="screen", + parameters=[ + { + "map_projector_info_path": map_projector_info_path, + "lanelet2_map_path": "", + "use_local_projector": False, + }, + ], + ) + + context = {} + + return ( + LaunchDescription( + [ + map_projection_loader_node, + # Start test after 1s - gives time for the static_centerline_optimizer to finish initialization + launch.actions.TimerAction( + period=1.0, actions=[launch_testing.actions.ReadyToTest()] + ), + ] + ), + context, + ) + + +class TestLoadUTMFromYaml(unittest.TestCase): + @classmethod + def setUpClass(cls): + # Initialize the ROS context for the test node + rclpy.init() + + @classmethod + def tearDownClass(cls): + # Shutdown the ROS context + rclpy.shutdown() + + def setUp(self): + # Create a ROS node for tests + self.test_node = rclpy.create_node("test_node") + self.evaluation_time = 0.2 # 200ms + self.received_message = None + + def tearDown(self): + self.test_node.destroy_node() + + def callback(self, msg): + self.received_message = msg + + @staticmethod + def print_message(stat): + logger.debug("===========================") + logger.debug(stat) + + def test_node_link(self): + # Create custom QoS profile for subscription + custom_qos_profile = QoSProfile( + depth=1, + history=QoSHistoryPolicy.KEEP_LAST, + durability=QoSDurabilityPolicy.TRANSIENT_LOCAL, + ) + + # Create subscription to map_projector_info topic + subscription = self.test_node.create_subscription( + MapProjectorInfo, + "/map/map_projector_info", + self.callback, + custom_qos_profile, + ) + + # Give time for the message to be received and processed + rclpy.spin_until_future_complete( + self.test_node, rclpy.task.Future(), timeout_sec=self.evaluation_time + ) + + # Load the yaml file directly + map_projector_info_path = os.path.join( + get_package_share_directory("map_projection_loader"), YAML_FILE_PATH + ) + with open(map_projector_info_path) as f: + yaml_data = yaml.load(f, Loader=yaml.FullLoader) + + # Test if message received + self.assertIsNotNone( + self.received_message, "No message received on map_projector_info topic" + ) + self.assertEqual(self.received_message.projector_type, yaml_data["projector_type"]) + self.assertEqual(self.received_message.vertical_datum, yaml_data["vertical_datum"]) + self.assertEqual( + self.received_message.map_origin.latitude, yaml_data["map_origin"]["latitude"] + ) + self.assertEqual( + self.received_message.map_origin.longitude, yaml_data["map_origin"]["longitude"] + ) + + self.test_node.destroy_subscription(subscription) + + +@launch_testing.post_shutdown_test() +class TestProcessOutput(unittest.TestCase): + def test_exit_code(self, proc_info): + # Check that process exits with code 0: no error + launch_testing.asserts.assertExitCodes(proc_info) diff --git a/map/util/lanelet2_map_preprocessor/CMakeLists.txt b/map/util/lanelet2_map_preprocessor/CMakeLists.txt index caa1d27b3e292..8a6b16c05011b 100644 --- a/map/util/lanelet2_map_preprocessor/CMakeLists.txt +++ b/map/util/lanelet2_map_preprocessor/CMakeLists.txt @@ -4,12 +4,18 @@ project(lanelet2_map_preprocessor) find_package(autoware_cmake REQUIRED) autoware_package() +find_package(PCL REQUIRED COMPONENTS common io kdtree) + include_directories( include SYSTEM ${PCL_INCLUDE_DIRS} ) +link_libraries( + ${PCL_LIBRARIES} +) + ament_auto_add_executable(fix_z_value_by_pcd src/fix_z_value_by_pcd.cpp) ament_auto_add_executable(transform_maps src/transform_maps.cpp) ament_auto_add_executable(merge_close_lines src/merge_close_lines.cpp) diff --git a/map/util/lanelet2_map_preprocessor/package.xml b/map/util/lanelet2_map_preprocessor/package.xml index 15f061369e649..79fce94f87a95 100644 --- a/map/util/lanelet2_map_preprocessor/package.xml +++ b/map/util/lanelet2_map_preprocessor/package.xml @@ -11,7 +11,7 @@ autoware_cmake lanelet2_extension - pcl_ros + libpcl-all-dev rclcpp ament_lint_auto diff --git a/map/util/lanelet2_map_preprocessor/src/fix_lane_change_tags.cpp b/map/util/lanelet2_map_preprocessor/src/fix_lane_change_tags.cpp index 601e8dfa0117f..8900381060472 100644 --- a/map/util/lanelet2_map_preprocessor/src/fix_lane_change_tags.cpp +++ b/map/util/lanelet2_map_preprocessor/src/fix_lane_change_tags.cpp @@ -21,6 +21,7 @@ #include #include #include +#include #include #include diff --git a/mkdocs.yaml b/mkdocs.yaml index a72529fbe959a..2ca8b3c86ad43 100644 --- a/mkdocs.yaml +++ b/mkdocs.yaml @@ -55,7 +55,8 @@ plugins: regex: - ^(?!(.*/)?assets/).*\.(?!(.*\.)?md|(.*\.)?svg|(.*\.)?png|(.*\.)?gif|(.*\.)?jpg).*$ - ^(.*/)?[^.]*$ - - macros + - macros: + module_name: mkdocs_macros - mkdocs-video - same-dir - search diff --git a/mkdocs_macros.py b/mkdocs_macros.py new file mode 100644 index 0000000000000..56292a815d80d --- /dev/null +++ b/mkdocs_macros.py @@ -0,0 +1,70 @@ +import json + +from tabulate import tabulate + +# This file is for defining macros for mkdocs-macros plugin +# Check https://mkdocs-macros-plugin.readthedocs.io/en/latest/macros/ for the details + + +def format_param_type(param_type): + if param_type == "number": + return "float" + else: + return param_type + + +def format_param_range(param): + list_of_range = [] + if "enum" in param.keys(): + list_of_range.append(param["enum"]) + if "minimum" in param.keys(): + list_of_range.append("≥" + str(param["minimum"])) + if "exclusiveMinimum" in param.keys(): + list_of_range.append(">" + str(param["exclusiveMinimum"])) + if "maximum" in param.keys(): + list_of_range.append("≤" + str(param["maximum"])) + if "exclusiveMaximum" in param.keys(): + list_of_range.append("<" + str(param["exclusiveMaximum"])) + if "exclusive" in param.keys(): + list_of_range.append("≠" + str(param["exclusive"])) + + if len(list_of_range) == 0: + return "N/A" + else: + range_in_text = "" + for item in list_of_range: + if range_in_text != "": + range_in_text += "
" + range_in_text += str(item) + return range_in_text + + +def extract_parameter_info(parameters, namespace=""): + params = [] + for k, v in parameters.items(): + if v["type"] != "object": + param = {} + param["Name"] = namespace + k + param["Type"] = format_param_type(v["type"]) + param["Description"] = v["description"] + param["Default"] = v["default"] + param["Range"] = format_param_range(v) + params.append(param) + else: # if the object is namespace, then dive deeper in to json value + params.extend(extract_parameter_info(v["properties"], k + ".")) + return params + + +def format_json(json_data): + parameters = list(json_data["definitions"].values())[0]["properties"] + # cspell: ignore tablefmt + markdown_table = tabulate(extract_parameter_info(parameters), headers="keys", tablefmt="github") + return markdown_table + + +def define_env(env): + @env.macro + def json_to_markdown(json_schema_file_path): + with open(json_schema_file_path) as f: + data = json.load(f) + return format_json(data) diff --git a/perception/cluster_merger/CMakeLists.txt b/perception/cluster_merger/CMakeLists.txt new file mode 100644 index 0000000000000..49506f4b439fb --- /dev/null +++ b/perception/cluster_merger/CMakeLists.txt @@ -0,0 +1,27 @@ +cmake_minimum_required(VERSION 3.8) +project(cluster_merger) + +# find dependencies +find_package(autoware_cmake REQUIRED) +autoware_package() + +# Targets +ament_auto_add_library(cluster_merger_node_component SHARED + src/cluster_merger/node.cpp +) + +rclcpp_components_register_node(cluster_merger_node_component + PLUGIN "cluster_merger::ClusterMergerNode" + EXECUTABLE cluster_merger_node) + + +if(BUILD_TESTING) + list(APPEND AMENT_LINT_AUTO_EXCLUDE ament_cmake_uncrustify) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package( + INSTALL_TO_SHARE + launch +) diff --git a/perception/cluster_merger/README.md b/perception/cluster_merger/README.md new file mode 100644 index 0000000000000..b46f7401b70ec --- /dev/null +++ b/perception/cluster_merger/README.md @@ -0,0 +1,72 @@ +# cluster merger + +## Purpose + +cluster_merger is a package for merging pointcloud clusters as detected objects with feature type. + +## Inner-working / Algorithms + +The clusters of merged topics are simply concatenated from clusters of input topics. + +## Input / Output + +### Input + +| Name | Type | Description | +| ---------------- | -------------------------------------------------------- | ------------------- | +| `input/cluster0` | `tier4_perception_msgs::msg::DetectedObjectsWithFeature` | pointcloud clusters | +| `input/cluster1` | `tier4_perception_msgs::msg::DetectedObjectsWithFeature` | pointcloud clusters | + +### Output + +| Name | Type | Description | +| ----------------- | ----------------------------------------------------- | --------------- | +| `output/clusters` | `autoware_auto_perception_msgs::msg::DetectedObjects` | merged clusters | + +## Parameters + +| Name | Type | Description | Default value | +| :---------------- | :----- | :----------------------------------- | :------------ | +| `output_frame_id` | string | The header frame_id of output topic. | base_link | + +## Assumptions / Known limits + + + +## (Optional) Error detection and handling + + + +## (Optional) Performance characterization + + + +## (Optional) References/External links + + + +## (Optional) Future extensions / Unimplemented parts diff --git a/perception/cluster_merger/include/cluster_merger/node.hpp b/perception/cluster_merger/include/cluster_merger/node.hpp new file mode 100644 index 0000000000000..f36116378af78 --- /dev/null +++ b/perception/cluster_merger/include/cluster_merger/node.hpp @@ -0,0 +1,73 @@ +// Copyright 2023 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 CLUSTER_MERGER__NODE_HPP_ +#define CLUSTER_MERGER__NODE_HPP_ + +#include "message_filters/subscriber.h" +#include "message_filters/sync_policies/approximate_time.h" +#include "message_filters/synchronizer.h" +#include "rclcpp/rclcpp.hpp" +#include "tier4_autoware_utils/ros/transform_listener.hpp" + +#include "tier4_perception_msgs/msg/detected_objects_with_feature.hpp" + +#include +#include + +#include +#include +#include +#include + +namespace cluster_merger +{ +using tier4_perception_msgs::msg::DetectedObjectsWithFeature; +using tier4_perception_msgs::msg::DetectedObjectWithFeature; + +class ClusterMergerNode : public rclcpp::Node +{ +public: + explicit ClusterMergerNode(const rclcpp::NodeOptions & node_options); + +private: + // Subscriber + + tf2_ros::Buffer tf_buffer_; + tf2_ros::TransformListener tf_listener_; + + rclcpp::Subscription::SharedPtr sub_objects_{}; + message_filters::Subscriber objects0_sub_; + message_filters::Subscriber objects1_sub_; + typedef message_filters::sync_policies::ApproximateTime< + DetectedObjectsWithFeature, DetectedObjectsWithFeature> + SyncPolicy; + typedef message_filters::Synchronizer Sync; + Sync sync_; + + std::string output_frame_id_; + + std::vector::SharedPtr> sub_objects_array{}; + std::shared_ptr transform_listener_; + + void objectsCallback( + const DetectedObjectsWithFeature::ConstSharedPtr & input_objects0_msg, + const DetectedObjectsWithFeature::ConstSharedPtr & input_objects1_msg); + // Publisher + rclcpp::Publisher::SharedPtr pub_objects_; +}; + +} // namespace cluster_merger + +#endif // CLUSTER_MERGER__NODE_HPP_ diff --git a/perception/cluster_merger/launch/cluster_merger.launch.xml b/perception/cluster_merger/launch/cluster_merger.launch.xml new file mode 100644 index 0000000000000..1bbd0ebd91e12 --- /dev/null +++ b/perception/cluster_merger/launch/cluster_merger.launch.xml @@ -0,0 +1,14 @@ + + + + + + + + + + + + + + diff --git a/perception/cluster_merger/package.xml b/perception/cluster_merger/package.xml new file mode 100644 index 0000000000000..14826ad07e098 --- /dev/null +++ b/perception/cluster_merger/package.xml @@ -0,0 +1,28 @@ + + + + cluster_merger + 0.1.0 + The ROS 2 cluster merger package + Yukihiro Saito + Shunsuke Miura + autoware + Apache License 2.0 + + ament_cmake_auto + + geometry_msgs + message_filters + object_recognition_utils + rclcpp + rclcpp_components + tier4_autoware_utils + tier4_perception_msgs + autoware_cmake + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/perception/cluster_merger/src/cluster_merger/node.cpp b/perception/cluster_merger/src/cluster_merger/node.cpp new file mode 100644 index 0000000000000..48bf953027510 --- /dev/null +++ b/perception/cluster_merger/src/cluster_merger/node.cpp @@ -0,0 +1,73 @@ +// Copyright 2023 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 "cluster_merger/node.hpp" + +#include "object_recognition_utils/object_recognition_utils.hpp" + +#include +#include +#include +namespace cluster_merger +{ + +ClusterMergerNode::ClusterMergerNode(const rclcpp::NodeOptions & node_options) +: rclcpp::Node("cluster_merger_node", node_options), + tf_buffer_(get_clock()), + tf_listener_(tf_buffer_), + objects0_sub_(this, "input/cluster0", rclcpp::QoS{1}.get_rmw_qos_profile()), + objects1_sub_(this, "input/cluster1", rclcpp::QoS{1}.get_rmw_qos_profile()), + sync_(SyncPolicy(10), objects0_sub_, objects1_sub_) +{ + output_frame_id_ = declare_parameter("output_frame_id"); + using std::placeholders::_1; + using std::placeholders::_2; + sync_.registerCallback(std::bind(&ClusterMergerNode::objectsCallback, this, _1, _2)); + + // Publisher + pub_objects_ = create_publisher("~/output/clusters", rclcpp::QoS{1}); +} + +void ClusterMergerNode::objectsCallback( + const DetectedObjectsWithFeature::ConstSharedPtr & input_objects0_msg, + const DetectedObjectsWithFeature::ConstSharedPtr & input_objects1_msg) +{ + if (pub_objects_->get_subscription_count() < 1) { + return; + } + // TODO(badai-nguyen): transform input topics to desired frame before concatenating + /* transform to the same with cluster0 frame id*/ + DetectedObjectsWithFeature transformed_objects0; + DetectedObjectsWithFeature transformed_objects1; + if ( + !object_recognition_utils::transformObjectsWithFeature( + *input_objects0_msg, output_frame_id_, tf_buffer_, transformed_objects0) || + !object_recognition_utils::transformObjectsWithFeature( + *input_objects1_msg, output_frame_id_, tf_buffer_, transformed_objects1)) { + return; + } + + DetectedObjectsWithFeature output_objects; + output_objects.header = input_objects0_msg->header; + // add check frame id and transform if they are different + output_objects.feature_objects = transformed_objects0.feature_objects; + output_objects.feature_objects.insert( + output_objects.feature_objects.end(), transformed_objects1.feature_objects.begin(), + transformed_objects1.feature_objects.end()); + pub_objects_->publish(output_objects); +} +} // namespace cluster_merger + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(cluster_merger::ClusterMergerNode) diff --git a/perception/crosswalk_traffic_light_estimator/CMakeLists.txt b/perception/crosswalk_traffic_light_estimator/CMakeLists.txt index 11a6b232caf90..2947070eca01e 100644 --- a/perception/crosswalk_traffic_light_estimator/CMakeLists.txt +++ b/perception/crosswalk_traffic_light_estimator/CMakeLists.txt @@ -28,6 +28,8 @@ endif() ## Install ## ############# -ament_auto_package(INSTALL_TO_SHARE - launch +ament_auto_package( + INSTALL_TO_SHARE + launch + config ) diff --git a/perception/crosswalk_traffic_light_estimator/README.md b/perception/crosswalk_traffic_light_estimator/README.md index e23b454905081..73c7151997c54 100644 --- a/perception/crosswalk_traffic_light_estimator/README.md +++ b/perception/crosswalk_traffic_light_estimator/README.md @@ -22,9 +22,10 @@ ## Parameters -| Name | Type | Description | Default value | -| :---------------------- | :----- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | -| `use_last_detect_color` | `bool` | If this parameter is `true`, this module estimates pedestrian's traffic signal as RED not only when vehicle's traffic signal is detected as GREEN/AMBER but also when detection results change GREEN/AMBER to UNKNOWN. (If detection results change RED or AMBER to UNKNOWN, this module estimates pedestrian's traffic signal as UNKNOWN.) If this parameter is `false`, this module use only latest detection results for estimation. (Only when the detection result is GREEN/AMBER, this module estimates pedestrian's traffic signal as RED.) | `true` | +| Name | Type | Description | Default value | +| :---------------------------- | :------- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | +| `use_last_detect_color` | `bool` | If this parameter is `true`, this module estimates pedestrian's traffic signal as RED not only when vehicle's traffic signal is detected as GREEN/AMBER but also when detection results change GREEN/AMBER to UNKNOWN. (If detection results change RED or AMBER to UNKNOWN, this module estimates pedestrian's traffic signal as UNKNOWN.) If this parameter is `false`, this module use only latest detection results for estimation. (Only when the detection result is GREEN/AMBER, this module estimates pedestrian's traffic signal as RED.) | `true` | +| `last_detect_color_hold_time` | `double` | The time threshold to hold for last detect color. | `2.0` | ## Inner-workings / Algorithms diff --git a/perception/crosswalk_traffic_light_estimator/config/crosswalk_traffic_light_estimator.param.yaml b/perception/crosswalk_traffic_light_estimator/config/crosswalk_traffic_light_estimator.param.yaml new file mode 100644 index 0000000000000..33168c87de7eb --- /dev/null +++ b/perception/crosswalk_traffic_light_estimator/config/crosswalk_traffic_light_estimator.param.yaml @@ -0,0 +1,18 @@ +# Copyright 2023 UCI SORA Lab +# +# 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. + +/**: + ros__parameters: + use_last_detect_color: true + last_detect_color_hold_time: 2.0 diff --git a/perception/crosswalk_traffic_light_estimator/include/crosswalk_traffic_light_estimator/node.hpp b/perception/crosswalk_traffic_light_estimator/include/crosswalk_traffic_light_estimator/node.hpp index 0850436851e6a..e38b747a6ce67 100644 --- a/perception/crosswalk_traffic_light_estimator/include/crosswalk_traffic_light_estimator/node.hpp +++ b/perception/crosswalk_traffic_light_estimator/include/crosswalk_traffic_light_estimator/node.hpp @@ -15,13 +15,12 @@ #ifndef CROSSWALK_TRAFFIC_LIGHT_ESTIMATOR__NODE_HPP_ #define CROSSWALK_TRAFFIC_LIGHT_ESTIMATOR__NODE_HPP_ -#include -#include -#include #include -#include +#include +#include #include +#include #include #include #include @@ -30,6 +29,7 @@ #include #include +#include #include #include #include @@ -47,9 +47,9 @@ using autoware_planning_msgs::msg::LaneletRoute; using tier4_autoware_utils::DebugPublisher; using tier4_autoware_utils::StopWatch; using tier4_debug_msgs::msg::Float64Stamped; -using tier4_perception_msgs::msg::TrafficLightElement; -using tier4_perception_msgs::msg::TrafficSignal; -using tier4_perception_msgs::msg::TrafficSignalArray; +using TrafficSignal = autoware_perception_msgs::msg::TrafficSignal; +using TrafficSignalArray = autoware_perception_msgs::msg::TrafficSignalArray; +using TrafficSignalElement = autoware_perception_msgs::msg::TrafficSignalElement; using TrafficSignalAndTime = std::pair; using TrafficLightIdMap = std::unordered_map; @@ -89,6 +89,9 @@ class CrosswalkTrafficLightEstimatorNode : public rclcpp::Node const lanelet::ConstLineStringsOrPolygons3d & traffic_lights, const TrafficLightIdMap & traffic_light_id_map) const; + boost::optional getHighestConfidenceTrafficSignal( + const lanelet::Id & id, const TrafficLightIdMap & traffic_light_id_map) const; + // Node param bool use_last_detect_color_; double last_detect_color_hold_time_; diff --git a/perception/crosswalk_traffic_light_estimator/launch/crosswalk_traffic_light_estimator.launch.xml b/perception/crosswalk_traffic_light_estimator/launch/crosswalk_traffic_light_estimator.launch.xml index 91bec60c90aee..21d718c3439cd 100644 --- a/perception/crosswalk_traffic_light_estimator/launch/crosswalk_traffic_light_estimator.launch.xml +++ b/perception/crosswalk_traffic_light_estimator/launch/crosswalk_traffic_light_estimator.launch.xml @@ -1,9 +1,23 @@ - + + + + diff --git a/perception/crosswalk_traffic_light_estimator/package.xml b/perception/crosswalk_traffic_light_estimator/package.xml index 435ecf6e6fa3d..bec5b666683c4 100644 --- a/perception/crosswalk_traffic_light_estimator/package.xml +++ b/perception/crosswalk_traffic_light_estimator/package.xml @@ -5,6 +5,8 @@ The crosswalk_traffic_light_estimator package Satoshi Ota + Shunsuke Miura + Tao Zhong Apache License 2.0 ament_cmake_auto @@ -12,6 +14,7 @@ autoware_auto_mapping_msgs autoware_auto_planning_msgs + autoware_perception_msgs autoware_planning_msgs lanelet2_extension rclcpp diff --git a/perception/crosswalk_traffic_light_estimator/schema/crosswalk_traffic_light_estimator.schema.json b/perception/crosswalk_traffic_light_estimator/schema/crosswalk_traffic_light_estimator.schema.json new file mode 100644 index 0000000000000..ecc273263236a --- /dev/null +++ b/perception/crosswalk_traffic_light_estimator/schema/crosswalk_traffic_light_estimator.schema.json @@ -0,0 +1,36 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Crosswalk Traffic Light Estimator Node", + "type": "object", + "definitions": { + "crosswalk_traffic_light_estimator": { + "type": "object", + "properties": { + "use_last_detect_color": { + "type": "boolean", + "default": true, + "description": "If this parameter is true, this module estimates pedestrian's traffic signal as RED not only when vehicle's traffic signal is detected as GREEN/AMBER but also when detection results change GREEN/AMBER to UNKNOWN. (If detection results change RED or AMBER to UNKNOWN, this module estimates pedestrian's traffic signal as UNKNOWN.) If this parameter is false, this module use only latest detection results for estimation. (Only when the detection result is GREEN/AMBER, this module estimates pedestrian's traffic signal as RED.)." + }, + "last_detect_color_hold_time": { + "type": "number", + "default": 2.0, + "exclusiveMinimum": 0.0, + "description": "The time threshold to hold for last detect color." + } + }, + "required": ["use_last_detect_color", "last_detect_color_hold_time"] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/crosswalk_traffic_light_estimator" + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] +} diff --git a/perception/crosswalk_traffic_light_estimator/src/node.cpp b/perception/crosswalk_traffic_light_estimator/src/node.cpp index 55d272cb71cfe..870b8bc7c13f5 100644 --- a/perception/crosswalk_traffic_light_estimator/src/node.cpp +++ b/perception/crosswalk_traffic_light_estimator/src/node.cpp @@ -1,4 +1,4 @@ -// Copyright 2022 TIER IV, Inc. +// Copyright 2022-2023 UCI SORA Lab, 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. @@ -13,6 +13,7 @@ // limitations under the License. #include "crosswalk_traffic_light_estimator/node.hpp" +#include #include #include @@ -81,8 +82,8 @@ CrosswalkTrafficLightEstimatorNode::CrosswalkTrafficLightEstimatorNode( { using std::placeholders::_1; - use_last_detect_color_ = this->declare_parameter("use_last_detect_color", true); - last_detect_color_hold_time_ = this->declare_parameter("last_detect_color_hold_time", 2.0); + use_last_detect_color_ = declare_parameter("use_last_detect_color"); + last_detect_color_hold_time_ = declare_parameter("last_detect_color_hold_time"); sub_map_ = create_subscription( "~/input/vector_map", rclcpp::QoS{1}.transient_local(), @@ -160,7 +161,7 @@ void CrosswalkTrafficLightEstimatorNode::onTrafficLightArray( TrafficLightIdMap traffic_light_id_map; for (const auto & traffic_signal : msg->signals) { - traffic_light_id_map[traffic_signal.traffic_light_id] = + traffic_light_id_map[traffic_signal.traffic_signal_id] = std::pair(traffic_signal, get_clock()->now()); } @@ -191,11 +192,11 @@ void CrosswalkTrafficLightEstimatorNode::updateLastDetectedSignal( continue; } - if (elements.front().color == TrafficLightElement::UNKNOWN) { + if (elements.front().color == TrafficSignalElement::UNKNOWN) { continue; } - const auto & id = input_traffic_signal.second.first.traffic_light_id; + const auto & id = input_traffic_signal.second.first.traffic_signal_id; if (last_detect_color_.count(id) == 0) { last_detect_color_.insert(std::make_pair(id, input_traffic_signal.second)); @@ -207,7 +208,7 @@ void CrosswalkTrafficLightEstimatorNode::updateLastDetectedSignal( std::vector erase_id_list; for (auto & last_traffic_signal : last_detect_color_) { - const auto & id = last_traffic_signal.second.first.traffic_light_id; + const auto & id = last_traffic_signal.second.first.traffic_signal_id; if (traffic_light_id_map.count(id) == 0) { // hold signal recognition results for [last_detect_color_hold_time_] seconds. @@ -227,19 +228,14 @@ void CrosswalkTrafficLightEstimatorNode::setCrosswalkTrafficSignal( const auto tl_reg_elems = crosswalk.regulatoryElementsAs(); for (const auto & tl_reg_elem : tl_reg_elems) { - const auto crosswalk_traffic_lights = tl_reg_elem->trafficLights(); - - for (const auto & traffic_light : crosswalk_traffic_lights) { - const auto ll_traffic_light = static_cast(traffic_light); - - TrafficSignal output_traffic_signal; - TrafficLightElement output_traffic_light; - output_traffic_light.color = color; - output_traffic_light.confidence = 1.0; - output_traffic_signal.elements.push_back(output_traffic_light); - output_traffic_signal.traffic_light_id = ll_traffic_light.id(); - msg.signals.push_back(output_traffic_signal); - } + TrafficSignal output_traffic_signal; + TrafficSignalElement output_traffic_signal_element; + output_traffic_signal_element.color = color; + output_traffic_signal_element.shape = TrafficSignalElement::CIRCLE; + output_traffic_signal_element.confidence = 1.0; + output_traffic_signal.elements.push_back(output_traffic_signal_element); + output_traffic_signal.traffic_signal_id = tl_reg_elem->id(); + msg.signals.push_back(output_traffic_signal); } } @@ -256,34 +252,32 @@ lanelet::ConstLanelets CrosswalkTrafficLightEstimatorNode::getNonRedLanelets( } const auto tl_reg_elem = tl_reg_elems.front(); - const auto traffic_lights_for_vehicle = tl_reg_elem->trafficLights(); - const auto current_detected_signal = - getHighestConfidenceTrafficSignal(traffic_lights_for_vehicle, traffic_light_id_map); + getHighestConfidenceTrafficSignal(tl_reg_elem->id(), traffic_light_id_map); if (!current_detected_signal && !use_last_detect_color_) { continue; } const auto current_is_not_red = - current_detected_signal ? current_detected_signal.get() == TrafficLightElement::GREEN || - current_detected_signal.get() == TrafficLightElement::AMBER + current_detected_signal ? current_detected_signal.get() == TrafficSignalElement::GREEN || + current_detected_signal.get() == TrafficSignalElement::AMBER : true; const auto current_is_unknown_or_none = - current_detected_signal ? current_detected_signal.get() == TrafficLightElement::UNKNOWN + current_detected_signal ? current_detected_signal.get() == TrafficSignalElement::UNKNOWN : true; const auto last_detected_signal = - getHighestConfidenceTrafficSignal(traffic_lights_for_vehicle, last_detect_color_); + getHighestConfidenceTrafficSignal(tl_reg_elem->id(), last_detect_color_); if (!last_detected_signal) { continue; } const auto was_not_red = current_is_unknown_or_none && - (last_detected_signal.get() == TrafficLightElement::GREEN || - last_detected_signal.get() == TrafficLightElement::AMBER) && + (last_detected_signal.get() == TrafficSignalElement::GREEN || + last_detected_signal.get() == TrafficSignalElement::AMBER) && use_last_detect_color_; if (!current_is_not_red && !was_not_red) { @@ -324,13 +318,13 @@ uint8_t CrosswalkTrafficLightEstimatorNode::estimateCrosswalkTrafficSignal( } if (has_straight_non_red_lane || has_related_non_red_tl) { - return TrafficLightElement::RED; + return TrafficSignalElement::RED; } const auto has_merge_lane = hasMergeLane(non_red_lanelets, routing_graph_ptr_); return !has_merge_lane && has_left_non_red_lane && has_right_non_red_lane - ? TrafficLightElement::RED - : TrafficLightElement::UNKNOWN; + ? TrafficSignalElement::RED + : TrafficSignalElement::UNKNOWN; } boost::optional CrosswalkTrafficLightEstimatorNode::getHighestConfidenceTrafficSignal( @@ -367,6 +361,28 @@ boost::optional CrosswalkTrafficLightEstimatorNode::getHighestConfidenc return ret; } + +boost::optional CrosswalkTrafficLightEstimatorNode::getHighestConfidenceTrafficSignal( + const lanelet::Id & id, const TrafficLightIdMap & traffic_light_id_map) const +{ + boost::optional ret{boost::none}; + + double highest_confidence = 0.0; + if (traffic_light_id_map.count(id) == 0) { + return ret; + } + + for (const auto & element : traffic_light_id_map.at(id).first.elements) { + if (element.confidence < highest_confidence) { + continue; + } + + highest_confidence = element.confidence; + ret = element.color; + } + + return ret; +} } // namespace traffic_light #include diff --git a/perception/detected_object_validation/config/obstacle_pointcloud_based_validator.param.yaml b/perception/detected_object_validation/config/obstacle_pointcloud_based_validator.param.yaml new file mode 100644 index 0000000000000..745a0fd6591a9 --- /dev/null +++ b/perception/detected_object_validation/config/obstacle_pointcloud_based_validator.param.yaml @@ -0,0 +1,13 @@ +/**: + ros__parameters: + min_points_num: + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN + [10, 10, 10, 10, 10, 10, 10, 10] + + max_points_num: + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN + [10, 10, 10, 10, 10, 10, 10, 10] + + min_points_and_distance_ratio: + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN + [800.0, 800.0, 800.0, 800.0, 800.0, 800.0, 800.0, 800.0] diff --git a/perception/detected_object_validation/include/detected_object_filter/object_lanelet_filter.hpp b/perception/detected_object_validation/include/detected_object_filter/object_lanelet_filter.hpp index d1d3cf9e904c5..ae6162542a1c3 100644 --- a/perception/detected_object_validation/include/detected_object_filter/object_lanelet_filter.hpp +++ b/perception/detected_object_validation/include/detected_object_filter/object_lanelet_filter.hpp @@ -17,14 +17,13 @@ #include "utils/utils.hpp" -#include -#include #include #include #include #include +#include #include #include @@ -52,6 +51,7 @@ class ObjectLaneletFilterNode : public rclcpp::Node lanelet::LaneletMapPtr lanelet_map_ptr_; lanelet::ConstLanelets road_lanelets_; + lanelet::ConstLanelets shoulder_lanelets_; std::string lanelet_frame_id_; tf2_ros::Buffer tf_buffer_; diff --git a/perception/detected_object_validation/include/detected_object_filter/object_position_filter.hpp b/perception/detected_object_validation/include/detected_object_filter/object_position_filter.hpp index b3ab5d104aee7..b9384e0f70379 100644 --- a/perception/detected_object_validation/include/detected_object_filter/object_position_filter.hpp +++ b/perception/detected_object_validation/include/detected_object_filter/object_position_filter.hpp @@ -17,8 +17,6 @@ #include "utils/utils.hpp" -#include -#include #include #include diff --git a/perception/detected_object_validation/include/obstacle_pointcloud_based_validator/debugger.hpp b/perception/detected_object_validation/include/obstacle_pointcloud_based_validator/debugger.hpp index 97c4387122301..9ef1f75427b65 100644 --- a/perception/detected_object_validation/include/obstacle_pointcloud_based_validator/debugger.hpp +++ b/perception/detected_object_validation/include/obstacle_pointcloud_based_validator/debugger.hpp @@ -16,7 +16,6 @@ #define OBSTACLE_POINTCLOUD_BASED_VALIDATOR__DEBUGGER_HPP_ #include -#include #include #include diff --git a/perception/detected_object_validation/include/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp b/perception/detected_object_validation/include/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp index 270e5c7bdb7ff..5819302664907 100644 --- a/perception/detected_object_validation/include/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp +++ b/perception/detected_object_validation/include/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp @@ -32,14 +32,14 @@ #include #include - +#include namespace obstacle_pointcloud_based_validator { struct PointsNumThresholdParam { - size_t min_points_num; - size_t max_points_num; - float min_points_and_distance_ratio; + std::vector min_points_num; + std::vector max_points_num; + std::vector min_points_and_distance_ratio; }; class ObstaclePointCloudBasedValidator : public rclcpp::Node { diff --git a/perception/detected_object_validation/launch/obstacle_pointcloud_based_validator.launch.xml b/perception/detected_object_validation/launch/obstacle_pointcloud_based_validator.launch.xml index 8196e95f37957..799b605658345 100644 --- a/perception/detected_object_validation/launch/obstacle_pointcloud_based_validator.launch.xml +++ b/perception/detected_object_validation/launch/obstacle_pointcloud_based_validator.launch.xml @@ -3,11 +3,13 @@ + + diff --git a/perception/detected_object_validation/src/object_lanelet_filter.cpp b/perception/detected_object_validation/src/object_lanelet_filter.cpp index d22a05fc7162d..f9b208cca02bc 100644 --- a/perception/detected_object_validation/src/object_lanelet_filter.cpp +++ b/perception/detected_object_validation/src/object_lanelet_filter.cpp @@ -14,8 +14,10 @@ #include "detected_object_filter/object_lanelet_filter.hpp" +#include +#include #include -#include +#include #include #include @@ -60,6 +62,7 @@ void ObjectLaneletFilterNode::mapCallback( lanelet::utils::conversion::fromBinMsg(*map_msg, lanelet_map_ptr_); const lanelet::ConstLanelets all_lanelets = lanelet::utils::query::laneletLayer(lanelet_map_ptr_); road_lanelets_ = lanelet::utils::query::roadLanelets(all_lanelets); + shoulder_lanelets_ = lanelet::utils::query::shoulderLanelets(all_lanelets); } void ObjectLaneletFilterNode::objectCallback( @@ -85,7 +88,10 @@ void ObjectLaneletFilterNode::objectCallback( // calculate convex hull const auto convex_hull = getConvexHull(transformed_objects); // get intersected lanelets - lanelet::ConstLanelets intersected_lanelets = getIntersectedLanelets(convex_hull, road_lanelets_); + lanelet::ConstLanelets intersected_road_lanelets = + getIntersectedLanelets(convex_hull, road_lanelets_); + lanelet::ConstLanelets intersected_shoulder_lanelets = + getIntersectedLanelets(convex_hull, shoulder_lanelets_); int index = 0; for (const auto & object : transformed_objects.objects) { @@ -99,7 +105,9 @@ void ObjectLaneletFilterNode::objectCallback( polygon.outer().emplace_back(point_transformed.x, point_transformed.y); } polygon.outer().push_back(polygon.outer().front()); - if (isPolygonOverlapLanelets(polygon, intersected_lanelets)) { + if (isPolygonOverlapLanelets(polygon, intersected_road_lanelets)) { + output_object_msg.objects.emplace_back(input_msg->objects.at(index)); + } else if (isPolygonOverlapLanelets(polygon, intersected_shoulder_lanelets)) { output_object_msg.objects.emplace_back(input_msg->objects.at(index)); } } else { diff --git a/perception/detected_object_validation/src/object_position_filter.cpp b/perception/detected_object_validation/src/object_position_filter.cpp index 6980f5fd3c17f..0f59e60d57d55 100644 --- a/perception/detected_object_validation/src/object_position_filter.cpp +++ b/perception/detected_object_validation/src/object_position_filter.cpp @@ -14,8 +14,6 @@ #include "detected_object_filter/object_position_filter.hpp" -#include - namespace object_position_filter { ObjectPositionFilterNode::ObjectPositionFilterNode(const rclcpp::NodeOptions & node_options) diff --git a/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp b/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp index bad20d8da577f..47640c9332e4d 100644 --- a/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp +++ b/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp @@ -15,7 +15,7 @@ #include "obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp" #include -#include +#include #include @@ -92,10 +92,12 @@ ObstaclePointCloudBasedValidator::ObstaclePointCloudBasedValidator( objects_pub_ = create_publisher( "~/output/objects", rclcpp::QoS{1}); - points_num_threshold_param_.min_points_num = declare_parameter("min_points_num", 10); - points_num_threshold_param_.max_points_num = declare_parameter("max_points_num", 10); + points_num_threshold_param_.min_points_num = + declare_parameter>("min_points_num"); + points_num_threshold_param_.max_points_num = + declare_parameter>("max_points_num"); points_num_threshold_param_.min_points_and_distance_ratio = - declare_parameter("min_points_and_distance_ratio", 800.0); + declare_parameter>("min_points_and_distance_ratio"); const bool enable_debugger = declare_parameter("enable_debugger", false); if (enable_debugger) debugger_ = std::make_shared(this); @@ -134,6 +136,7 @@ void ObstaclePointCloudBasedValidator::onObjectsAndObstaclePointCloud( for (size_t i = 0; i < transformed_objects.objects.size(); ++i) { const auto & transformed_object = transformed_objects.objects.at(i); + const auto object_label_id = transformed_object.classification.front().label; const auto & object = input_objects->objects.at(i); const auto & transformed_object_position = transformed_object.kinematics.pose_with_covariance.pose.position; @@ -155,13 +158,17 @@ void ObstaclePointCloudBasedValidator::onObjectsAndObstaclePointCloud( if (debugger_) debugger_->addNeighborPointcloud(neighbor_pointcloud); // Filter object that have few pointcloud in them. + // TODO(badai-nguyen) add 3d validator option const auto num = getPointCloudNumWithinPolygon(transformed_object, neighbor_pointcloud); const auto object_distance = std::hypot(transformed_object_position.x, transformed_object_position.y); size_t min_pointcloud_num = std::clamp( static_cast( - points_num_threshold_param_.min_points_and_distance_ratio / object_distance + 0.5f), - points_num_threshold_param_.min_points_num, points_num_threshold_param_.max_points_num); + points_num_threshold_param_.min_points_and_distance_ratio.at(object_label_id) / + object_distance + + 0.5f), + static_cast(points_num_threshold_param_.min_points_num.at(object_label_id)), + static_cast(points_num_threshold_param_.max_points_num.at(object_label_id))); if (num) { (min_pointcloud_num <= num.value()) ? output.objects.push_back(object) : removed_objects.objects.push_back(object); diff --git a/perception/detected_object_validation/src/occupancy_grid_based_validator.cpp b/perception/detected_object_validation/src/occupancy_grid_based_validator.cpp index 50e3f3b540929..948f040d7ebde 100644 --- a/perception/detected_object_validation/src/occupancy_grid_based_validator.cpp +++ b/perception/detected_object_validation/src/occupancy_grid_based_validator.cpp @@ -16,7 +16,7 @@ #include #include -#include +#include #include diff --git a/perception/detection_by_tracker/include/detection_by_tracker/debugger.hpp b/perception/detection_by_tracker/include/detection_by_tracker/debugger.hpp index 09ae73a7fa03c..fb7642356e8d9 100644 --- a/perception/detection_by_tracker/include/detection_by_tracker/debugger.hpp +++ b/perception/detection_by_tracker/include/detection_by_tracker/debugger.hpp @@ -20,7 +20,6 @@ #include #include #include -#include #include #include diff --git a/perception/detection_by_tracker/include/detection_by_tracker/detection_by_tracker_core.hpp b/perception/detection_by_tracker/include/detection_by_tracker/detection_by_tracker_core.hpp index 577e0ff12c367..0eacfa527750b 100644 --- a/perception/detection_by_tracker/include/detection_by_tracker/detection_by_tracker_core.hpp +++ b/perception/detection_by_tracker/include/detection_by_tracker/detection_by_tracker_core.hpp @@ -22,7 +22,6 @@ #include #include #include -#include #include #include diff --git a/perception/detection_by_tracker/src/detection_by_tracker_core.cpp b/perception/detection_by_tracker/src/detection_by_tracker_core.cpp index de71f94151486..7b436e1edd64c 100644 --- a/perception/detection_by_tracker/src/detection_by_tracker_core.cpp +++ b/perception/detection_by_tracker/src/detection_by_tracker_core.cpp @@ -16,6 +16,9 @@ #include "object_recognition_utils/object_recognition_utils.hpp" +#include +#include + #include #include #include diff --git a/perception/elevation_map_loader/include/elevation_map_loader/elevation_map_loader_node.hpp b/perception/elevation_map_loader/include/elevation_map_loader/elevation_map_loader_node.hpp index 3f383ce3d6d0e..7c0609ee1cdce 100644 --- a/perception/elevation_map_loader/include/elevation_map_loader/elevation_map_loader_node.hpp +++ b/perception/elevation_map_loader/include/elevation_map_loader/elevation_map_loader_node.hpp @@ -19,8 +19,6 @@ #include #include #include -#include -#include #include #include @@ -30,6 +28,7 @@ #include #include +#include #include #include diff --git a/perception/elevation_map_loader/src/elevation_map_loader_node.cpp b/perception/elevation_map_loader/src/elevation_map_loader_node.cpp index 096e084a70286..425af519a80ea 100644 --- a/perception/elevation_map_loader/src/elevation_map_loader_node.cpp +++ b/perception/elevation_map_loader/src/elevation_map_loader_node.cpp @@ -20,6 +20,8 @@ #include #include #include +#include +#include #include #include diff --git a/perception/euclidean_cluster/README.md b/perception/euclidean_cluster/README.md index 6c8995a44c56c..b57204c84cc3b 100644 --- a/perception/euclidean_cluster/README.md +++ b/perception/euclidean_cluster/README.md @@ -10,7 +10,7 @@ This package has two clustering methods: `euclidean_cluster` and `voxel_grid_bas ### euclidean_cluster -`pcl::EuclideanClusterExtraction` is applied to points. See [official document](https://pcl.readthedocs.io/en/latest/cluster_extraction.html) for details. +`pcl::EuclideanClusterExtraction` is applied to points. See [official document](https://pcl.readthedocs.io/projects/tutorials/en/master/cluster_extraction.html) for details. ### voxel_grid_based_euclidean_cluster diff --git a/perception/euclidean_cluster/config/compare_map.param.yaml b/perception/euclidean_cluster/config/compare_map.param.yaml deleted file mode 100644 index 3dd303464a2c1..0000000000000 --- a/perception/euclidean_cluster/config/compare_map.param.yaml +++ /dev/null @@ -1,20 +0,0 @@ -/**: - ros__parameters: - - # distance threshold for compare compare - distance_threshold: 0.5 - - # publish voxelized map pointcloud for debug - publish_debug_pcd: False - - # use dynamic map loading - use_dynamic_map_loading: True - - # time interval to check dynamic map loading - timer_interval_ms: 100 - - # distance threshold for dynamic map update - map_update_distance_threshold: 10.0 - - # radius map for dynamic map loading - map_loader_radius: 150.0 diff --git a/perception/euclidean_cluster/config/outlier.param.yaml b/perception/euclidean_cluster/config/outlier.param.yaml deleted file mode 100644 index 1962fba1f332a..0000000000000 --- a/perception/euclidean_cluster/config/outlier.param.yaml +++ /dev/null @@ -1,8 +0,0 @@ -/**: - ros__parameters: - input_frame: base_link - output_frame: base_link - voxel_size_x: 0.3 - voxel_size_y: 0.3 - voxel_size_z: 100.0 - voxel_points_threshold: 3 diff --git a/perception/euclidean_cluster/config/voxel_grid.param.yaml b/perception/euclidean_cluster/config/voxel_grid.param.yaml deleted file mode 100644 index 3ff32bfbb7146..0000000000000 --- a/perception/euclidean_cluster/config/voxel_grid.param.yaml +++ /dev/null @@ -1,7 +0,0 @@ -/**: - ros__parameters: - input_frame: base_link - output_frame: base_link - voxel_size_x: 0.15 - voxel_size_y: 0.15 - voxel_size_z: 0.15 diff --git a/perception/euclidean_cluster/config/voxel_grid_based_euclidean_cluster.param.yaml b/perception/euclidean_cluster/config/voxel_grid_based_euclidean_cluster.param.yaml index 2f3de2b789eb3..26b027f0077aa 100644 --- a/perception/euclidean_cluster/config/voxel_grid_based_euclidean_cluster.param.yaml +++ b/perception/euclidean_cluster/config/voxel_grid_based_euclidean_cluster.param.yaml @@ -7,9 +7,11 @@ max_cluster_size: 3000 use_height: false input_frame: "base_link" - max_x: 70.0 - min_x: -70.0 - max_y: 70.0 - min_y: -70.0 - max_z: 4.5 - min_z: -4.5 + + # low height crop box filter param + max_x: 200.0 + min_x: -200.0 + max_y: 200.0 + min_y: -200.0 + max_z: 2.0 + min_z: -10.0 diff --git a/perception/euclidean_cluster/launch/euclidean_cluster.launch.py b/perception/euclidean_cluster/launch/euclidean_cluster.launch.py index db86bbf80d250..66c25396a656e 100644 --- a/perception/euclidean_cluster/launch/euclidean_cluster.launch.py +++ b/perception/euclidean_cluster/launch/euclidean_cluster.launch.py @@ -35,61 +35,35 @@ def load_composable_node_param(param_path): ns = "" pkg = "euclidean_cluster" - # set voxel grid filter as a component - voxel_grid_filter_component = ComposableNode( + low_height_cropbox_filter_component = ComposableNode( package="pointcloud_preprocessor", - plugin="pointcloud_preprocessor::VoxelGridDownsampleFilterComponent", - name=AnonName("voxel_grid_filter"), + namespace=ns, + plugin="pointcloud_preprocessor::CropBoxFilterComponent", + name="low_height_crop_box_filter", remappings=[ ("input", LaunchConfiguration("input_pointcloud")), - ("output", "voxel_grid_filtered/pointcloud"), - ], - parameters=[load_composable_node_param("voxel_grid_param_path")], - ) - - # set compare map filter as a component - compare_map_filter_component = ComposableNode( - package="compare_map_segmentation", - plugin="compare_map_segmentation::VoxelBasedCompareMapFilterComponent", - name=AnonName("compare_map_filter"), - remappings=[ - ("input", "voxel_grid_filtered/pointcloud"), - ("map", LaunchConfiguration("input_map")), - ("output", "compare_map_filtered/pointcloud"), - ("map_loader_service", "/map/get_differential_pointcloud_map"), - ("kinematic_state", "/localization/kinematic_state"), - ], - parameters=[ - { - "distance_threshold": 0.5, - "timer_interval_ms": 100, - "use_dynamic_map_loading": True, - "downsize_ratio_z_axis": 0.5, - "map_update_distance_threshold": 10.0, - "map_loader_radius": 150.0, - "publish_debug_pcd": True, - "input_frame": "map", - } + ("output", "low_height/pointcloud"), ], + parameters=[load_composable_node_param("voxel_grid_based_euclidean_param_path")], ) - use_map_euclidean_cluster_component = ComposableNode( + use_low_height_euclidean_component = ComposableNode( package=pkg, plugin="euclidean_cluster::EuclideanClusterNode", name=AnonName("euclidean_cluster"), remappings=[ - ("input", "compare_map_filtered/pointcloud"), + ("input", "low_height/pointcloud"), ("output", LaunchConfiguration("output_clusters")), ], parameters=[load_composable_node_param("euclidean_param_path")], ) - disuse_map_euclidean_cluster_component = ComposableNode( + disuse_low_height_euclidean_component = ComposableNode( package=pkg, plugin="euclidean_cluster::EuclideanClusterNode", name=AnonName("euclidean_cluster"), remappings=[ - ("input", "voxel_grid_filtered/pointcloud"), + ("input", LaunchConfiguration("input_pointcloud")), ("output", LaunchConfiguration("output_clusters")), ], parameters=[load_composable_node_param("euclidean_param_path")], @@ -100,26 +74,26 @@ def load_composable_node_param(param_path): namespace=ns, package="rclcpp_components", executable="component_container", - composable_node_descriptions=[voxel_grid_filter_component], + composable_node_descriptions=[], output="screen", ) - use_map_loader = LoadComposableNodes( + use_low_height_pointcloud_loader = LoadComposableNodes( composable_node_descriptions=[ - compare_map_filter_component, - use_map_euclidean_cluster_component, + low_height_cropbox_filter_component, + use_low_height_euclidean_component, ], target_container=container, - condition=IfCondition(LaunchConfiguration("use_pointcloud_map")), + condition=IfCondition(LaunchConfiguration("use_low_height_cropbox")), ) - disuse_map_loader = LoadComposableNodes( - composable_node_descriptions=[disuse_map_euclidean_cluster_component], + disuse_low_height_pointcloud_loader = LoadComposableNodes( + composable_node_descriptions=[disuse_low_height_euclidean_component], target_container=container, - condition=UnlessCondition(LaunchConfiguration("use_pointcloud_map")), + condition=UnlessCondition(LaunchConfiguration("use_low_height_cropbox")), ) - return [container, use_map_loader, disuse_map_loader] + return [container, use_low_height_pointcloud_loader, disuse_low_height_pointcloud_loader] def generate_launch_description(): @@ -131,14 +105,7 @@ def add_launch_arg(name: str, default_value=None): add_launch_arg("input_pointcloud", "/perception/obstacle_segmentation/pointcloud"), add_launch_arg("input_map", "/map/pointcloud_map"), add_launch_arg("output_clusters", "clusters"), - add_launch_arg("use_pointcloud_map", "false"), - add_launch_arg( - "voxel_grid_param_path", - [ - FindPackageShare("autoware_launch"), - "/config/perception/object_recognition/detection/clustering/voxel_grid.param.yaml", - ], - ), + add_launch_arg("use_low_height_cropbox", "false"), add_launch_arg( "euclidean_param_path", [ diff --git a/perception/euclidean_cluster/launch/euclidean_cluster.launch.xml b/perception/euclidean_cluster/launch/euclidean_cluster.launch.xml index 7159fb4c42793..fd1ea82befae0 100644 --- a/perception/euclidean_cluster/launch/euclidean_cluster.launch.xml +++ b/perception/euclidean_cluster/launch/euclidean_cluster.launch.xml @@ -3,16 +3,14 @@ - - + - - + diff --git a/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.py b/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.py index 82ce5605b67cd..00bcd4422bd3c 100644 --- a/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.py +++ b/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.py @@ -16,10 +16,8 @@ from launch.actions import DeclareLaunchArgument from launch.actions import OpaqueFunction from launch.conditions import IfCondition -from launch.substitutions import AndSubstitution -from launch.substitutions import AnonName +from launch.conditions import UnlessCondition from launch.substitutions import LaunchConfiguration -from launch.substitutions import NotSubstitution from launch_ros.actions import ComposableNodeContainer from launch_ros.actions import LoadComposableNodes from launch_ros.descriptions import ComposableNode @@ -36,159 +34,31 @@ def load_composable_node_param(param_path): ns = "" pkg = "euclidean_cluster" - # set compare map filter as a component - compare_map_filter_component = ComposableNode( - package="compare_map_segmentation", - namespace=ns, - plugin="compare_map_segmentation::VoxelBasedCompareMapFilterComponent", - name=AnonName("compare_map_filter"), - remappings=[ - ("input", LaunchConfiguration("input_pointcloud")), - ("map", LaunchConfiguration("input_map")), - ("output", "map_filter/pointcloud"), - ("map_loader_service", "/map/get_differential_pointcloud_map"), - ("kinematic_state", "/localization/kinematic_state"), - ], - parameters=[load_composable_node_param("compare_map_param_path")], - ) - - # separate range of pointcloud when map_filter used - use_map_short_range_crop_box_filter_component = ComposableNode( - package="pointcloud_preprocessor", - namespace=ns, - plugin="pointcloud_preprocessor::CropBoxFilterComponent", - name="short_distance_crop_box_range", - remappings=[ - ("input", "map_filter/pointcloud"), - ("output", "short_range/pointcloud"), - ], - parameters=[ - load_composable_node_param("voxel_grid_based_euclidean_param_path"), - { - "negative": False, - }, - ], - ) - - use_map_long_range_crop_box_filter_component = ComposableNode( - package="pointcloud_preprocessor", - namespace=ns, - plugin="pointcloud_preprocessor::CropBoxFilterComponent", - name="long_distance_crop_box_range", - remappings=[ - ("input", LaunchConfiguration("input_pointcloud")), - ("output", "long_range/pointcloud"), - ], - parameters=[ - load_composable_node_param("voxel_grid_based_euclidean_param_path"), - { - "negative": True, - }, - ], - ) - - # disuse_map_voxel_grid_filter - disuse_map_short_range_crop_box_filter_component = ComposableNode( + low_height_cropbox_filter_component = ComposableNode( package="pointcloud_preprocessor", namespace=ns, plugin="pointcloud_preprocessor::CropBoxFilterComponent", - name="short_distance_crop_box_range", + name="low_height_crop_box_filter", remappings=[ ("input", LaunchConfiguration("input_pointcloud")), - ("output", "short_range/pointcloud"), - ], - parameters=[ - load_composable_node_param("voxel_grid_based_euclidean_param_path"), - { - "negative": False, - }, - ], - ) - - disuse_map_long_range_crop_box_filter_component = ComposableNode( - package="pointcloud_preprocessor", - namespace=ns, - plugin="pointcloud_preprocessor::CropBoxFilterComponent", - name="long_distance_crop_box_range", - remappings=[ - ("input", LaunchConfiguration("input_pointcloud")), - ("output", "long_range/pointcloud"), - ], - parameters=[ - load_composable_node_param("voxel_grid_based_euclidean_param_path"), - { - "negative": True, - }, - ], - ) - - # set voxel grid filter as a component - voxel_grid_filter_component = ComposableNode( - package="pointcloud_preprocessor", - namespace=ns, - plugin="pointcloud_preprocessor::ApproximateDownsampleFilterComponent", - name=AnonName("voxel_grid_filter"), - remappings=[ - ("input", "short_range/pointcloud"), - ("output", "downsampled/short_range/pointcloud"), - ], - parameters=[load_composable_node_param("voxel_grid_param_path")], - ) - - # set outlier filter as a component - outlier_filter_component = ComposableNode( - package="pointcloud_preprocessor", - namespace=ns, - plugin="pointcloud_preprocessor::VoxelGridOutlierFilterComponent", - name="outlier_filter", - remappings=[ - ("input", "downsampled/short_range/pointcloud"), - ("output", "outlier_filter/pointcloud"), - ], - parameters=[load_composable_node_param("outlier_param_path")], - ) - - # concat with-outlier pointcloud and without-outlier pcl - downsample_concat_component = ComposableNode( - package="pointcloud_preprocessor", - namespace=ns, - plugin="pointcloud_preprocessor::PointCloudConcatenateDataSynchronizerComponent", - name="concat_downsampled_pcl", - remappings=[("output", "downsampled/concatenated/pointcloud")], - parameters=[ - { - "input_topics": ["long_range/pointcloud", "outlier_filter/pointcloud"], - "output_frame": "base_link", - } - ], - extra_arguments=[{"use_intra_process_comms": True}], - ) - - # set euclidean cluster as a component - use_downsample_euclidean_cluster_component = ComposableNode( - package=pkg, - namespace=ns, - plugin="euclidean_cluster::VoxelGridBasedEuclideanClusterNode", - name="euclidean_cluster", - remappings=[ - ("input", "downsampled/concatenated/pointcloud"), - ("output", LaunchConfiguration("output_clusters")), + ("output", "low_height/pointcloud"), ], parameters=[load_composable_node_param("voxel_grid_based_euclidean_param_path")], ) - use_map_disuse_downsample_euclidean_component = ComposableNode( + use_low_height_euclidean_component = ComposableNode( package=pkg, namespace=ns, plugin="euclidean_cluster::VoxelGridBasedEuclideanClusterNode", name="euclidean_cluster", remappings=[ - ("input", "map_filter/pointcloud"), + ("input", "low_height/pointcloud"), ("output", LaunchConfiguration("output_clusters")), ], parameters=[load_composable_node_param("voxel_grid_based_euclidean_param_path")], ) - disuse_map_disuse_downsample_euclidean_component = ComposableNode( + + disuse_low_height_euclidean_component = ComposableNode( package=pkg, namespace=ns, plugin="euclidean_cluster::VoxelGridBasedEuclideanClusterNode", @@ -209,75 +79,24 @@ def load_composable_node_param(param_path): output="screen", ) - use_map_use_downsample_loader = LoadComposableNodes( + use_low_height_pointcloud_loader = LoadComposableNodes( composable_node_descriptions=[ - compare_map_filter_component, - use_map_short_range_crop_box_filter_component, - use_map_long_range_crop_box_filter_component, - voxel_grid_filter_component, - outlier_filter_component, - downsample_concat_component, - use_downsample_euclidean_cluster_component, + low_height_cropbox_filter_component, + use_low_height_euclidean_component, ], target_container=container, - condition=IfCondition( - AndSubstitution( - LaunchConfiguration("use_pointcloud_map"), - LaunchConfiguration("use_downsample_pointcloud"), - ) - ), + condition=IfCondition(LaunchConfiguration("use_low_height_cropbox")), ) - disuse_map_use_downsample_loader = LoadComposableNodes( - composable_node_descriptions=[ - disuse_map_short_range_crop_box_filter_component, - disuse_map_long_range_crop_box_filter_component, - voxel_grid_filter_component, - outlier_filter_component, - downsample_concat_component, - use_downsample_euclidean_cluster_component, - ], + disuse_low_height_pointcloud_loader = LoadComposableNodes( + composable_node_descriptions=[disuse_low_height_euclidean_component], target_container=container, - condition=IfCondition( - AndSubstitution( - NotSubstitution(LaunchConfiguration("use_pointcloud_map")), - LaunchConfiguration("use_downsample_pointcloud"), - ) - ), - ) - - use_map_disuse_downsample_loader = LoadComposableNodes( - composable_node_descriptions=[ - compare_map_filter_component, - use_map_disuse_downsample_euclidean_component, - ], - target_container=container, - condition=IfCondition( - AndSubstitution( - (LaunchConfiguration("use_pointcloud_map")), - NotSubstitution(LaunchConfiguration("use_downsample_pointcloud")), - ) - ), - ) - - disuse_map_disuse_downsample_loader = LoadComposableNodes( - composable_node_descriptions=[ - disuse_map_disuse_downsample_euclidean_component, - ], - target_container=container, - condition=IfCondition( - AndSubstitution( - NotSubstitution(LaunchConfiguration("use_pointcloud_map")), - NotSubstitution(LaunchConfiguration("use_downsample_pointcloud")), - ) - ), + condition=UnlessCondition(LaunchConfiguration("use_low_height_cropbox")), ) return [ container, - use_map_use_downsample_loader, - disuse_map_use_downsample_loader, - use_map_disuse_downsample_loader, - disuse_map_disuse_downsample_loader, + use_low_height_pointcloud_loader, + disuse_low_height_pointcloud_loader, ] @@ -290,29 +109,7 @@ def add_launch_arg(name: str, default_value=None): add_launch_arg("input_pointcloud", "/perception/obstacle_segmentation/pointcloud"), add_launch_arg("input_map", "/map/pointcloud_map"), add_launch_arg("output_clusters", "clusters"), - add_launch_arg("use_pointcloud_map", "false"), - add_launch_arg("use_downsample_pointcloud", "false"), - add_launch_arg( - "voxel_grid_param_path", - [ - FindPackageShare("autoware_launch"), - "/config/perception/object_recognition/detection/clustering/voxel_grid.param.yaml", - ], - ), - add_launch_arg( - "outlier_param_path", - [ - FindPackageShare("autoware_launch"), - "/config/perception/object_recognition/detection/clustering/outlier.param.yaml", - ], - ), - add_launch_arg( - "compare_map_param_path", - [ - FindPackageShare("autoware_launch"), - "/config/perception/object_recognition/detection/clustering/compare_map.param.yaml", - ], - ), + add_launch_arg("use_low_height_cropbox", "false"), add_launch_arg( "voxel_grid_based_euclidean_param_path", [ diff --git a/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.xml b/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.xml index 2833fed81c28e..b6a426dabfd12 100644 --- a/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.xml +++ b/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.xml @@ -3,22 +3,14 @@ - - - - - + - - - - - + diff --git a/perception/euclidean_cluster/src/euclidean_cluster_node.cpp b/perception/euclidean_cluster/src/euclidean_cluster_node.cpp index 744eb7af36a5b..a890c19e25d06 100644 --- a/perception/euclidean_cluster/src/euclidean_cluster_node.cpp +++ b/perception/euclidean_cluster/src/euclidean_cluster_node.cpp @@ -38,11 +38,18 @@ EuclideanClusterNode::EuclideanClusterNode(const rclcpp::NodeOptions & options) cluster_pub_ = this->create_publisher( "output", rclcpp::QoS{1}); debug_pub_ = this->create_publisher("debug/clusters", 1); + stop_watch_ptr_ = std::make_unique>(); + debug_publisher_ = + std::make_unique(this, "euclidean_cluster"); + stop_watch_ptr_->tic("cyclic_time"); + stop_watch_ptr_->tic("processing_time"); } void EuclideanClusterNode::onPointCloud( const sensor_msgs::msg::PointCloud2::ConstSharedPtr input_msg) { + stop_watch_ptr_->toc("processing_time", true); + // convert ros to pcl pcl::PointCloud::Ptr raw_pointcloud_ptr(new pcl::PointCloud); pcl::fromROSMsg(*input_msg, *raw_pointcloud_ptr); @@ -65,6 +72,14 @@ void EuclideanClusterNode::onPointCloud( convertObjectMsg2SensorMsg(output, debug); debug_pub_->publish(debug); } + if (debug_publisher_) { + const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); + const double processing_time_ms = stop_watch_ptr_->toc("processing_time", true); + debug_publisher_->publish( + "debug/cyclic_time_ms", cyclic_time_ms); + debug_publisher_->publish( + "debug/processing_time_ms", processing_time_ms); + } } } // namespace euclidean_cluster diff --git a/perception/euclidean_cluster/src/euclidean_cluster_node.hpp b/perception/euclidean_cluster/src/euclidean_cluster_node.hpp index e68a346a9a7d3..16e179b2512d3 100644 --- a/perception/euclidean_cluster/src/euclidean_cluster_node.hpp +++ b/perception/euclidean_cluster/src/euclidean_cluster_node.hpp @@ -17,13 +17,15 @@ #include "euclidean_cluster/euclidean_cluster.hpp" #include +#include +#include #include #include #include +#include #include - namespace euclidean_cluster { class EuclideanClusterNode : public rclcpp::Node @@ -39,6 +41,8 @@ class EuclideanClusterNode : public rclcpp::Node rclcpp::Publisher::SharedPtr debug_pub_; std::shared_ptr cluster_; + std::unique_ptr> stop_watch_ptr_; + std::unique_ptr debug_publisher_; }; } // namespace euclidean_cluster diff --git a/perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.cpp b/perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.cpp index 01856f3e6c836..a28791e5d4057 100644 --- a/perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.cpp +++ b/perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.cpp @@ -42,18 +42,33 @@ VoxelGridBasedEuclideanClusterNode::VoxelGridBasedEuclideanClusterNode( cluster_pub_ = this->create_publisher( "output", rclcpp::QoS{1}); debug_pub_ = this->create_publisher("debug/clusters", 1); + stop_watch_ptr_ = std::make_unique>(); + debug_publisher_ = std::make_unique( + this, "voxel_grid_based_euclidean_cluster"); + stop_watch_ptr_->tic("cyclic_time"); + stop_watch_ptr_->tic("processing_time"); } void VoxelGridBasedEuclideanClusterNode::onPointCloud( const sensor_msgs::msg::PointCloud2::ConstSharedPtr input_msg) { + stop_watch_ptr_->toc("processing_time", true); + // convert ros to pcl pcl::PointCloud::Ptr raw_pointcloud_ptr(new pcl::PointCloud); - pcl::fromROSMsg(*input_msg, *raw_pointcloud_ptr); + if (input_msg->data.empty()) { + // NOTE: prevent pcl log spam + RCLCPP_WARN_STREAM_THROTTLE( + this->get_logger(), *this->get_clock(), 1000, "Empty sensor points!"); + } else { + pcl::fromROSMsg(*input_msg, *raw_pointcloud_ptr); + } // clustering std::vector> clusters; - cluster_->cluster(raw_pointcloud_ptr, clusters); + if (!raw_pointcloud_ptr->empty()) { + cluster_->cluster(raw_pointcloud_ptr, clusters); + } // build output msg tier4_perception_msgs::msg::DetectedObjectsWithFeature output; @@ -69,6 +84,14 @@ void VoxelGridBasedEuclideanClusterNode::onPointCloud( convertObjectMsg2SensorMsg(output, debug); debug_pub_->publish(debug); } + if (debug_publisher_) { + const double processing_time_ms = stop_watch_ptr_->toc("processing_time", true); + const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); + debug_publisher_->publish( + "debug/cyclic_time_ms", cyclic_time_ms); + debug_publisher_->publish( + "debug/processing_time_ms", processing_time_ms); + } } } // namespace euclidean_cluster diff --git a/perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.hpp b/perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.hpp index d05e5ca1356fe..18330b5f12074 100644 --- a/perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.hpp +++ b/perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.hpp @@ -17,6 +17,8 @@ #include "euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp" #include +#include +#include #include #include @@ -39,6 +41,8 @@ class VoxelGridBasedEuclideanClusterNode : public rclcpp::Node rclcpp::Publisher::SharedPtr debug_pub_; std::shared_ptr cluster_; + std::unique_ptr> stop_watch_ptr_; + std::unique_ptr debug_publisher_; }; } // namespace euclidean_cluster diff --git a/perception/front_vehicle_velocity_estimator/include/front_vehicle_velocity_estimator/front_vehicle_velocity_estimator.hpp b/perception/front_vehicle_velocity_estimator/include/front_vehicle_velocity_estimator/front_vehicle_velocity_estimator.hpp index 5141bcad04990..01f738fc6beaa 100644 --- a/perception/front_vehicle_velocity_estimator/include/front_vehicle_velocity_estimator/front_vehicle_velocity_estimator.hpp +++ b/perception/front_vehicle_velocity_estimator/include/front_vehicle_velocity_estimator/front_vehicle_velocity_estimator.hpp @@ -19,7 +19,7 @@ #include "pcl/point_types.h" #include "pcl_conversions/pcl_conversions.h" #include "rclcpp/logger.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" +#include "tier4_autoware_utils/geometry/boost_geometry.hpp" #include "autoware_auto_perception_msgs/msg/detected_objects.hpp" #include "nav_msgs/msg/odometry.hpp" diff --git a/perception/front_vehicle_velocity_estimator/src/front_vehicle_velocity_estimator_node/front_vehicle_velocity_estimator.cpp b/perception/front_vehicle_velocity_estimator/src/front_vehicle_velocity_estimator_node/front_vehicle_velocity_estimator.cpp index f1b8be19c3223..6f59823615c04 100644 --- a/perception/front_vehicle_velocity_estimator/src/front_vehicle_velocity_estimator_node/front_vehicle_velocity_estimator.cpp +++ b/perception/front_vehicle_velocity_estimator/src/front_vehicle_velocity_estimator_node/front_vehicle_velocity_estimator.cpp @@ -14,6 +14,8 @@ #include "front_vehicle_velocity_estimator/front_vehicle_velocity_estimator.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" + #include #include diff --git a/perception/ground_segmentation/docs/image/scan_ground_filter_parameters.drawio.svg b/perception/ground_segmentation/docs/image/scan_ground_filter_parameters.drawio.svg new file mode 100644 index 0000000000000..985b99d82ba9b --- /dev/null +++ b/perception/ground_segmentation/docs/image/scan_ground_filter_parameters.drawio.svg @@ -0,0 +1,225 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+
+ detection_range_z_max +
+
+
+
+ + detection_range_z_max + + + + + + + + + + + +
+
+
+
+ global_slope +
+
+
+
+ + global_slope + + + + + + + + + + + + +
+
+
+
+ local_slope +
+
+
+
+ + local_slope + + + + + + + + + + + +
+
+
+
+
split_height_distance
+
+
+
+
+ + split_height_distance + + + + + + + + + +
+
+
+
+
+
grid_size_m
+
+
+
+
+
+ + grid_size_m + + + + + + + + + + + + +
+
+
+
+ z +
+
+
+
+ + z + + + + + + + Text is not SVG - cannot display + + + diff --git a/perception/ground_segmentation/docs/scan-ground-filter.md b/perception/ground_segmentation/docs/scan-ground-filter.md index 0c07ce600f625..bc44544fa298c 100644 --- a/perception/ground_segmentation/docs/scan-ground-filter.md +++ b/perception/ground_segmentation/docs/scan-ground-filter.md @@ -29,25 +29,26 @@ This implementation inherits `pointcloud_preprocessor::Filter` class, please ref #### Core Parameters -| Name | Type | Default Value | Description | -| --------------------------------- | ------ | ------------- | ------------------------------------------------------------------------------------------------------------------------------------------------- | -| `input_frame` | string | "base_link" | frame id of input pointcloud | -| `output_frame` | string | "base_link" | frame id of output pointcloud | -| `global_slope_max_angle_deg` | double | 8.0 | The global angle to classify as the ground or object [deg] | -| `local_slope_max_angle_deg` | double | 10.0 | The local angle to classify as the ground or object [deg] | -| `radial_divider_angle_deg` | double | 1.0 | The angle which divide the whole pointcloud to sliced group [deg] | -| `split_points_distance_tolerance` | double | 0.2 | The xy-distance threshold to to distinguishing far and near [m] | -| `split_height_distance` | double | 0.2 | The height threshold to distinguishing far and near [m] | -| `use_virtual_ground_point` | bool | true | whether to use the ground center of front wheels as the virtual ground point. | -| `detection_range_z_max` | float | 2.5 | Maximum height of detection range [m], applied only for elevation_grid_mode | -| `center_pcl_shift` | float | 0.0 | The x-axis offset of addition LiDARs from vehicle center of mass [m],
recommended to use only for additional LiDARs in elevation_grid_mode | -| `non_ground_height_threshold` | float | 0.2 | Height threshold of non ground objects [m], applied only for elevation_grid_mode | -| `grid_mode_switch_radius` | float | 20.0 | The distance where grid division mode change from by distance to by vertical angle [m],
applied only for elevation_grid_mode | -| `grid_size_m` | float | 0.5 | The first grid size [m], applied only for elevation_grid_mode | -| `gnd_grid_buffer_size` | uint16 | 4 | Number of grids using to estimate local ground slope,
applied only for elevation_grid_mode | -| `low_priority_region_x` | float | -20.0 | The non-zero x threshold in back side from which small objects detection is low priority [m] | -| `elevation_grid_mode` | bool | true | Elevation grid scan mode option | -| `use_recheck_ground_cluster` | bool | true | Enable recheck ground cluster | +![scan_ground_parameter](./image/scan_ground_filter_parameters.drawio.svg) +| Name | Type | Default Value | Description | +| --------------------------------- | ------ | ------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| `input_frame` | string | "base_link" | frame id of input pointcloud | +| `output_frame` | string | "base_link" | frame id of output pointcloud | +| `global_slope_max_angle_deg` | double | 8.0 | The global angle to classify as the ground or object [deg].
A large threshold may reduce false positive of high slope road classification but it may lead to increase false negative of non-ground classification, particularly for small objects. | +| `local_slope_max_angle_deg` | double | 10.0 | The local angle to classify as the ground or object [deg] when comparing with adjacent point.
A small value enhance accuracy classification of object with inclined surface. This should be considered together with `split_points_distance_tolerance` value. | +| `radial_divider_angle_deg` | double | 1.0 | The angle which divide the whole pointcloud to sliced group [deg] | +| `split_points_distance_tolerance` | double | 0.2 | The xy-distance threshold to distinguish far and near [m] | +| `split_height_distance` | double | 0.2 | The height threshold to distinguish ground and non-ground pointcloud when comparing with adjacent points [m].
A small threshold improves classification of non-ground point, especially for high elevation resolution pointcloud lidar. However, it might cause false positive for small step-like road surface or misaligned multiple lidar configuration. | +| `use_virtual_ground_point` | bool | true | whether to use the ground center of front wheels as the virtual ground point. | +| `detection_range_z_max` | float | 2.5 | Maximum height of detection range [m], applied only for elevation_grid_mode | +| `center_pcl_shift` | float | 0.0 | The x-axis offset of addition LiDARs from vehicle center of mass [m],
recommended to use only for additional LiDARs in elevation_grid_mode | +| `non_ground_height_threshold` | float | 0.2 | Height threshold of non ground objects [m] as `split_height_distance` and applied only for elevation_grid_mode | +| `grid_mode_switch_radius` | float | 20.0 | The distance where grid division mode change from by distance to by vertical angle [m],
applied only for elevation_grid_mode | +| `grid_size_m` | float | 0.5 | The first grid size [m], applied only for elevation_grid_mode.
A large value enhances the prediction stability for ground surface. suitable for rough surface or multiple lidar configuration. | +| `gnd_grid_buffer_size` | uint16 | 4 | Number of grids using to estimate local ground slope,
applied only for elevation_grid_mode | +| `low_priority_region_x` | float | -20.0 | The non-zero x threshold in back side from which small objects detection is low priority [m] | +| `elevation_grid_mode` | bool | true | Elevation grid scan mode option | +| `use_recheck_ground_cluster` | bool | true | Enable recheck ground cluster | ## Assumptions / Known limits diff --git a/perception/heatmap_visualizer/CMakeLists.txt b/perception/heatmap_visualizer/CMakeLists.txt index ed995491f2af6..75fa02a1cdcf7 100644 --- a/perception/heatmap_visualizer/CMakeLists.txt +++ b/perception/heatmap_visualizer/CMakeLists.txt @@ -16,4 +16,5 @@ rclcpp_components_register_node(heatmap_visualizer_component ament_auto_package(INSTALL_TO_SHARE launch + config ) diff --git a/perception/heatmap_visualizer/README.md b/perception/heatmap_visualizer/README.md index dd8b6f2a7f2b2..747a158b3d487 100644 --- a/perception/heatmap_visualizer/README.md +++ b/perception/heatmap_visualizer/README.md @@ -44,14 +44,15 @@ When publishing, firstly these values are normalized to [0, 1] using maximum and ### Core Parameters -| Name | Type | Default Value | Description | -| ----------------------------- | ------ | ------------- | ------------------------------------------------- | -| `frame_count` | int | `50` | The number of frames to publish heatmap | -| `map_frame` | string | `base_link` | the frame of heatmap to be respected | -| `map_length` | float | `200.0` | the length of map in meter | -| `map_resolution` | float | `0.8` | the resolution of map | -| `use_confidence` | bool | `false` | the flag if use confidence score as heatmap value | -| `rename_car_to_truck_and_bus` | bool | `true` | the flag if rename car to truck or bus | +| Name | Type | Default Value | Description | +| --------------------- | ------------- | ------------------------------------------------------------------------------------- | ----------------------------------------------- | +| `publish_frame_count` | int | `50` | The number of frames to publish heatmap | +| `heatmap_frame_id` | string | `base_link` | The frame ID of heatmap to be respected | +| `heatmap_length` | float | `200.0` | A length of map in meter | +| `heatmap_resolution` | float | `0.8` | A resolution of map | +| `use_confidence` | bool | `false` | A flag if use confidence score as heatmap value | +| `class_names` | array | `["UNKNOWN", "CAR", "TRUCK", "BUS", "TRAILER", "BICYCLE", "MOTORBIKE", "PEDESTRIAN"]` | An array of class names to be published | +| `rename_to_car` | bool | `true` | A flag if rename car like vehicle to car | ## Assumptions / Known limits diff --git a/perception/heatmap_visualizer/config/heatmap_visualizer.param.yaml b/perception/heatmap_visualizer/config/heatmap_visualizer.param.yaml new file mode 100644 index 0000000000000..2da863ea5e37a --- /dev/null +++ b/perception/heatmap_visualizer/config/heatmap_visualizer.param.yaml @@ -0,0 +1,33 @@ +# Copyright 2023 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. + +/**: + ros__parameters: + publish_frame_count: 50 + heatmap_frame_id: "base_link" + heatmap_length: 200.0 + heatmap_resolution: 0.8 + use_confidence: false + class_names: + [ + "UNKNOWN", + "CAR", + "TRUCK", + "BUS", + "TRAILER", + "BICYCLE", + "MOTORBIKE", + "PEDESTRIAN", + ] + rename_to_car: false diff --git a/perception/heatmap_visualizer/launch/heatmap_visualizer.launch.xml b/perception/heatmap_visualizer/launch/heatmap_visualizer.launch.xml index e87ae4e63a4d1..7251d776b6110 100644 --- a/perception/heatmap_visualizer/launch/heatmap_visualizer.launch.xml +++ b/perception/heatmap_visualizer/launch/heatmap_visualizer.launch.xml @@ -2,20 +2,10 @@ - - - - - - + - - - - - - + diff --git a/perception/heatmap_visualizer/schema/heatmap_visualizer.schema.json b/perception/heatmap_visualizer/schema/heatmap_visualizer.schema.json new file mode 100644 index 0000000000000..cb67ae383c35a --- /dev/null +++ b/perception/heatmap_visualizer/schema/heatmap_visualizer.schema.json @@ -0,0 +1,81 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Heatmap Visualizer Node", + "type": "object", + "definitions": { + "heatmap_visualizer": { + "type": "object", + "properties": { + "publish_frame_count": { + "type": "integer", + "description": "The number of frames to be published at once.", + "default": 50, + "minimum": 1 + }, + "heatmap_frame_id": { + "type": "string", + "description": "A frame ID in which visualized heatmap is with respect to.", + "default": "base_link" + }, + "heatmap_length": { + "type": "number", + "description": "A size length of heatmap [m].", + "default": 200.0, + "exclusiveMinimum": 0.0 + }, + "heatmap_resolution": { + "type": "number", + "description": "A cell resolution of heatmap [m].", + "default": 0.8, + "exclusiveMinimum": 0.0 + }, + "use_confidence": { + "type": "boolean", + "description": "Indicates whether to use object existence probability as score. It this parameter is false, 1.0 is set to score.", + "default": false + }, + "class_names": { + "type": "array", + "description": "An array of object class names.", + "default": [ + "UNKNOWN", + "CAR", + "TRUCK", + "BUS", + "TRAILER", + "BICYCLE", + "MOTORBIKE", + "PEDESTRIAN" + ], + "uniqueItems": true + }, + "rename_to_car": { + "type": "boolean", + "description": "Indicates whether to rename car like vehicle into car.", + "default": false + } + }, + "required": [ + "publish_frame_count", + "heatmap_frame_id", + "heatmap_length", + "heatmap_resolution", + "use_confidence", + "class_names", + "rename_to_car" + ] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/heatmap_visualizer" + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] +} diff --git a/perception/heatmap_visualizer/src/heatmap_visualizer_node.cpp b/perception/heatmap_visualizer/src/heatmap_visualizer_node.cpp index ad57d8638065f..d9337e0c9f61d 100644 --- a/perception/heatmap_visualizer/src/heatmap_visualizer_node.cpp +++ b/perception/heatmap_visualizer/src/heatmap_visualizer_node.cpp @@ -27,13 +27,13 @@ namespace heatmap_visualizer HeatmapVisualizerNode::HeatmapVisualizerNode(const rclcpp::NodeOptions & node_options) : Node("heatmap_visualizer", node_options), frame_count_(0) { - total_frame_ = declare_parameter("frame_count", 50); - map_frame_ = declare_parameter("map_frame", "base_link"); - map_length_ = declare_parameter("map_length", 200.0); - map_resolution_ = declare_parameter("map_resolution", 0.8); - use_confidence_ = declare_parameter("use_confidence", false); - class_names_ = declare_parameter("class_names", class_names_); - rename_car_to_truck_and_bus_ = declare_parameter("rename_car_to_truck_and_bus", false); + total_frame_ = static_cast(declare_parameter("publish_frame_count")); + map_frame_ = declare_parameter("heatmap_frame_id"); + map_length_ = static_cast(declare_parameter("heatmap_length")); + map_resolution_ = static_cast(declare_parameter("heatmap_resolution")); + use_confidence_ = declare_parameter("use_confidence"); + class_names_ = declare_parameter>("class_names"); + rename_car_to_truck_and_bus_ = declare_parameter("rename_to_car"); width_ = static_cast(map_length_ / map_resolution_); height_ = static_cast(map_length_ / map_resolution_); diff --git a/perception/heatmap_visualizer/src/utils.cpp b/perception/heatmap_visualizer/src/utils.cpp index 2c1be762b2cc9..c4b1c3d6a22c6 100644 --- a/perception/heatmap_visualizer/src/utils.cpp +++ b/perception/heatmap_visualizer/src/utils.cpp @@ -15,7 +15,8 @@ #include "heatmap_visualizer/utils.hpp" #include -#include +#include +#include #include diff --git a/perception/image_projection_based_fusion/CMakeLists.txt b/perception/image_projection_based_fusion/CMakeLists.txt index 906ad71d21732..53aafa44b75d6 100644 --- a/perception/image_projection_based_fusion/CMakeLists.txt +++ b/perception/image_projection_based_fusion/CMakeLists.txt @@ -22,6 +22,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED src/utils/utils.cpp src/roi_cluster_fusion/node.cpp src/roi_detected_object_fusion/node.cpp + src/roi_pointcloud_fusion/node.cpp ) target_link_libraries(${PROJECT_NAME} @@ -39,6 +40,11 @@ rclcpp_components_register_node(${PROJECT_NAME} EXECUTABLE roi_cluster_fusion_node ) +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "image_projection_based_fusion::RoiPointCloudFusionNode" + EXECUTABLE roi_pointcloud_fusion_node +) + set(CUDA_VERBOSE OFF) # set flags for CUDA availability @@ -101,52 +107,8 @@ else() set(CUDNN_AVAIL OFF) endif() -# Create folder to store trained models. -# NOTE: This must be created regardless of CUDA_AVAIL to be specified in ament_auto_package() -set(DATA_PATH ${CMAKE_CURRENT_SOURCE_DIR}/data) -execute_process(COMMAND mkdir -p ${DATA_PATH}) - if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL) - # Download trained models - - message(STATUS "start to download") - function(download FILE_NAME FILE_HASH) - message(STATUS "Checking and downloading ${FILE_NAME}") - set(FILE_PATH ${DATA_PATH}/${FILE_NAME}) - set(STATUS_CODE 0) - message(STATUS "start ${FILE_NAME}") - if(EXISTS ${FILE_PATH}) - message(STATUS "found ${FILE_NAME}") - file(MD5 ${FILE_PATH} EXISTING_FILE_HASH) - if("${FILE_HASH}" STREQUAL "${EXISTING_FILE_HASH}") - message(STATUS "same ${FILE_NAME}") - message(STATUS "File already exists.") - else() - message(STATUS "diff ${FILE_NAME}") - message(STATUS "File hash changes. Downloading now ...") - file(DOWNLOAD https://awf.ml.dev.web.auto/perception/models/pointpainting/v4/${FILE_NAME} - ${FILE_PATH} STATUS DOWNLOAD_STATUS TIMEOUT 300) - list(GET DOWNLOAD_STATUS 0 STATUS_CODE) - list(GET DOWNLOAD_STATUS 1 ERROR_MESSAGE) - endif() - else() - message(STATUS "not found ${FILE_NAME}") - message(STATUS "File doesn't exists. Downloading now ...") - file(DOWNLOAD https://awf.ml.dev.web.auto/perception/models/pointpainting/v4/${FILE_NAME} - ${FILE_PATH} STATUS DOWNLOAD_STATUS TIMEOUT 300) - list(GET DOWNLOAD_STATUS 0 STATUS_CODE) - list(GET DOWNLOAD_STATUS 1 ERROR_MESSAGE) - endif() - if(${STATUS_CODE} EQUAL 0) - message(STATUS "Download completed successfully!") - else() - message(FATAL_ERROR "Error occurred during download: ${ERROR_MESSAGE}") - endif() - endfunction() - - # default model - download(pts_voxel_encoder_pointpainting.onnx 25c70f76a45a64944ccd19f604c99410) - download(pts_backbone_neck_head_pointpainting.onnx 2c7108245240fbd7bf0104dd68225868) + find_package(OpenCV REQUIRED) find_package(Eigen3 REQUIRED) @@ -191,5 +153,4 @@ endif() ament_auto_package(INSTALL_TO_SHARE launch config - data ) diff --git a/perception/image_projection_based_fusion/README.md b/perception/image_projection_based_fusion/README.md index 1bbee35ab44f8..207989d8a6f25 100644 --- a/perception/image_projection_based_fusion/README.md +++ b/perception/image_projection_based_fusion/README.md @@ -58,3 +58,4 @@ The rclcpp::TimerBase timer could not break a for loop, therefore even if time i | roi_cluster_fusion | Overwrite a classification label of clusters by that of ROIs from a 2D object detector. | [link](./docs/roi-cluster-fusion.md) | | roi_detected_object_fusion | Overwrite a classification label of detected objects by that of ROIs from a 2D object detector. | [link](./docs/roi-detected-object-fusion.md) | | pointpainting_fusion | Paint the point cloud with the ROIs from a 2D object detector and feed to a 3D object detector. | [link](./docs/pointpainting-fusion.md) | +| roi_pointcloud_fusion | Matching pointcloud with ROIs from a 2D object detector to detect unknown-labeled objects | [link](./docs/roi-pointcloud-fusion.md) | diff --git a/perception/image_projection_based_fusion/docs/images/roi_pointcloud_fusion.png b/perception/image_projection_based_fusion/docs/images/roi_pointcloud_fusion.png new file mode 100644 index 0000000000000..c529de7c728fb Binary files /dev/null and b/perception/image_projection_based_fusion/docs/images/roi_pointcloud_fusion.png differ diff --git a/perception/image_projection_based_fusion/docs/roi-cluster-fusion.md b/perception/image_projection_based_fusion/docs/roi-cluster-fusion.md index 01cdc3756866b..03eaab2a3c6ca 100644 --- a/perception/image_projection_based_fusion/docs/roi-cluster-fusion.md +++ b/perception/image_projection_based_fusion/docs/roi-cluster-fusion.md @@ -32,18 +32,20 @@ The clusters are projected onto image planes, and then if the ROIs of clusters a ### Core Parameters -| Name | Type | Description | -| --------------------------- | ----- | -------------------------------------------------------------------------------- | -| `use_iou_x` | bool | calculate IoU only along x-axis | -| `use_iou_y` | bool | calculate IoU only along y-axis | -| `use_iou` | bool | calculate IoU both along x-axis and y-axis | -| `use_cluster_semantic_type` | bool | if `false`, the labels of clusters are overwritten by `UNKNOWN` before fusion | -| `only_allow_inside_cluster` | bool | if `true`, the only clusters contained inside RoIs by a detector | -| `roi_scale_factor` | float | the scale factor for offset of detector RoIs if `only_allow_inside_cluster=true` | -| `iou_threshold` | float | the IoU threshold to overwrite a label of clusters with a label of roi | -| `unknown_iou_threshold` | float | the IoU threshold to fuse cluster with unknown label of roi | -| `rois_number` | int | the number of input rois | -| `debug_mode` | bool | If `true`, subscribe and publish images for visualization. | +| Name | Type | Description | +| --------------------------- | ------ | ---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| `fusion_distance` | double | If the detected object's distance to frame_id is less than the threshold, the fusion will be processed | +| `trust_object_distance` | double | if the detected object's distance is less than the `trust_object_distance`, `trust_object_iou_mode` will be used, otherwise `non_trust_object_iou_mode` will be used | +| `trust_object_iou_mode` | string | select mode from 3 options {`iou`, `iou_x`, `iou_y`} to calculate IoU in range of [`0`, `trust_distance`].
 `iou`: IoU along x-axis and y-axis
 `iou_x`: IoU along x-axis
 `iou_y`: IoU along y-axis | +| `non_trust_object_iou_mode` | string | the IOU mode using in range of [`trust_distance`, `fusion_distance`] if `trust_distance` < `fusion_distance` | +| `use_cluster_semantic_type` | bool | if `false`, the labels of clusters are overwritten by `UNKNOWN` before fusion | +| `only_allow_inside_cluster` | bool | if `true`, the only clusters contained inside RoIs by a detector | +| `roi_scale_factor` | double | the scale factor for offset of detector RoIs if `only_allow_inside_cluster=true` | +| `iou_threshold` | double | the IoU threshold to overwrite a label of clusters with a label of roi | +| `unknown_iou_threshold` | double | the IoU threshold to fuse cluster with unknown label of roi | +| `remove_unknown` | bool | if `true`, remove all `UNKNOWN` labeled objects from output | +| `rois_number` | int | the number of input rois | +| `debug_mode` | bool | If `true`, subscribe and publish images for visualization. | ## Assumptions / Known limits diff --git a/perception/image_projection_based_fusion/docs/roi-pointcloud-fusion.md b/perception/image_projection_based_fusion/docs/roi-pointcloud-fusion.md new file mode 100644 index 0000000000000..5d0b241a578d6 --- /dev/null +++ b/perception/image_projection_based_fusion/docs/roi-pointcloud-fusion.md @@ -0,0 +1,93 @@ +# roi_pointcloud_fusion + +## Purpose + +The node `roi_pointcloud_fusion` is to cluster the pointcloud based on Region Of Interests (ROIs) detected by a 2D object detector, specific for unknown labeled ROI. + +## Inner-workings / Algorithms + +- The pointclouds are projected onto image planes and extracted as cluster if they are inside the unknown labeled ROIs. +- Since the cluster might contain unrelated points from background, then a refinement step is added to filter the background pointcloud by distance to camera. + +![roi_pointcloud_fusion_image](./images/roi_pointcloud_fusion.png) + +## Inputs / Outputs + +### Input + +| Name | Type | Description | +| ------------------------ | -------------------------------------------------------- | --------------------------------------------------------- | +| `input` | `sensor_msgs::msg::PointCloud2` | input pointcloud | +| `input/camera_info[0-7]` | `sensor_msgs::msg::CameraInfo` | camera information to project 3d points onto image planes | +| `input/rois[0-7]` | `tier4_perception_msgs::msg::DetectedObjectsWithFeature` | ROIs from each image | +| `input/image_raw[0-7]` | `sensor_msgs::msg::Image` | images for visualization | + +### Output + +| Name | Type | Description | +| ----------------- | -------------------------------------------------------- | -------------------------------------------- | +| `output` | `sensor_msgs::msg::PointCloud2` | output pointcloud as default of interface | +| `output_clusters` | `tier4_perception_msgs::msg::DetectedObjectsWithFeature` | output clusters | +| `debug/clusters` | `sensor_msgs/msg/PointCloud2` | colored cluster pointcloud for visualization | + +## Parameters + +### Core Parameters + +| Name | Type | Description | +| ---------------------- | ------ | -------------------------------------------------------------------------------------------- | +| `min_cluster_size` | int | the minimum number of points that a cluster needs to contain in order to be considered valid | +| `cluster_2d_tolerance` | double | cluster tolerance measured in radial direction | +| `rois_number` | int | the number of input rois | + +## Assumptions / Known limits + +- Currently, the refinement is only based on distance to camera, the roi based clustering is supposed to work well with small object ROIs. Others criteria for refinement might needed in the future. + + + +## (Optional) Error detection and handling + + + +## (Optional) Performance characterization + + + +## (Optional) References/External links + + + +## (Optional) Future extensions / Unimplemented parts + + diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/fusion_node.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/fusion_node.hpp index 016633fa4ef92..5174aebe069a8 100644 --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/fusion_node.hpp +++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/fusion_node.hpp @@ -29,6 +29,9 @@ #include #include #include +#include +#include +#include #include #include @@ -49,6 +52,8 @@ using autoware_auto_perception_msgs::msg::DetectedObjects; using sensor_msgs::msg::PointCloud2; using tier4_perception_msgs::msg::DetectedObjectsWithFeature; using tier4_perception_msgs::msg::DetectedObjectWithFeature; +using PointCloud = pcl::PointCloud; +using autoware_auto_perception_msgs::msg::ObjectClassification; template class FusionNode : public rclcpp::Node diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/voxel_generator.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/voxel_generator.hpp index 3e223dd3101df..5e7a5087efb84 100755 --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/voxel_generator.hpp +++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/voxel_generator.hpp @@ -17,6 +17,7 @@ #include +#include #include namespace image_projection_based_fusion diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_cluster_fusion/node.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_cluster_fusion/node.hpp index 38f9e71eea02d..e54710ad477da 100644 --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_cluster_fusion/node.hpp +++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_cluster_fusion/node.hpp @@ -17,10 +17,12 @@ #include "image_projection_based_fusion/fusion_node.hpp" +#include #include - +#include namespace image_projection_based_fusion { +const std::map IOU_MODE_MAP{{"iou", 0}, {"iou_x", 1}, {"iou_y", 2}}; class RoiClusterFusionNode : public FusionNode @@ -38,21 +40,23 @@ class RoiClusterFusionNode const sensor_msgs::msg::CameraInfo & camera_info, DetectedObjectsWithFeature & output_cluster_msg) override; - bool use_iou_x_{false}; - bool use_iou_y_{false}; - bool use_iou_{false}; + std::string trust_object_iou_mode_{"iou"}; bool use_cluster_semantic_type_{false}; bool only_allow_inside_cluster_{false}; - float roi_scale_factor_{1.1f}; - float iou_threshold_{0.0f}; - float unknown_iou_threshold_{0.0f}; + double roi_scale_factor_{1.1}; + double iou_threshold_{0.0}; + double unknown_iou_threshold_{0.0}; const float min_roi_existence_prob_ = 0.1; // keep small value to lessen affect on merger object stage bool remove_unknown_; - float trust_distance_; - - bool filter_by_distance(const DetectedObjectWithFeature & obj); + double fusion_distance_; + double trust_object_distance_; + std::string non_trust_object_iou_mode_{"iou_x"}; + bool is_far_enough(const DetectedObjectWithFeature & obj, const double distance_threshold); bool out_of_scope(const DetectedObjectWithFeature & obj); + double cal_iou_by_mode( + const sensor_msgs::msg::RegionOfInterest & roi_1, + const sensor_msgs::msg::RegionOfInterest & roi_2, const std::string iou_mode); // bool CheckUnknown(const DetectedObjectsWithFeature & obj); }; diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_pointcloud_fusion/node.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_pointcloud_fusion/node.hpp new file mode 100644 index 0000000000000..2b4eb822a9edb --- /dev/null +++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_pointcloud_fusion/node.hpp @@ -0,0 +1,53 @@ +// Copyright 2023 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 IMAGE_PROJECTION_BASED_FUSION__ROI_POINTCLOUD_FUSION__NODE_HPP_ +#define IMAGE_PROJECTION_BASED_FUSION__ROI_POINTCLOUD_FUSION__NODE_HPP_ + +#include "image_projection_based_fusion/fusion_node.hpp" + +#include +#include +namespace image_projection_based_fusion +{ +class RoiPointCloudFusionNode +: public FusionNode +{ +private: + int min_cluster_size_{1}; + bool fuse_unknown_only_{true}; + double cluster_2d_tolerance_; + + rclcpp::Publisher::SharedPtr pub_objects_ptr_; + std::vector output_fused_objects_; + rclcpp::Publisher::SharedPtr cluster_debug_pub_; + + /* data */ +public: + explicit RoiPointCloudFusionNode(const rclcpp::NodeOptions & options); + +protected: + void preprocess(sensor_msgs::msg::PointCloud2 & pointcloud_msg) override; + + void postprocess(sensor_msgs::msg::PointCloud2 & pointcloud_msg) override; + + void fuseOnSingleImage( + const PointCloud2 & input_pointcloud_msg, const std::size_t image_id, + const DetectedObjectsWithFeature & input_roi_msg, + const sensor_msgs::msg::CameraInfo & camera_info, PointCloud2 & output_pointcloud_msg) override; + bool out_of_scope(const DetectedObjectWithFeature & obj); +}; + +} // namespace image_projection_based_fusion +#endif // IMAGE_PROJECTION_BASED_FUSION__ROI_POINTCLOUD_FUSION__NODE_HPP_ diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/utils/utils.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/utils/utils.hpp index 541cf997412c2..d7fd3c3882046 100644 --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/utils/utils.hpp +++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/utils/utils.hpp @@ -32,18 +32,54 @@ #include #endif +#include "image_projection_based_fusion/fusion_node.hpp" + +#include +#include +#include +#include + +#include "autoware_auto_perception_msgs/msg/shape.hpp" +#include "tier4_perception_msgs/msg/detected_object_with_feature.hpp" + +#include +#include +#include + #include #include +#include namespace image_projection_based_fusion { +using PointCloud = pcl::PointCloud; std::optional getTransformStamped( const tf2_ros::Buffer & tf_buffer, const std::string & target_frame_id, const std::string & source_frame_id, const rclcpp::Time & time); Eigen::Affine3d transformToEigen(const geometry_msgs::msg::Transform & t); +void convertCluster2FeatureObject( + const std_msgs::msg::Header & header, const PointCloud & cluster, + DetectedObjectWithFeature & feature_obj); +PointCloud closest_cluster( + PointCloud & cluster, const double cluster_2d_tolerance, const int min_cluster_size, + const pcl::PointXYZ & center); + +void updateOutputFusedObjects( + std::vector & output_objs, const std::vector & clusters, + const std_msgs::msg::Header & in_cloud_header, const std_msgs::msg::Header & in_roi_header, + const tf2_ros::Buffer & tf_buffer, const int min_cluster_size, const float cluster_2d_tolerance, + std::vector & output_fused_objects); + +geometry_msgs::msg::Point getCentroid(const sensor_msgs::msg::PointCloud2 & pointcloud); + +pcl::PointXYZ getClosestPoint(const pcl::PointCloud & cluster); +void addShapeAndKinematic( + const pcl::PointCloud & cluster, + tier4_perception_msgs::msg::DetectedObjectWithFeature & feature_obj); + } // namespace image_projection_based_fusion #endif // IMAGE_PROJECTION_BASED_FUSION__UTILS__UTILS_HPP_ 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 e6275a2ee83c7..8e79e21db40e3 100644 --- a/perception/image_projection_based_fusion/launch/pointpainting_fusion.launch.xml +++ b/perception/image_projection_based_fusion/launch/pointpainting_fusion.launch.xml @@ -18,8 +18,9 @@ + - + diff --git a/perception/image_projection_based_fusion/launch/roi_cluster_fusion.launch.xml b/perception/image_projection_based_fusion/launch/roi_cluster_fusion.launch.xml index 37fbd309d3ee5..60f6f943b8cda 100644 --- a/perception/image_projection_based_fusion/launch/roi_cluster_fusion.launch.xml +++ b/perception/image_projection_based_fusion/launch/roi_cluster_fusion.launch.xml @@ -1,5 +1,5 @@ - + @@ -20,7 +20,8 @@ - + + @@ -45,16 +46,17 @@ - - - + + + - + - + + diff --git a/perception/image_projection_based_fusion/launch/roi_pointcloud_fusion.launch.xml b/perception/image_projection_based_fusion/launch/roi_pointcloud_fusion.launch.xml new file mode 100644 index 0000000000000..181f4ccb88320 --- /dev/null +++ b/perception/image_projection_based_fusion/launch/roi_pointcloud_fusion.launch.xml @@ -0,0 +1,77 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/perception/image_projection_based_fusion/package.xml b/perception/image_projection_based_fusion/package.xml index 32d7a5633b811..5ff99af2ebb21 100644 --- a/perception/image_projection_based_fusion/package.xml +++ b/perception/image_projection_based_fusion/package.xml @@ -14,6 +14,7 @@ autoware_auto_perception_msgs autoware_point_types cv_bridge + euclidean_cluster image_transport lidar_centerpoint message_filters diff --git a/perception/image_projection_based_fusion/src/fusion_node.cpp b/perception/image_projection_based_fusion/src/fusion_node.cpp index 1bc351b08c01b..a540688f7e751 100644 --- a/perception/image_projection_based_fusion/src/fusion_node.cpp +++ b/perception/image_projection_based_fusion/src/fusion_node.cpp @@ -422,4 +422,5 @@ void FusionNode::publish(const Msg & output_msg) template class FusionNode; template class FusionNode; template class FusionNode; +template class FusionNode; } // namespace image_projection_based_fusion 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 5253ac192c786..9f8ef7b94a123 100644 --- a/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp +++ b/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp @@ -133,7 +133,7 @@ PointPaintingFusionNode::PointPaintingFusionNode(const rclcpp::NodeOptions & opt auto it = find(paint_class_names.begin(), paint_class_names.end(), cls); if (it != paint_class_names.end()) { int index = it - paint_class_names.begin(); - class_index_[cls] = index + 1; + class_index_[cls] = pow(2, index); // regard each class as a bit in binary } else { isClassTable_.erase(cls); } @@ -345,7 +345,8 @@ dc | dc dc dc dc ||zc| data = &painted_pointcloud_msg.data[0]; auto p_class = reinterpret_cast(&output[stride + class_offset]); for (const auto & cls : isClassTable_) { - *p_class = cls.second(label2d) ? class_index_[cls.first] : *p_class; + // add up the class values if the point belongs to multiple classes + *p_class = cls.second(label2d) ? (class_index_[cls.first] + *p_class) : *p_class; } } #if 0 diff --git a/perception/image_projection_based_fusion/src/pointpainting_fusion/voxel_generator.cpp b/perception/image_projection_based_fusion/src/pointpainting_fusion/voxel_generator.cpp index bd49d9e446f1f..f462b9b0ba17a 100755 --- a/perception/image_projection_based_fusion/src/pointpainting_fusion/voxel_generator.cpp +++ b/perception/image_projection_based_fusion/src/pointpainting_fusion/voxel_generator.cpp @@ -59,8 +59,10 @@ std::size_t VoxelGenerator::pointsToVoxels( point[1] = point_current.y(); point[2] = point_current.z(); point[3] = time_lag; + // decode the class value back to one-hot binary and assign it to point for (std::size_t i = 1; i <= config_.class_size_; i++) { - point[3 + i] = (*class_iter == i) ? 1 : 0; + auto decode = std::bitset<8>(*class_iter).to_string(); + point[3 + i] = decode[-i] == 1 ? 1 : 0; } out_of_range = false; diff --git a/perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp b/perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp index 9180b18adeed8..4a46c8aace696 100644 --- a/perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp +++ b/perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp @@ -36,16 +36,16 @@ namespace image_projection_based_fusion RoiClusterFusionNode::RoiClusterFusionNode(const rclcpp::NodeOptions & options) : FusionNode("roi_cluster_fusion", options) { - use_iou_x_ = declare_parameter("use_iou_x", true); - use_iou_y_ = declare_parameter("use_iou_y", false); - use_iou_ = declare_parameter("use_iou", false); - use_cluster_semantic_type_ = declare_parameter("use_cluster_semantic_type", false); - only_allow_inside_cluster_ = declare_parameter("only_allow_inside_cluster_", true); - roi_scale_factor_ = declare_parameter("roi_scale_factor", 1.1); - iou_threshold_ = declare_parameter("iou_threshold", 0.1); - unknown_iou_threshold_ = declare_parameter("unknown_iou_threshold", 0.1); - remove_unknown_ = declare_parameter("remove_unknown", false); - trust_distance_ = declare_parameter("trust_distance", 100.0); + trust_object_iou_mode_ = declare_parameter("trust_object_iou_mode"); + non_trust_object_iou_mode_ = declare_parameter("non_trust_object_iou_mode"); + use_cluster_semantic_type_ = declare_parameter("use_cluster_semantic_type"); + only_allow_inside_cluster_ = declare_parameter("only_allow_inside_cluster"); + roi_scale_factor_ = declare_parameter("roi_scale_factor"); + iou_threshold_ = declare_parameter("iou_threshold"); + unknown_iou_threshold_ = declare_parameter("unknown_iou_threshold"); + remove_unknown_ = declare_parameter("remove_unknown"); + fusion_distance_ = declare_parameter("fusion_distance"); + trust_object_distance_ = declare_parameter("trust_object_distance"); } void RoiClusterFusionNode::preprocess(DetectedObjectsWithFeature & output_cluster_msg) @@ -111,7 +111,7 @@ void RoiClusterFusionNode::fuseOnSingleImage( continue; } - if (filter_by_distance(input_cluster_msg.feature_objects.at(i))) { + if (is_far_enough(input_cluster_msg.feature_objects.at(i), fusion_distance_)) { continue; } @@ -169,31 +169,38 @@ void RoiClusterFusionNode::fuseOnSingleImage( } for (const auto & feature_obj : input_roi_msg.feature_objects) { - int index = 0; + int index = -1; + bool associated = false; double max_iou = 0.0; + bool is_roi_label_known = + feature_obj.object.classification.front().label != ObjectClassification::UNKNOWN; for (const auto & cluster_map : m_cluster_roi) { - double iou(0.0), iou_x(0.0), iou_y(0.0); - if (use_iou_) { - iou = calcIoU(cluster_map.second, feature_obj.feature.roi); - } - if (use_iou_x_) { - iou_x = calcIoUX(cluster_map.second, feature_obj.feature.roi); - } - if (use_iou_y_) { - iou_y = calcIoUY(cluster_map.second, feature_obj.feature.roi); + double iou(0.0); + bool is_use_non_trust_object_iou_mode = is_far_enough( + input_cluster_msg.feature_objects.at(cluster_map.first), trust_object_distance_); + if (is_use_non_trust_object_iou_mode || is_roi_label_known) { + iou = + cal_iou_by_mode(cluster_map.second, feature_obj.feature.roi, non_trust_object_iou_mode_); + } else { + iou = cal_iou_by_mode(cluster_map.second, feature_obj.feature.roi, trust_object_iou_mode_); } + const bool passed_inside_cluster_gate = only_allow_inside_cluster_ ? is_inside(feature_obj.feature.roi, cluster_map.second, roi_scale_factor_) : true; - if (max_iou < iou + iou_x + iou_y && passed_inside_cluster_gate) { + if (max_iou < iou && passed_inside_cluster_gate) { index = cluster_map.first; - max_iou = iou + iou_x + iou_y; + max_iou = iou; + associated = true; } } + + if (!associated) { + continue; + } + if (!output_cluster_msg.feature_objects.empty()) { - bool is_roi_label_known = feature_obj.object.classification.front().label != - autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN; bool is_roi_existence_prob_higher = output_cluster_msg.feature_objects.at(index).object.existence_probability <= feature_obj.object.existence_probability; @@ -265,11 +272,30 @@ bool RoiClusterFusionNode::out_of_scope(const DetectedObjectWithFeature & obj) return is_out; } -bool RoiClusterFusionNode::filter_by_distance(const DetectedObjectWithFeature & obj) +bool RoiClusterFusionNode::is_far_enough( + const DetectedObjectWithFeature & obj, const double distance_threshold) { const auto & position = obj.object.kinematics.pose_with_covariance.pose.position; - const auto square_distance = position.x * position.x + position.y + position.y; - return square_distance > trust_distance_ * trust_distance_; + return position.x * position.x + position.y * position.y > + distance_threshold * distance_threshold; +} + +double RoiClusterFusionNode::cal_iou_by_mode( + const sensor_msgs::msg::RegionOfInterest & roi_1, + const sensor_msgs::msg::RegionOfInterest & roi_2, const std::string iou_mode) +{ + switch (IOU_MODE_MAP.at(iou_mode)) { + case 0 /* use iou mode */: + return calcIoU(roi_1, roi_2); + + case 1 /* use iou_x mode */: + return calcIoUX(roi_1, roi_2); + + case 2 /* use iou_y mode */: + return calcIoUY(roi_1, roi_2); + default: + return 0.0; + } } } // namespace image_projection_based_fusion diff --git a/perception/image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp b/perception/image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp new file mode 100644 index 0000000000000..233e568ebee0b --- /dev/null +++ b/perception/image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp @@ -0,0 +1,165 @@ +// Copyright 2023 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 "image_projection_based_fusion/roi_pointcloud_fusion/node.hpp" + +#include "image_projection_based_fusion/utils/geometry.hpp" +#include "image_projection_based_fusion/utils/utils.hpp" + +#ifdef ROS_DISTRO_GALACTIC +#include +#include +#else +#include +#include +#endif + +#include "euclidean_cluster/utils.hpp" + +namespace image_projection_based_fusion +{ +RoiPointCloudFusionNode::RoiPointCloudFusionNode(const rclcpp::NodeOptions & options) +: FusionNode("roi_pointcloud_fusion", options) +{ + fuse_unknown_only_ = declare_parameter("fuse_unknown_only"); + min_cluster_size_ = declare_parameter("min_cluster_size"); + cluster_2d_tolerance_ = declare_parameter("cluster_2d_tolerance"); + pub_objects_ptr_ = + this->create_publisher("output_clusters", rclcpp::QoS{1}); + cluster_debug_pub_ = this->create_publisher("debug/clusters", 1); +} + +void RoiPointCloudFusionNode::preprocess(__attribute__((unused)) + sensor_msgs::msg::PointCloud2 & pointcloud_msg) +{ + return; +} + +void RoiPointCloudFusionNode::postprocess(__attribute__((unused)) + sensor_msgs::msg::PointCloud2 & pointcloud_msg) +{ + const auto objects_sub_count = pub_objects_ptr_->get_subscription_count() + + pub_objects_ptr_->get_intra_process_subscription_count(); + if (objects_sub_count < 1) { + return; + } + + DetectedObjectsWithFeature output_msg; + output_msg.header = pointcloud_msg.header; + output_msg.feature_objects = output_fused_objects_; + + if (objects_sub_count > 0) { + pub_objects_ptr_->publish(output_msg); + } + output_fused_objects_.clear(); + // publish debug cluster + if (cluster_debug_pub_->get_subscription_count() > 0) { + sensor_msgs::msg::PointCloud2 debug_cluster_msg; + euclidean_cluster::convertObjectMsg2SensorMsg(output_msg, debug_cluster_msg); + cluster_debug_pub_->publish(debug_cluster_msg); + } +} +void RoiPointCloudFusionNode::fuseOnSingleImage( + const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, + __attribute__((unused)) const std::size_t image_id, + const DetectedObjectsWithFeature & input_roi_msg, + const sensor_msgs::msg::CameraInfo & camera_info, + __attribute__((unused)) sensor_msgs::msg::PointCloud2 & output_pointcloud_msg) +{ + if (input_pointcloud_msg.data.empty()) { + return; + } + std::vector output_objs; + // select ROIs for fusion + for (const auto & feature_obj : input_roi_msg.feature_objects) { + if (fuse_unknown_only_) { + bool is_roi_label_unknown = feature_obj.object.classification.front().label == + autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN; + if (is_roi_label_unknown) { + output_objs.push_back(feature_obj); + } + } else { + // TODO(badai-nguyen): selected class from a list + output_objs.push_back(feature_obj); + } + } + if (output_objs.empty()) { + return; + } + + // transform pointcloud to camera optical frame id + Eigen::Matrix4d projection; + projection << camera_info.p.at(0), camera_info.p.at(1), camera_info.p.at(2), camera_info.p.at(3), + camera_info.p.at(4), camera_info.p.at(5), camera_info.p.at(6), camera_info.p.at(7), + camera_info.p.at(8), camera_info.p.at(9), camera_info.p.at(10), camera_info.p.at(11); + geometry_msgs::msg::TransformStamped transform_stamped; + { + const auto transform_stamped_optional = getTransformStamped( + tf_buffer_, input_roi_msg.header.frame_id, input_pointcloud_msg.header.frame_id, + input_roi_msg.header.stamp); + if (!transform_stamped_optional) { + return; + } + transform_stamped = transform_stamped_optional.value(); + } + + sensor_msgs::msg::PointCloud2 transformed_cloud; + tf2::doTransform(input_pointcloud_msg, transformed_cloud, transform_stamped); + + std::vector clusters; + clusters.resize(output_objs.size()); + + for (sensor_msgs::PointCloud2ConstIterator iter_x(transformed_cloud, "x"), + iter_y(transformed_cloud, "y"), iter_z(transformed_cloud, "z"), + iter_orig_x(input_pointcloud_msg, "x"), iter_orig_y(input_pointcloud_msg, "y"), + iter_orig_z(input_pointcloud_msg, "z"); + iter_x != iter_x.end(); + ++iter_x, ++iter_y, ++iter_z, ++iter_orig_x, ++iter_orig_y, ++iter_orig_z) { + if (*iter_z <= 0.0) { + continue; + } + Eigen::Vector4d projected_point = projection * Eigen::Vector4d(*iter_x, *iter_y, *iter_z, 1.0); + Eigen::Vector2d normalized_projected_point = Eigen::Vector2d( + projected_point.x() / projected_point.z(), projected_point.y() / projected_point.z()); + + for (std::size_t i = 0; i < output_objs.size(); ++i) { + auto & feature_obj = output_objs.at(i); + const auto & check_roi = feature_obj.feature.roi; + auto & cluster = clusters.at(i); + + if ( + check_roi.x_offset <= normalized_projected_point.x() && + check_roi.y_offset <= normalized_projected_point.y() && + check_roi.x_offset + check_roi.width >= normalized_projected_point.x() && + check_roi.y_offset + check_roi.height >= normalized_projected_point.y()) { + cluster.push_back(pcl::PointXYZ(*iter_orig_x, *iter_orig_y, *iter_orig_z)); + } + } + } + + // refine and update output_fused_objects_ + updateOutputFusedObjects( + output_objs, clusters, input_pointcloud_msg.header, input_roi_msg.header, tf_buffer_, + min_cluster_size_, cluster_2d_tolerance_, output_fused_objects_); +} + +bool RoiPointCloudFusionNode::out_of_scope(__attribute__((unused)) + const DetectedObjectWithFeature & obj) +{ + return false; +} +} // namespace image_projection_based_fusion + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(image_projection_based_fusion::RoiPointCloudFusionNode) diff --git a/perception/image_projection_based_fusion/src/utils/utils.cpp b/perception/image_projection_based_fusion/src/utils/utils.cpp index 670c5596001fb..76cd1e3c61e82 100644 --- a/perception/image_projection_based_fusion/src/utils/utils.cpp +++ b/perception/image_projection_based_fusion/src/utils/utils.cpp @@ -13,7 +13,6 @@ // limitations under the License. #include "image_projection_based_fusion/utils/utils.hpp" - namespace image_projection_based_fusion { @@ -39,4 +38,197 @@ Eigen::Affine3d transformToEigen(const geometry_msgs::msg::Transform & t) return a; } +void convertCluster2FeatureObject( + const std_msgs::msg::Header & header, const PointCloud & cluster, + DetectedObjectWithFeature & feature_obj) +{ + PointCloud2 ros_cluster; + pcl::toROSMsg(cluster, ros_cluster); + ros_cluster.header = header; + feature_obj.feature.cluster = ros_cluster; + feature_obj.object.kinematics.pose_with_covariance.pose.position = getCentroid(ros_cluster); + feature_obj.object.existence_probability = 1.0f; +} + +PointCloud closest_cluster( + PointCloud & cluster, const double cluster_2d_tolerance, const int min_cluster_size, + const pcl::PointXYZ & center) +{ + // sort point by distance to camera origin + + auto func = [center](const pcl::PointXYZ & p1, const pcl::PointXYZ & p2) { + return tier4_autoware_utils::calcDistance2d(center, p1) < + tier4_autoware_utils::calcDistance2d(center, p2); + }; + std::sort(cluster.begin(), cluster.end(), func); + PointCloud out_cluster; + for (auto & point : cluster) { + if (out_cluster.empty()) { + out_cluster.push_back(point); + continue; + } + if (tier4_autoware_utils::calcDistance2d(out_cluster.back(), point) < cluster_2d_tolerance) { + out_cluster.push_back(point); + continue; + } + if (out_cluster.size() >= static_cast(min_cluster_size)) { + return out_cluster; + } + out_cluster.clear(); + out_cluster.push_back(point); + } + return out_cluster; +} + +void updateOutputFusedObjects( + std::vector & output_objs, const std::vector & clusters, + const std_msgs::msg::Header & in_cloud_header, const std_msgs::msg::Header & in_roi_header, + const tf2_ros::Buffer & tf_buffer, const int min_cluster_size, const float cluster_2d_tolerance, + std::vector & output_fused_objects) +{ + if (output_objs.size() != clusters.size()) { + return; + } + Eigen::Vector3d orig_camera_frame, orig_point_frame; + Eigen::Affine3d camera2lidar_affine; + orig_camera_frame << 0.0, 0.0, 0.0; + { + const auto transform_stamped_optional = getTransformStamped( + tf_buffer, in_cloud_header.frame_id, in_roi_header.frame_id, in_roi_header.stamp); + if (!transform_stamped_optional) { + return; + } + camera2lidar_affine = transformToEigen(transform_stamped_optional.value().transform); + } + orig_point_frame = camera2lidar_affine * orig_camera_frame; + pcl::PointXYZ camera_orig_point_frame = + pcl::PointXYZ(orig_point_frame.x(), orig_point_frame.y(), orig_point_frame.z()); + + for (std::size_t i = 0; i < output_objs.size(); ++i) { + PointCloud cluster = clusters.at(i); + auto & feature_obj = output_objs.at(i); + if (cluster.points.size() < std::size_t(min_cluster_size)) { + continue; + } + + // TODO(badai-nguyen): change to interface to select refine criteria like closest, largest + // to output refine cluster and centroid + auto refine_cluster = + closest_cluster(cluster, cluster_2d_tolerance, min_cluster_size, camera_orig_point_frame); + if (refine_cluster.points.size() < std::size_t(min_cluster_size)) { + continue; + } + + refine_cluster.width = refine_cluster.points.size(); + refine_cluster.height = 1; + refine_cluster.is_dense = false; + // convert cluster to object + convertCluster2FeatureObject(in_cloud_header, refine_cluster, feature_obj); + output_fused_objects.push_back(feature_obj); + } +} + +void addShapeAndKinematic( + const pcl::PointCloud & cluster, + tier4_perception_msgs::msg::DetectedObjectWithFeature & feature_obj) +{ + if (cluster.empty()) { + return; + } + pcl::PointXYZ centroid = pcl::PointXYZ(0.0, 0.0, 0.0); + float max_z = -1e6; + float min_z = 1e6; + for (const auto & point : cluster) { + centroid.x += point.x; + centroid.y += point.y; + centroid.z += point.z; + max_z = max_z < point.z ? point.z : max_z; + min_z = min_z > point.z ? point.z : min_z; + } + centroid.x = centroid.x / static_cast(cluster.size()); + centroid.y = centroid.y / static_cast(cluster.size()); + centroid.z = centroid.z / static_cast(cluster.size()); + + std::vector cluster2d; + std::vector cluster2d_convex; + + for (size_t i = 0; i < cluster.size(); ++i) { + cluster2d.push_back( + cv::Point((cluster.at(i).x - centroid.x) * 1000.0, (cluster.at(i).y - centroid.y) * 1000.)); + } + cv::convexHull(cluster2d, cluster2d_convex); + if (cluster2d_convex.empty()) { + return; + } + pcl::PointXYZ polygon_centroid = pcl::PointXYZ(0.0, 0.0, 0.0); + for (size_t i = 0; i < cluster2d_convex.size(); ++i) { + polygon_centroid.x += static_cast(cluster2d_convex.at(i).x) / 1000.0; + polygon_centroid.y += static_cast(cluster2d_convex.at(i).y) / 1000.0; + } + polygon_centroid.x = polygon_centroid.x / static_cast(cluster2d_convex.size()); + polygon_centroid.y = polygon_centroid.y / static_cast(cluster2d_convex.size()); + + autoware_auto_perception_msgs::msg::Shape shape; + for (size_t i = 0; i < cluster2d_convex.size(); ++i) { + geometry_msgs::msg::Point32 point; + point.x = cluster2d_convex.at(i).x / 1000.0; + point.y = cluster2d_convex.at(i).y / 1000.0; + point.z = 0.0; + shape.footprint.points.push_back(point); + } + shape.type = autoware_auto_perception_msgs::msg::Shape::POLYGON; + constexpr float eps = 0.01; + shape.dimensions.x = 0; + shape.dimensions.y = 0; + shape.dimensions.z = std::max((max_z - min_z), eps); + feature_obj.object.shape = shape; + feature_obj.object.kinematics.pose_with_covariance.pose.position.x = + centroid.x + polygon_centroid.x; + feature_obj.object.kinematics.pose_with_covariance.pose.position.y = + centroid.y + polygon_centroid.y; + feature_obj.object.kinematics.pose_with_covariance.pose.position.z = + min_z + shape.dimensions.z * 0.5; + feature_obj.object.existence_probability = 1.0; + feature_obj.object.kinematics.pose_with_covariance.pose.orientation.x = 0; + feature_obj.object.kinematics.pose_with_covariance.pose.orientation.y = 0; + feature_obj.object.kinematics.pose_with_covariance.pose.orientation.z = 0; + feature_obj.object.kinematics.pose_with_covariance.pose.orientation.w = 1; +} + +geometry_msgs::msg::Point getCentroid(const sensor_msgs::msg::PointCloud2 & pointcloud) +{ + geometry_msgs::msg::Point centroid; + centroid.x = 0.0f; + centroid.y = 0.0f; + centroid.z = 0.0f; + for (sensor_msgs::PointCloud2ConstIterator iter_x(pointcloud, "x"), + iter_y(pointcloud, "y"), iter_z(pointcloud, "z"); + iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z) { + centroid.x += *iter_x; + centroid.y += *iter_y; + centroid.z += *iter_z; + } + const size_t size = pointcloud.width * pointcloud.height; + centroid.x = centroid.x / static_cast(size); + centroid.y = centroid.y / static_cast(size); + centroid.z = centroid.z / static_cast(size); + return centroid; +} + +pcl::PointXYZ getClosestPoint(const pcl::PointCloud & cluster) +{ + pcl::PointXYZ closest_point; + double min_dist = 1e6; + pcl::PointXYZ orig_point = pcl::PointXYZ(0.0, 0.0, 0.0); + for (std::size_t i = 0; i < cluster.points.size(); ++i) { + pcl::PointXYZ point = cluster.points.at(i); + double dist_closest_point = tier4_autoware_utils::calcDistance2d(point, orig_point); + if (min_dist > dist_closest_point) { + min_dist = dist_closest_point; + closest_point = pcl::PointXYZ(point.x, point.y, point.z); + } + } + return closest_point; +} + } // namespace image_projection_based_fusion diff --git a/perception/lidar_apollo_instance_segmentation/CMakeLists.txt b/perception/lidar_apollo_instance_segmentation/CMakeLists.txt index 24f97e4201e00..4ae15948ad09e 100644 --- a/perception/lidar_apollo_instance_segmentation/CMakeLists.txt +++ b/perception/lidar_apollo_instance_segmentation/CMakeLists.txt @@ -24,55 +24,6 @@ if(NOT ${tensorrt_common_FOUND}) endif() find_package(cuda_utils REQUIRED) - -# download weight files -set(DATA_PATH "${CMAKE_CURRENT_SOURCE_DIR}/data") -if(NOT EXISTS "${DATA_PATH}") - execute_process(COMMAND mkdir -p ${DATA_PATH}) -endif() - -function(download FILE_NAME FILE_HASH) - message(STATUS "Checking and downloading ${FILE_NAME}") - set(FILE_PATH ${DATA_PATH}/${FILE_NAME}) - set(STATUS_CODE 0) - message(STATUS "start ${FILE_NAME}") - if(EXISTS ${FILE_PATH}) - message(STATUS "found ${FILE_NAME}") - file(MD5 ${FILE_PATH} EXISTING_FILE_HASH) - if(${FILE_HASH} STREQUAL ${EXISTING_FILE_HASH}) - message(STATUS "same ${FILE_NAME}") - message(STATUS "File already exists.") - else() - message(STATUS "diff ${FILE_NAME}") - message(STATUS "File hash changes. Downloading now ...") - file(DOWNLOAD - https://awf.ml.dev.web.auto/perception/models/lidar_apollo_instance_segmentation/${FILE_NAME} ${FILE_PATH} - STATUS DOWNLOAD_STATUS TIMEOUT 300 - ) - list(GET DOWNLOAD_STATUS 0 STATUS_CODE) - list(GET DOWNLOAD_STATUS 1 ERROR_MESSAGE) - endif() - else() - message(STATUS "not found ${FILE_NAME}") - message(STATUS "File doesn't exists. Downloading now ...") - file(DOWNLOAD - https://awf.ml.dev.web.auto/perception/models/lidar_apollo_instance_segmentation/${FILE_NAME} ${FILE_PATH} - STATUS DOWNLOAD_STATUS TIMEOUT 300 - ) - list(GET DOWNLOAD_STATUS 0 STATUS_CODE) - list(GET DOWNLOAD_STATUS 1 ERROR_MESSAGE) - endif() - if(${STATUS_CODE} EQUAL 0) - message(STATUS "Download completed successfully!") - else() - message(FATAL_ERROR "Error occurred during download: ${ERROR_MESSAGE}") - endif() -endfunction() - -download(vlp-16.onnx 63a5a1bb73f7dbb64cf70d04eca45fb4) -download(hdl-64.onnx 009745e33b1df44b68296431cc384cd2) -download(vls-128.onnx b2d709f56f73ae2518c9bf4d0214468f) - add_library(${PROJECT_NAME} SHARED src/node.cpp src/detector.cpp @@ -129,7 +80,6 @@ ament_package() install( DIRECTORY config - data launch DESTINATION share/${PROJECT_NAME} ) diff --git a/perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/cluster2d.hpp b/perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/cluster2d.hpp index 53cab7409578e..1bb6da0374fe9 100644 --- a/perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/cluster2d.hpp +++ b/perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/cluster2d.hpp @@ -79,12 +79,12 @@ struct Obstacle float height; float heading; MetaType meta_type; - std::vector meta_type_probs; + std::vector meta_type_probabilities; Obstacle() : score(0.0), height(-5.0), heading(0.0), meta_type(META_UNKNOWN) { cloud_ptr.reset(new pcl::PointCloud); - meta_type_probs.assign(MAX_META_TYPE, 0.0); + meta_type_probabilities.assign(MAX_META_TYPE, 0.0); } }; diff --git a/perception/lidar_apollo_instance_segmentation/launch/lidar_apollo_instance_segmentation.launch.xml b/perception/lidar_apollo_instance_segmentation/launch/lidar_apollo_instance_segmentation.launch.xml index bceb928dd6692..7f5024929b531 100644 --- a/perception/lidar_apollo_instance_segmentation/launch/lidar_apollo_instance_segmentation.launch.xml +++ b/perception/lidar_apollo_instance_segmentation/launch/lidar_apollo_instance_segmentation.launch.xml @@ -7,7 +7,8 @@ - + + diff --git a/perception/lidar_apollo_instance_segmentation/src/cluster2d.cpp b/perception/lidar_apollo_instance_segmentation/src/cluster2d.cpp index a7b1bee69aebb..8d758531758eb 100644 --- a/perception/lidar_apollo_instance_segmentation/src/cluster2d.cpp +++ b/perception/lidar_apollo_instance_segmentation/src/cluster2d.cpp @@ -235,13 +235,13 @@ void Cluster2D::classify(const float * inferred_data) for (size_t grid_id = 0; grid_id < obs->grids.size(); grid_id++) { int grid = obs->grids[grid_id]; for (int k = 0; k < num_classes; k++) { - obs->meta_type_probs[k] += classify_pt_data[k * size_ + grid]; + obs->meta_type_probabilities[k] += classify_pt_data[k * size_ + grid]; } } int meta_type_id = 0; for (int k = 0; k < num_classes; k++) { - obs->meta_type_probs[k] /= obs->grids.size(); - if (obs->meta_type_probs[k] > obs->meta_type_probs[meta_type_id]) { + obs->meta_type_probabilities[k] /= obs->grids.size(); + if (obs->meta_type_probabilities[k] > obs->meta_type_probabilities[meta_type_id]) { meta_type_id = k; } } diff --git a/perception/lidar_apollo_segmentation_tvm/include/lidar_apollo_segmentation_tvm/cluster2d.hpp b/perception/lidar_apollo_segmentation_tvm/include/lidar_apollo_segmentation_tvm/cluster2d.hpp index 35655cd16cb7c..4587184ecfb1f 100644 --- a/perception/lidar_apollo_segmentation_tvm/include/lidar_apollo_segmentation_tvm/cluster2d.hpp +++ b/perception/lidar_apollo_segmentation_tvm/include/lidar_apollo_segmentation_tvm/cluster2d.hpp @@ -57,12 +57,12 @@ struct Obstacle float height; float heading; MetaType meta_type; - std::vector meta_type_probs; + std::vector meta_type_probabilities; Obstacle() : score(0.0), height(-5.0), heading(0.0), meta_type(MetaType::META_UNKNOWN) { cloud_ptr.reset(new pcl::PointCloud); - meta_type_probs.assign(static_cast(MetaType::MAX_META_TYPE), 0.0); + meta_type_probabilities.assign(static_cast(MetaType::MAX_META_TYPE), 0.0); } }; diff --git a/perception/lidar_apollo_segmentation_tvm/package.xml b/perception/lidar_apollo_segmentation_tvm/package.xml index f4f283abe9450..21cd79c27b205 100755 --- a/perception/lidar_apollo_segmentation_tvm/package.xml +++ b/perception/lidar_apollo_segmentation_tvm/package.xml @@ -21,6 +21,7 @@ tf2_eigen tf2_geometry_msgs tf2_ros + tier4_autoware_utils tier4_perception_msgs tvm_utility tvm_vendor diff --git a/perception/lidar_apollo_segmentation_tvm/src/cluster2d.cpp b/perception/lidar_apollo_segmentation_tvm/src/cluster2d.cpp index 39c0da04a9c39..f96a32a5bb465 100644 --- a/perception/lidar_apollo_segmentation_tvm/src/cluster2d.cpp +++ b/perception/lidar_apollo_segmentation_tvm/src/cluster2d.cpp @@ -218,13 +218,13 @@ void Cluster2D::classify(const float * inferred_data) for (size_t grid_id = 0; grid_id < obs->grids.size(); grid_id++) { int32_t grid = obs->grids[grid_id]; for (int k = 0; k < num_classes; k++) { - obs->meta_type_probs[k] += classify_pt_data[k * siz_ + grid]; + obs->meta_type_probabilities[k] += classify_pt_data[k * siz_ + grid]; } } int meta_type_id = 0; for (int k = 0; k < num_classes; k++) { - obs->meta_type_probs[k] /= static_cast(obs->grids.size()); - if (obs->meta_type_probs[k] > obs->meta_type_probs[meta_type_id]) { + obs->meta_type_probabilities[k] /= static_cast(obs->grids.size()); + if (obs->meta_type_probabilities[k] > obs->meta_type_probabilities[meta_type_id]) { meta_type_id = k; } } diff --git a/perception/lidar_apollo_segmentation_tvm/src/lidar_apollo_segmentation_tvm.cpp b/perception/lidar_apollo_segmentation_tvm/src/lidar_apollo_segmentation_tvm.cpp index fb7ad38d7d642..e6c95f202106d 100644 --- a/perception/lidar_apollo_segmentation_tvm/src/lidar_apollo_segmentation_tvm.cpp +++ b/perception/lidar_apollo_segmentation_tvm/src/lidar_apollo_segmentation_tvm.cpp @@ -15,12 +15,14 @@ #include #include #include +#include #include #include #include #include +// cspell: ignore bcnn using model_zoo::perception::lidar_obstacle_detection::baidu_cnn::onnx_bcnn::config; namespace autoware diff --git a/perception/lidar_apollo_segmentation_tvm_nodes/README.md b/perception/lidar_apollo_segmentation_tvm_nodes/README.md index 626048b2416a0..488eea4f92168 100644 --- a/perception/lidar_apollo_segmentation_tvm_nodes/README.md +++ b/perception/lidar_apollo_segmentation_tvm_nodes/README.md @@ -49,6 +49,10 @@ The input are non-ground points as a PointCloud2 message from the sensor_msgs pa The output is a [DetectedObjectsWithFeature](https://github.com/tier4/tier4_autoware_msgs/blob/tier4/universe/tier4_perception_msgs/msg/object_recognition/DetectedObjectsWithFeature.msg). +#### Parameters + +{{ json_to_markdown("perception/lidar_apollo_segmentation_tvm_nodes/schema/lidar_apollo_segmentation_tvm_nodes.schema.json") }} + ### Error detection and handling Abort and warn when the input frame can't be converted to `base_link`. diff --git a/perception/lidar_centerpoint/CMakeLists.txt b/perception/lidar_centerpoint/CMakeLists.txt index f2935ae98a4fa..7ee5622414366 100644 --- a/perception/lidar_centerpoint/CMakeLists.txt +++ b/perception/lidar_centerpoint/CMakeLists.txt @@ -69,54 +69,7 @@ else() set(CUDNN_AVAIL OFF) endif() -message(STATUS "start to download") if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL) -# Download trained models - set(DATA_PATH ${CMAKE_CURRENT_SOURCE_DIR}/data) - set(DOWNLOAD_BASE_URL https://awf.ml.dev.web.auto/perception/models/centerpoint) - execute_process(COMMAND mkdir -p ${DATA_PATH}) - - function(download VERSION FILE_NAME FILE_HASH) - message(STATUS "Checking and downloading ${FILE_NAME} ") - set(DOWNLOAD_URL ${DOWNLOAD_BASE_URL}/${VERSION}/${FILE_NAME}) - set(FILE_PATH ${DATA_PATH}/${FILE_NAME}) - set(STATUS_CODE 0) - message(STATUS "start ${FILE_NAME}") - if(EXISTS ${FILE_PATH}) - message(STATUS "found ${FILE_NAME}") - file(MD5 ${FILE_PATH} EXISTING_FILE_HASH) - if("${FILE_HASH}" STREQUAL "${EXISTING_FILE_HASH}") - message(STATUS "same ${FILE_NAME}") - message(STATUS "File already exists.") - else() - message(STATUS "diff ${FILE_NAME}") - message(STATUS "File hash changes. Downloading now ...") - file(DOWNLOAD ${DOWNLOAD_URL} ${FILE_PATH} STATUS DOWNLOAD_STATUS TIMEOUT 300) - list(GET DOWNLOAD_STATUS 0 STATUS_CODE) - list(GET DOWNLOAD_STATUS 1 ERROR_MESSAGE) - endif() - else() - message(STATUS "not found ${FILE_NAME}") - message(STATUS "File doesn't exists. Downloading now ...") - file(DOWNLOAD ${DOWNLOAD_URL} ${FILE_PATH} STATUS DOWNLOAD_STATUS TIMEOUT 300) - list(GET DOWNLOAD_STATUS 0 STATUS_CODE) - list(GET DOWNLOAD_STATUS 1 ERROR_MESSAGE) - endif() - if(${STATUS_CODE} EQUAL 0) - message(STATUS "Download completed successfully!") - else() - message(FATAL_ERROR "Error occurred during download: ${ERROR_MESSAGE} ${DOWNLOAD_URL}") - endif() - endfunction() - - # centerpoint - download(v2 pts_voxel_encoder_centerpoint.onnx e8369d7e0f59951cf87c0274830de1fe) - download(v2 pts_backbone_neck_head_centerpoint.onnx 44e44d26781a69c741bf9bddde57fbb3) - - # centerpoint_tiny - download(v2 pts_voxel_encoder_centerpoint_tiny.onnx 5fb774fac44dc83949d05eb3ba077309) - download(v2 pts_backbone_neck_head_centerpoint_tiny.onnx e4658325b70222f7c3637fe00e586b82) - find_package(ament_cmake_auto REQUIRED) ament_auto_find_build_dependencies() @@ -206,7 +159,6 @@ if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL) ament_auto_package( INSTALL_TO_SHARE launch - data config ) diff --git a/perception/lidar_centerpoint/launch/centerpoint_vs_centerpoint-tiny/centerpoint_vs_centerpoint-tiny.launch.xml b/perception/lidar_centerpoint/launch/centerpoint_vs_centerpoint-tiny/centerpoint_vs_centerpoint-tiny.launch.xml index 13fd386238eda..b9c056cfb5686 100644 --- a/perception/lidar_centerpoint/launch/centerpoint_vs_centerpoint-tiny/centerpoint_vs_centerpoint-tiny.launch.xml +++ b/perception/lidar_centerpoint/launch/centerpoint_vs_centerpoint-tiny/centerpoint_vs_centerpoint-tiny.launch.xml @@ -5,6 +5,7 @@ + @@ -21,7 +22,7 @@ - + @@ -35,7 +36,7 @@ - + diff --git a/perception/lidar_centerpoint/launch/centerpoint_vs_centerpoint-tiny/rviz/centerpoint.rviz b/perception/lidar_centerpoint/launch/centerpoint_vs_centerpoint-tiny/rviz/centerpoint.rviz index 9a390d10895f8..879837e101d47 100644 --- a/perception/lidar_centerpoint/launch/centerpoint_vs_centerpoint-tiny/rviz/centerpoint.rviz +++ b/perception/lidar_centerpoint/launch/centerpoint_vs_centerpoint-tiny/rviz/centerpoint.rviz @@ -24,8 +24,6 @@ Panels: - /Current View1 Name: Views Splitter Ratio: 0.5 - - Class: tier4_localization_rviz_plugin/InitialPoseButtonPanel - Name: InitialPoseButtonPanel - Class: AutowareDateTimePanel Name: AutowareDateTimePanel Visualization Manager: @@ -1233,7 +1231,7 @@ Visualization Manager: Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/startplanner + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/start_planner Value: false - Class: rviz_default_plugins/MarkerArray Enabled: false @@ -1820,8 +1818,6 @@ Window Geometry: Height: 1028 Hide Left Dock: true Hide Right Dock: false - InitialPoseButtonPanel: - collapsed: false QMainWindow State: 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 RecognitionResultOnImage: collapsed: false diff --git a/perception/lidar_centerpoint/launch/centerpoint_vs_centerpoint-tiny/rviz/centerpoint_tiny.rviz b/perception/lidar_centerpoint/launch/centerpoint_vs_centerpoint-tiny/rviz/centerpoint_tiny.rviz index 8ec93bb5b50d6..539f89305e3c6 100644 --- a/perception/lidar_centerpoint/launch/centerpoint_vs_centerpoint-tiny/rviz/centerpoint_tiny.rviz +++ b/perception/lidar_centerpoint/launch/centerpoint_vs_centerpoint-tiny/rviz/centerpoint_tiny.rviz @@ -25,8 +25,6 @@ Panels: - /Current View1 Name: Views Splitter Ratio: 0.5 - - Class: tier4_localization_rviz_plugin/InitialPoseButtonPanel - Name: InitialPoseButtonPanel - Class: AutowareDateTimePanel Name: AutowareDateTimePanel Visualization Manager: @@ -1234,7 +1232,7 @@ Visualization Manager: Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/startplanner + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/start_planner Value: false - Class: rviz_default_plugins/MarkerArray Enabled: false @@ -1821,8 +1819,6 @@ Window Geometry: Height: 1028 Hide Left Dock: true Hide Right Dock: false - InitialPoseButtonPanel: - collapsed: false QMainWindow State: 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 RecognitionResultOnImage: collapsed: false diff --git a/perception/lidar_centerpoint/launch/lidar_centerpoint.launch.xml b/perception/lidar_centerpoint/launch/lidar_centerpoint.launch.xml index d552cb702b980..a7f181ab78ac6 100644 --- a/perception/lidar_centerpoint/launch/lidar_centerpoint.launch.xml +++ b/perception/lidar_centerpoint/launch/lidar_centerpoint.launch.xml @@ -2,8 +2,9 @@ + - + diff --git a/perception/lidar_centerpoint/launch/single_inference_lidar_centerpoint.launch.xml b/perception/lidar_centerpoint/launch/single_inference_lidar_centerpoint.launch.xml index 0f6923d5e6414..491abfbad7764 100644 --- a/perception/lidar_centerpoint/launch/single_inference_lidar_centerpoint.launch.xml +++ b/perception/lidar_centerpoint/launch/single_inference_lidar_centerpoint.launch.xml @@ -1,7 +1,8 @@ + - + diff --git a/perception/lidar_centerpoint/lib/centerpoint_trt.cpp b/perception/lidar_centerpoint/lib/centerpoint_trt.cpp index 9b01ea8f53675..53e19500ff428 100644 --- a/perception/lidar_centerpoint/lib/centerpoint_trt.cpp +++ b/perception/lidar_centerpoint/lib/centerpoint_trt.cpp @@ -189,7 +189,7 @@ void CenterPointTRT::postProcess(std::vector & det_boxes3d) head_out_heatmap_d_.get(), head_out_offset_d_.get(), head_out_z_d_.get(), head_out_dim_d_.get(), head_out_rot_d_.get(), head_out_vel_d_.get(), det_boxes3d, stream_)); if (det_boxes3d.size() == 0) { - RCLCPP_WARN_STREAM(rclcpp::get_logger("lidar_centerpoint"), "No detected boxes."); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lidar_centerpoint"), "No detected boxes."); } } diff --git a/perception/lidar_centerpoint/lib/postprocess/non_maximum_suppression.cpp b/perception/lidar_centerpoint/lib/postprocess/non_maximum_suppression.cpp index 90129b837eb1b..d4d7b1379776a 100644 --- a/perception/lidar_centerpoint/lib/postprocess/non_maximum_suppression.cpp +++ b/perception/lidar_centerpoint/lib/postprocess/non_maximum_suppression.cpp @@ -16,8 +16,7 @@ #include "object_recognition_utils/geometry.hpp" #include "object_recognition_utils/object_recognition_utils.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" - +#include "tier4_autoware_utils/geometry/geometry.hpp" namespace centerpoint { diff --git a/perception/map_based_prediction/README.md b/perception/map_based_prediction/README.md index 256a1d0ae899f..6d3d7f5e035a9 100644 --- a/perception/map_based_prediction/README.md +++ b/perception/map_based_prediction/README.md @@ -109,6 +109,21 @@ For the additional information, here we show how we calculate lateral velocity. Currently, we use the upper method with a low-pass filter to calculate lateral velocity. +### Path generation + +Path generation is generated on the frenet frame. The path is generated by the following steps: + +1. Get the frenet frame of the reference path. +2. Generate the frenet frame of the current position of the object and the finite position of the object. +3. Optimize the path in each longitudinal and lateral coordinate of the frenet frame. (Use the quintic polynomial to fit start and end conditions.) +4. Convert the path to the global coordinate. + +See paper [2] for more details. + +#### Tuning lateral path shape + +`lateral_control_time_horizon` parameter supports the tuning of the lateral path shape. This parameter is used to calculate the time to reach the reference path. The smaller the value, the more the path will be generated to reach the reference path quickly. (Mostly the center of the lane.) + ### Path prediction for crosswalk users This module treats **Pedestrians** and **Bicycles** as objects using the crosswalk, and outputs prediction path based on map and estimated object's velocity, assuming the object has intention to cross the crosswalk, if the objects satisfies at least one of the following conditions: @@ -143,30 +158,30 @@ If the target object is inside the road or crosswalk, this module outputs one or ### Output -| Name | Type | Description | -| ------------------------ | ------------------------------------------------------ | ------------------------------------- | -| `~/objects` | `autoware_auto_perception_msgs::msg::PredictedObjects` | tracking objects with predicted path. | -| `~/objects_path_markers` | `visualization_msgs::msg::MarkerArray` | marker for visualization. | +| Name | Type | Description | +| ------------------------ | ------------------------------------------------------ | ------------------------------------------------------------------------------------- | +| `~/input/objects` | `autoware_auto_perception_msgs::msg::TrackedObjects` | tracking objects. Default is set to `/perception/object_recognition/tracking/objects` | +| `~/output/objects` | `autoware_auto_perception_msgs::msg::PredictedObjects` | tracking objects with predicted path. | +| `~/objects_path_markers` | `visualization_msgs::msg::MarkerArray` | marker for visualization. | ## Parameters -| Parameter | Type | Description | -| ------------------------------------------- | ------ | ------------------------------------------------------------------------------------------------------------ | -| `enable_delay_compensation` | bool | flag to enable the time delay compensation for the position of the object | -| `prediction_time_horizon` | double | predict time duration for predicted path [s] | -| `prediction_sampling_delta_time` | double | sampling time for points in predicted path [s] | -| `min_velocity_for_map_based_prediction` | double | apply map-based prediction to the objects with higher velocity than this value | -| `min_crosswalk_user_velocity` | double | minimum velocity use in path prediction for crosswalk users | -| `dist_threshold_for_searching_lanelet` | double | The threshold of the angle used when searching for the lane to which the object belongs [rad] | -| `delta_yaw_threshold_for_searching_lanelet` | double | The threshold of the distance used when searching for the lane to which the object belongs [m] | -| `sigma_lateral_offset` | double | Standard deviation for lateral position of objects [m] | -| `sigma_yaw_angle` | double | Standard deviation yaw angle of objects [rad] | -| `object_buffer_time_length` | double | Time span of object history to store the information [s] | -| `history_time_length` | double | Time span of object information used for prediction [s] | -| `dist_ratio_threshold_to_left_bound` | double | Conditions for using lane change detection of objects. Distance to the left bound of lanelet. | -| `dist_ratio_threshold_to_right_bound` | double | Conditions for using lane change detection of objects. Distance to the right bound of lanelet. | -| `diff_dist_threshold_to_left_bound` | double | Conditions for using lane change detection of objects. Differential value of horizontal position of objects. | -| `diff_dist_threshold_to_right_bound` | double | Conditions for using lane change detection of objects. Differential value of horizontal position of objects. | +| Parameter | Unit | Type | Description | +| ---------------------------------------------------------------- | ----- | ------ | ------------------------------------------------------------------------------------------------------------------------------------- | +| `enable_delay_compensation` | [-] | bool | flag to enable the time delay compensation for the position of the object | +| `prediction_time_horizon` | [s] | double | predict time duration for predicted path | +| `lateral_control_time_horizon` | [s] | double | time duration for predicted path will reach the reference path (mostly center of the lane) | +| `prediction_sampling_delta_time` | [s] | double | sampling time for points in predicted path | +| `min_velocity_for_map_based_prediction` | [m/s] | double | apply map-based prediction to the objects with higher velocity than this value | +| `min_crosswalk_user_velocity` | [m/s] | double | minimum velocity used when crosswalk user's velocity is calculated | +| `max_crosswalk_user_delta_yaw_threshold_for_lanelet` | [rad] | double | maximum yaw difference between crosswalk user and lanelet to use in path prediction for crosswalk users | +| `dist_threshold_for_searching_lanelet` | [m] | double | The threshold of the angle used when searching for the lane to which the object belongs | +| `delta_yaw_threshold_for_searching_lanelet` | [rad] | double | The threshold of the angle used when searching for the lane to which the object belongs | +| `sigma_lateral_offset` | [m] | double | Standard deviation for lateral position of objects | +| `sigma_yaw_angle_deg` | [deg] | double | Standard deviation yaw angle of objects | +| `object_buffer_time_length` | [s] | double | Time span of object history to store the information | +| `history_time_length` | [s] | double | Time span of object information used for prediction | +| `prediction_time_horizon_rate_for_validate_shoulder_lane_length` | [-] | double | prediction path will disabled when the estimated path length exceeds lanelet length. This parameter control the estimated path length | ## Assumptions / Known limits diff --git a/perception/map_based_prediction/config/map_based_prediction.param.yaml b/perception/map_based_prediction/config/map_based_prediction.param.yaml index cb9a61b600e43..260ae45da2e05 100644 --- a/perception/map_based_prediction/config/map_based_prediction.param.yaml +++ b/perception/map_based_prediction/config/map_based_prediction.param.yaml @@ -2,16 +2,21 @@ ros__parameters: enable_delay_compensation: true prediction_time_horizon: 10.0 #[s] + lateral_control_time_horizon: 5.0 #[s] prediction_sampling_delta_time: 0.5 #[s] min_velocity_for_map_based_prediction: 1.39 #[m/s] min_crosswalk_user_velocity: 1.39 #[m/s] + max_crosswalk_user_delta_yaw_threshold_for_lanelet: 0.785 #[m/s] dist_threshold_for_searching_lanelet: 3.0 #[m] delta_yaw_threshold_for_searching_lanelet: 0.785 #[rad] sigma_lateral_offset: 0.5 #[m] sigma_yaw_angle_deg: 5.0 #[angle degree] object_buffer_time_length: 2.0 #[s] history_time_length: 1.0 #[s] + # parameter for shoulder lane prediction + prediction_time_horizon_rate_for_validate_shoulder_lane_length: 0.8 + # parameters for lc prediction lane_change_detection: method: "lat_diff_distance" # choose from "lat_diff_distance" or "time_to_change_lane" time_to_change_lane: diff --git a/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp b/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp index f265633bd55dc..01f39057aef36 100644 --- a/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp +++ b/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp @@ -17,14 +17,9 @@ #include "map_based_prediction/path_generator.hpp" -#include -#include -#include -#include #include #include #include -#include #include #include @@ -35,12 +30,9 @@ #include #include -#include -#include -#include -#include -#include -#include +#include +#include +#include #include #include @@ -143,10 +135,12 @@ class MapBasedPredictionNode : public rclcpp::Node // Parameters bool enable_delay_compensation_; double prediction_time_horizon_; + double lateral_control_time_horizon_; double prediction_time_horizon_rate_for_validate_lane_length_; double prediction_sampling_time_interval_; double min_velocity_for_map_based_prediction_; double min_crosswalk_user_velocity_; + double max_crosswalk_user_delta_yaw_threshold_for_lanelet_; double debug_accumulated_time_; double dist_threshold_for_searching_lanelet_; double delta_yaw_threshold_for_searching_lanelet_; @@ -172,6 +166,14 @@ class MapBasedPredictionNode : public rclcpp::Node void mapCallback(const HADMapBin::ConstSharedPtr msg); void objectsCallback(const TrackedObjects::ConstSharedPtr in_objects); + bool doesPathCrossAnyFence(const PredictedPath & predicted_path); + bool doesPathCrossFence( + const PredictedPath & predicted_path, const lanelet::ConstLineString3d & fence_line); + lanelet::BasicLineString2d convertToFenceLine(const lanelet::ConstLineString3d & fence); + bool isIntersecting( + const geometry_msgs::msg::Point & point1, const geometry_msgs::msg::Point & point2, + const lanelet::ConstPoint3d & point3, const lanelet::ConstPoint3d & point4); + PredictedObjectKinematics convertToPredictedKinematics( const TrackedObjectKinematics & tracked_object); @@ -183,8 +185,7 @@ class MapBasedPredictionNode : public rclcpp::Node LaneletsData getCurrentLanelets(const TrackedObject & object); bool checkCloseLaneletCondition( - const std::pair & lanelet, const TrackedObject & object, - const bool check_distance = true); + const std::pair & lanelet, const TrackedObject & object); float calculateLocalLikelihood( const lanelet::Lanelet & current_lanelet, const TrackedObject & object) const; void updateObjectData(TrackedObject & object); diff --git a/perception/map_based_prediction/include/map_based_prediction/path_generator.hpp b/perception/map_based_prediction/include/map_based_prediction/path_generator.hpp index 280224dee59a2..4da4f62be2ede 100644 --- a/perception/map_based_prediction/include/map_based_prediction/path_generator.hpp +++ b/perception/map_based_prediction/include/map_based_prediction/path_generator.hpp @@ -49,7 +49,31 @@ struct FrenetPoint float d_acc; }; -using EntryPoint = std::pair; +struct CrosswalkEdgePoints +{ + Eigen::Vector2d front_center_point; + Eigen::Vector2d front_right_point; + Eigen::Vector2d front_left_point; + Eigen::Vector2d back_center_point; + Eigen::Vector2d back_right_point; + Eigen::Vector2d back_left_point; + + void swap() + { + const Eigen::Vector2d tmp_center_point = front_center_point; + const Eigen::Vector2d tmp_right_point = front_right_point; + const Eigen::Vector2d tmp_left_point = front_left_point; + + front_center_point = back_center_point; + front_right_point = back_right_point; + front_left_point = back_left_point; + + back_center_point = tmp_center_point; + back_right_point = tmp_right_point; + back_left_point = tmp_left_point; + } +}; + using FrenetPath = std::vector; using PosePath = std::vector; @@ -57,8 +81,8 @@ class PathGenerator { public: PathGenerator( - const double time_horizon, const double sampling_time_interval, - const double min_crosswalk_user_velocity); + const double time_horizon, const double lateral_time_horizon, + const double sampling_time_interval, const double min_crosswalk_user_velocity); PredictedPath generatePathForNonVehicleObject(const TrackedObject & object); @@ -70,7 +94,7 @@ class PathGenerator const TrackedObject & object, const PosePath & ref_paths); PredictedPath generatePathForCrosswalkUser( - const TrackedObject & object, const EntryPoint & reachable_crosswalk) const; + const TrackedObject & object, const CrosswalkEdgePoints & reachable_crosswalk) const; PredictedPath generatePathToTargetPoint( const TrackedObject & object, const Eigen::Vector2d & point) const; @@ -78,6 +102,7 @@ class PathGenerator private: // Parameters double time_horizon_; + double lateral_time_horizon_; double sampling_time_interval_; double min_crosswalk_user_velocity_; diff --git a/perception/map_based_prediction/launch/map_based_prediction.launch.xml b/perception/map_based_prediction/launch/map_based_prediction.launch.xml index f7ae53cd1d588..619c0c9507e0b 100644 --- a/perception/map_based_prediction/launch/map_based_prediction.launch.xml +++ b/perception/map_based_prediction/launch/map_based_prediction.launch.xml @@ -4,10 +4,12 @@ + - + + diff --git a/perception/map_based_prediction/package.xml b/perception/map_based_prediction/package.xml index 0765490502ec4..d7ff85adc9193 100644 --- a/perception/map_based_prediction/package.xml +++ b/perception/map_based_prediction/package.xml @@ -8,6 +8,9 @@ Tomoya Kimura Shunsuke Miura Yoshi Ri + Takayuki Murooka + Kyoichi Sugahara + Kotaro Uetake Apache License 2.0 ament_cmake diff --git a/perception/map_based_prediction/src/map_based_prediction_node.cpp b/perception/map_based_prediction/src/map_based_prediction_node.cpp index be6327cf5c01a..08fc06330b1d8 100644 --- a/perception/map_based_prediction/src/map_based_prediction_node.cpp +++ b/perception/map_based_prediction/src/map_based_prediction_node.cpp @@ -15,14 +15,26 @@ #include "map_based_prediction/map_based_prediction_node.hpp" #include +#include +#include +#include #include -#include +#include +#include +#include +#include +#include +#include #include #include #include +#include +#include +#include +#include #include #ifdef ROS_DISTRO_GALACTIC @@ -313,8 +325,11 @@ bool validateIsolatedLaneletLength( const auto & end_point = center_line.back(); // calc approx distance between closest point and end point const double approx_distance = lanelet::geometry::distance2d(obj_point, end_point); - const double min_length = - object.kinematics.twist_with_covariance.twist.linear.x * prediction_time; + // calc min length for prediction + const double abs_speed = std::hypot( + object.kinematics.twist_with_covariance.twist.linear.x, + object.kinematics.twist_with_covariance.twist.linear.y); + const double min_length = abs_speed * prediction_time; return approx_distance > min_length; } @@ -328,19 +343,18 @@ lanelet::ConstLanelets getLanelets(const map_based_prediction::LaneletsData & da return lanelets; } -EntryPoint getCrosswalkEntryPoint(const lanelet::ConstLanelet & crosswalk) +CrosswalkEdgePoints getCrosswalkEdgePoints(const lanelet::ConstLanelet & crosswalk) { - const auto & r_p_front = crosswalk.rightBound().front(); - const auto & l_p_front = crosswalk.leftBound().front(); - const Eigen::Vector2d front_entry_point( - (r_p_front.x() + l_p_front.x()) / 2.0, (r_p_front.y() + l_p_front.y()) / 2.0); + const Eigen::Vector2d r_p_front = crosswalk.rightBound().front().basicPoint2d(); + const Eigen::Vector2d l_p_front = crosswalk.leftBound().front().basicPoint2d(); + const Eigen::Vector2d front_center_point = (r_p_front + l_p_front) / 2.0; - const auto & r_p_back = crosswalk.rightBound().back(); - const auto & l_p_back = crosswalk.leftBound().back(); - const Eigen::Vector2d back_entry_point( - (r_p_back.x() + l_p_back.x()) / 2.0, (r_p_back.y() + l_p_back.y()) / 2.0); + const Eigen::Vector2d r_p_back = crosswalk.rightBound().back().basicPoint2d(); + const Eigen::Vector2d l_p_back = crosswalk.leftBound().back().basicPoint2d(); + const Eigen::Vector2d back_center_point = (r_p_back + l_p_back) / 2.0; - return std::make_pair(front_entry_point, back_entry_point); + return CrosswalkEdgePoints{front_center_point, r_p_front, l_p_front, + back_center_point, r_p_back, l_p_back}; } bool withinLanelet( @@ -356,17 +370,13 @@ bool withinLanelet( boost::geometry::correct(polygon); bool with_in_polygon = boost::geometry::within(p_object, polygon); - if (!use_yaw_information) { - return with_in_polygon; - } else { - // use yaw angle to compare - const double abs_yaw_diff = calcAbsYawDiffBetweenLaneletAndObject(object, lanelet); - if (abs_yaw_diff < yaw_threshold) { - return with_in_polygon; - } else { - return false; - } - } + if (!use_yaw_information) return with_in_polygon; + + // use yaw angle to compare + const double abs_yaw_diff = calcAbsYawDiffBetweenLaneletAndObject(object, lanelet); + if (abs_yaw_diff < yaw_threshold) return with_in_polygon; + + return false; } bool withinRoadLanelet( @@ -402,8 +412,8 @@ bool withinRoadLanelet( return false; } -boost::optional isReachableEntryPoint( - const TrackedObject & object, const EntryPoint & entry_point, +boost::optional isReachableCrosswalkEdgePoints( + const TrackedObject & object, const CrosswalkEdgePoints & edge_points, const lanelet::LaneletMapPtr & lanelet_map_ptr, const double time_horizon, const double min_object_vel) { @@ -414,15 +424,15 @@ boost::optional isReachableEntryPoint( const auto & obj_vel = object.kinematics.twist_with_covariance.twist.linear; const auto yaw = tier4_autoware_utils::getRPY(object.kinematics.pose_with_covariance.pose).z; - const auto & p1 = entry_point.first; - const auto & p2 = entry_point.second; + const auto & p1 = edge_points.front_center_point; + const auto & p2 = edge_points.back_center_point; - auto ret = std::make_pair(p1, p2); + CrosswalkEdgePoints ret{p1, {}, {}, p2, {}, {}}; auto distance_pedestrian_to_p1 = std::hypot(p1.x() - obj_pos.x, p1.y() - obj_pos.y); auto distance_pedestrian_to_p2 = std::hypot(p2.x() - obj_pos.x, p2.y() - obj_pos.y); if (distance_pedestrian_to_p2 < distance_pedestrian_to_p1) { - std::swap(ret.first, ret.second); + ret.swap(); std::swap(distance_pedestrian_to_p1, distance_pedestrian_to_p2); } @@ -451,7 +461,8 @@ boost::optional isReachableEntryPoint( } { - const Line object_to_entry_point{{obj_pos.x, obj_pos.y}, {ret.first.x(), ret.first.y()}}; + const Line object_to_entry_point{ + {obj_pos.x, obj_pos.y}, {ret.front_center_point.x(), ret.front_center_point.y()}}; std::vector tmp_intersects; boost::geometry::intersection( object_to_entry_point, lanelet.second.polygon2d().basicPolygon(), tmp_intersects); @@ -461,7 +472,8 @@ boost::optional isReachableEntryPoint( } { - const Line object_to_entry_point{{obj_pos.x, obj_pos.y}, {ret.second.x(), ret.second.y()}}; + const Line object_to_entry_point{ + {obj_pos.x, obj_pos.y}, {ret.back_center_point.x(), ret.back_center_point.y()}}; std::vector tmp_intersects; boost::geometry::intersection( object_to_entry_point, lanelet.second.polygon2d().basicPolygon(), tmp_intersects); @@ -484,12 +496,12 @@ boost::optional isReachableEntryPoint( } if (first_intersect_load && !second_intersect_load) { - std::swap(ret.first, ret.second); + ret.swap(); } const Eigen::Vector2d pedestrian_to_crosswalk( - (ret.first.x() + ret.second.x()) / 2.0 - obj_pos.x, - (ret.first.y() + ret.second.y()) / 2.0 - obj_pos.y); + (ret.front_center_point.x() + ret.back_center_point.x()) / 2.0 - obj_pos.x, + (ret.front_center_point.y() + ret.back_center_point.y()) / 2.0 - obj_pos.y); const Eigen::Vector2d pedestrian_heading_direction( obj_vel.x * std::cos(yaw), obj_vel.x * std::sin(yaw)); const auto reachable = @@ -505,8 +517,10 @@ boost::optional isReachableEntryPoint( } bool hasPotentialToReach( - const TrackedObject & object, const Eigen::Vector2d & point, const double time_horizon, - const double min_object_vel) + const TrackedObject & object, const Eigen::Vector2d & center_point, + const Eigen::Vector2d & right_point, const Eigen::Vector2d & left_point, + const double time_horizon, const double min_object_vel, + const double max_crosswalk_user_delta_yaw_threshold_for_lanelet) { const auto & obj_pos = object.kinematics.pose_with_covariance.pose.position; const auto & obj_vel = object.kinematics.twist_with_covariance.twist.linear; @@ -517,13 +531,47 @@ bool hasPotentialToReach( const auto is_stop_object = estimated_velocity < stop_velocity_th; const auto velocity = std::max(min_object_vel, estimated_velocity); - const Eigen::Vector2d pedestrian_to_crosswalk(point.x() - obj_pos.x, point.y() - obj_pos.y); - const Eigen::Vector2d pedestrian_heading_direction( - obj_vel.x * std::cos(yaw), obj_vel.x * std::sin(yaw)); - const auto heading_for_crosswalk = - pedestrian_to_crosswalk.dot(pedestrian_heading_direction) > 0.0; + const double pedestrian_to_crosswalk_center_direction = + std::atan2(center_point.y() - obj_pos.y, center_point.x() - obj_pos.x); + + const auto + [pedestrian_to_crosswalk_right_rel_direction, pedestrian_to_crosswalk_left_rel_direction] = + [&]() { + const double pedestrian_to_crosswalk_right_direction = + std::atan2(right_point.y() - obj_pos.y, right_point.x() - obj_pos.x); + const double pedestrian_to_crosswalk_left_direction = + std::atan2(left_point.y() - obj_pos.y, left_point.x() - obj_pos.x); + return std::make_pair( + tier4_autoware_utils::normalizeRadian( + pedestrian_to_crosswalk_right_direction - pedestrian_to_crosswalk_center_direction), + tier4_autoware_utils::normalizeRadian( + pedestrian_to_crosswalk_left_direction - pedestrian_to_crosswalk_center_direction)); + }(); + + const double pedestrian_heading_rel_direction = [&]() { + const double pedestrian_heading_direction = + std::atan2(obj_vel.x * std::sin(yaw), obj_vel.x * std::cos(yaw)); + return tier4_autoware_utils::normalizeRadian( + pedestrian_heading_direction - pedestrian_to_crosswalk_center_direction); + }(); - const auto reachable = pedestrian_to_crosswalk.norm() < velocity * time_horizon; + const double pedestrian_to_crosswalk_min_rel_direction = std::min( + pedestrian_to_crosswalk_right_rel_direction, pedestrian_to_crosswalk_left_rel_direction); + const double pedestrian_to_crosswalk_max_rel_direction = std::max( + pedestrian_to_crosswalk_right_rel_direction, pedestrian_to_crosswalk_left_rel_direction); + const double pedestrian_vel_angle_against_crosswalk = [&]() { + if (pedestrian_heading_rel_direction < pedestrian_to_crosswalk_min_rel_direction) { + return pedestrian_to_crosswalk_min_rel_direction - pedestrian_heading_rel_direction; + } + if (pedestrian_to_crosswalk_max_rel_direction < pedestrian_heading_rel_direction) { + return pedestrian_to_crosswalk_max_rel_direction - pedestrian_heading_rel_direction; + } + return 0.0; + }(); + const auto heading_for_crosswalk = std::abs(pedestrian_vel_angle_against_crosswalk) < + max_crosswalk_user_delta_yaw_threshold_for_lanelet; + const auto reachable = std::hypot(center_point.x() - obj_pos.x, center_point.y() - obj_pos.y) < + velocity * time_horizon; if (reachable && (heading_for_crosswalk || is_stop_object)) { return true; @@ -543,48 +591,53 @@ ObjectClassification::_label_type changeLabelForPrediction( const lanelet::LaneletMapPtr & lanelet_map_ptr_) { // for car like vehicle do not change labels - if ( - label == ObjectClassification::CAR || label == ObjectClassification::BUS || - label == ObjectClassification::TRUCK || label == ObjectClassification::TRAILER || - label == ObjectClassification::UNKNOWN) { - return label; - } else if ( // for bicycle and motorcycle - label == ObjectClassification::MOTORCYCLE || label == ObjectClassification::BICYCLE) { - // if object is within road lanelet and satisfies yaw constraints - const bool within_road_lanelet = withinRoadLanelet(object, lanelet_map_ptr_, true); - const float high_speed_threshold = 25.0 / 18.0 * 5.0; // High speed bicycle 25 km/h - const bool high_speed_object = - object.kinematics.twist_with_covariance.twist.linear.x > high_speed_threshold; - - // if the object is within lanelet, do the same estimation with vehicle - if (within_road_lanelet) { - return ObjectClassification::MOTORCYCLE; - } else if (high_speed_object) { + switch (label) { + case ObjectClassification::CAR: + case ObjectClassification::BUS: + case ObjectClassification::TRUCK: + case ObjectClassification::TRAILER: + case ObjectClassification::UNKNOWN: + return label; + + case ObjectClassification::MOTORCYCLE: + case ObjectClassification::BICYCLE: { // if object is within road lanelet and satisfies yaw + // constraints + const bool within_road_lanelet = withinRoadLanelet(object, lanelet_map_ptr_, true); + const float high_speed_threshold = + tier4_autoware_utils::kmph2mps(25.0); // High speed bicycle 25 km/h + // calc abs speed from x and y velocity + const double abs_speed = std::hypot( + object.kinematics.twist_with_covariance.twist.linear.x, + object.kinematics.twist_with_covariance.twist.linear.y); + const bool high_speed_object = abs_speed > high_speed_threshold; + + // if the object is within lanelet, do the same estimation with vehicle + if (within_road_lanelet) return ObjectClassification::MOTORCYCLE; // high speed object outside road lanelet will move like unknown object // return ObjectClassification::UNKNOWN; // temporary disabled - return label; - } else { + if (high_speed_object) return label; // Do nothing for now return ObjectClassification::BICYCLE; } - } else if (label == ObjectClassification::PEDESTRIAN) { - const bool within_road_lanelet = withinRoadLanelet(object, lanelet_map_ptr_, true); - const float max_velocity_for_human_mps = - 25.0 / 18.0 * 5.0; // Max human being motion speed is 25km/h - const bool high_speed_object = - object.kinematics.twist_with_covariance.twist.linear.x > max_velocity_for_human_mps; - // fast, human-like object: like segway - if (within_road_lanelet && high_speed_object) { - return label; // currently do nothing + + case ObjectClassification::PEDESTRIAN: { + const bool within_road_lanelet = withinRoadLanelet(object, lanelet_map_ptr_, true); + const float max_velocity_for_human_mps = + tier4_autoware_utils::kmph2mps(25.0); // Max human being motion speed is 25km/h + const double abs_speed = std::hypot( + object.kinematics.twist_with_covariance.twist.linear.x, + object.kinematics.twist_with_covariance.twist.linear.y); + const bool high_speed_object = abs_speed > max_velocity_for_human_mps; + // fast, human-like object: like segway + if (within_road_lanelet && high_speed_object) return label; // currently do nothing // return ObjectClassification::MOTORCYCLE; - } else if (high_speed_object) { - return label; // currently do nothing + if (high_speed_object) return label; // currently do nothing // fast human outside road lanelet will move like unknown object // return ObjectClassification::UNKNOWN; - } else { return label; } - } else { - return label; + + default: + return label; } } @@ -639,33 +692,35 @@ lanelet::Lanelets getLeftOppositeLanelets( MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_options) : Node("map_based_prediction", node_options), debug_accumulated_time_(0.0) { - enable_delay_compensation_ = declare_parameter("enable_delay_compensation", true); - prediction_time_horizon_ = declare_parameter("prediction_time_horizon", 10.0); - prediction_sampling_time_interval_ = declare_parameter("prediction_sampling_delta_time", 0.5); + enable_delay_compensation_ = declare_parameter("enable_delay_compensation"); + prediction_time_horizon_ = declare_parameter("prediction_time_horizon"); + lateral_control_time_horizon_ = + declare_parameter("lateral_control_time_horizon"); // [s] for lateral control point + prediction_sampling_time_interval_ = declare_parameter("prediction_sampling_delta_time"); min_velocity_for_map_based_prediction_ = - declare_parameter("min_velocity_for_map_based_prediction", 1.0); - min_crosswalk_user_velocity_ = declare_parameter("min_crosswalk_user_velocity", 1.0); + declare_parameter("min_velocity_for_map_based_prediction"); + min_crosswalk_user_velocity_ = declare_parameter("min_crosswalk_user_velocity"); + max_crosswalk_user_delta_yaw_threshold_for_lanelet_ = + declare_parameter("max_crosswalk_user_delta_yaw_threshold_for_lanelet"); dist_threshold_for_searching_lanelet_ = - declare_parameter("dist_threshold_for_searching_lanelet", 3.0); + declare_parameter("dist_threshold_for_searching_lanelet"); delta_yaw_threshold_for_searching_lanelet_ = - declare_parameter("delta_yaw_threshold_for_searching_lanelet", 0.785); - sigma_lateral_offset_ = declare_parameter("sigma_lateral_offset", 0.5); - sigma_yaw_angle_deg_ = declare_parameter("sigma_yaw_angle_deg", 5.0); - object_buffer_time_length_ = declare_parameter("object_buffer_time_length", 2.0); - history_time_length_ = declare_parameter("history_time_length", 1.0); + declare_parameter("delta_yaw_threshold_for_searching_lanelet"); + sigma_lateral_offset_ = declare_parameter("sigma_lateral_offset"); + sigma_yaw_angle_deg_ = declare_parameter("sigma_yaw_angle_deg"); + object_buffer_time_length_ = declare_parameter("object_buffer_time_length"); + history_time_length_ = declare_parameter("history_time_length"); { // lane change detection lane_change_detection_method_ = declare_parameter("lane_change_detection.method"); // lane change detection by time_to_change_lane - dist_threshold_to_bound_ = declare_parameter( - "lane_change_detection.time_to_change_lane.dist_threshold_for_lane_change_detection", - 1.0); // 1m - time_threshold_to_bound_ = declare_parameter( - "lane_change_detection.time_to_change_lane.time_threshold_for_lane_change_detection", - 5.0); // 5 sec - cutoff_freq_of_velocity_lpf_ = declare_parameter( - "lane_change_detection.time_to_change_lane.cutoff_freq_of_velocity_for_lane_change_detection", - 0.1); // 0.1Hz + dist_threshold_to_bound_ = declare_parameter( + "lane_change_detection.time_to_change_lane.dist_threshold_for_lane_change_detection"); // 1m + time_threshold_to_bound_ = declare_parameter( + "lane_change_detection.time_to_change_lane.time_threshold_for_lane_change_detection"); + cutoff_freq_of_velocity_lpf_ = declare_parameter( + "lane_change_detection.time_to_change_lane.cutoff_freq_of_velocity_for_lane_change_" + "detection"); // lane change detection by lat_diff_distance dist_ratio_threshold_to_left_bound_ = declare_parameter( @@ -680,23 +735,24 @@ MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_ num_continuous_state_transition_ = declare_parameter("lane_change_detection.num_continuous_state_transition"); } - reference_path_resolution_ = declare_parameter("reference_path_resolution", 0.5); + reference_path_resolution_ = declare_parameter("reference_path_resolution"); /* prediction path will disabled when the estimated path length exceeds lanelet length. This * parameter control the estimated path length = vx * th * (rate) */ prediction_time_horizon_rate_for_validate_lane_length_ = - declare_parameter("prediction_time_horizon_rate_for_validate_lane_length", 0.8); + declare_parameter("prediction_time_horizon_rate_for_validate_shoulder_lane_length"); path_generator_ = std::make_shared( - prediction_time_horizon_, prediction_sampling_time_interval_, min_crosswalk_user_velocity_); + prediction_time_horizon_, lateral_control_time_horizon_, prediction_sampling_time_interval_, + min_crosswalk_user_velocity_); sub_objects_ = this->create_subscription( - "/perception/object_recognition/tracking/objects", 1, + "~/input/objects", 1, std::bind(&MapBasedPredictionNode::objectsCallback, this, std::placeholders::_1)); sub_map_ = this->create_subscription( "/vector_map", rclcpp::QoS{1}.transient_local(), std::bind(&MapBasedPredictionNode::mapCallback, this, std::placeholders::_1)); - pub_objects_ = this->create_publisher("objects", rclcpp::QoS{1}); + pub_objects_ = this->create_publisher("~/output/objects", rclcpp::QoS{1}); pub_debug_markers_ = this->create_publisher("maneuver", rclcpp::QoS{1}); pub_calculation_time_ = create_publisher("~/debug/calculation_time", 1); @@ -794,101 +850,99 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt const auto & label_ = transformed_object.classification.front().label; const auto label = changeLabelForPrediction(label_, object, lanelet_map_ptr_); - // For crosswalk user - if (label == ObjectClassification::PEDESTRIAN || label == ObjectClassification::BICYCLE) { - const auto predicted_object = getPredictedObjectAsCrosswalkUser(transformed_object); - output.objects.push_back(predicted_object); - // For road user - } else if ( - label == ObjectClassification::CAR || label == ObjectClassification::BUS || - label == ObjectClassification::TRAILER || label == ObjectClassification::MOTORCYCLE || - label == ObjectClassification::TRUCK) { - // Update object yaw and velocity - updateObjectData(transformed_object); - - // Get Closest Lanelet - const auto current_lanelets = getCurrentLanelets(transformed_object); - - // Update Objects History - updateObjectsHistory(output.header, transformed_object, current_lanelets); - - // For off lane obstacles - if (current_lanelets.empty()) { - PredictedPath predicted_path = - path_generator_->generatePathForOffLaneVehicle(transformed_object); - predicted_path.confidence = 1.0; - if (predicted_path.path.empty()) { - continue; - } - - auto predicted_object = convertToPredictedObject(transformed_object); - predicted_object.kinematics.predicted_paths.push_back(predicted_path); - output.objects.push_back(predicted_object); - continue; + switch (label) { + case ObjectClassification::PEDESTRIAN: + case ObjectClassification::BICYCLE: { + const auto predicted_object_crosswalk = + getPredictedObjectAsCrosswalkUser(transformed_object); + output.objects.push_back(predicted_object_crosswalk); + break; } - - // For too-slow vehicle - if ( - std::fabs(transformed_object.kinematics.twist_with_covariance.twist.linear.x) < - min_velocity_for_map_based_prediction_) { - PredictedPath predicted_path = - path_generator_->generatePathForLowSpeedVehicle(transformed_object); - predicted_path.confidence = 1.0; - if (predicted_path.path.empty()) { - continue; + case ObjectClassification::CAR: + case ObjectClassification::BUS: + case ObjectClassification::TRAILER: + case ObjectClassification::MOTORCYCLE: + case ObjectClassification::TRUCK: { + // Update object yaw and velocity + updateObjectData(transformed_object); + + // Get Closest Lanelet + const auto current_lanelets = getCurrentLanelets(transformed_object); + + // Update Objects History + updateObjectsHistory(output.header, transformed_object, current_lanelets); + + // For off lane obstacles + if (current_lanelets.empty()) { + PredictedPath predicted_path = + path_generator_->generatePathForOffLaneVehicle(transformed_object); + predicted_path.confidence = 1.0; + if (predicted_path.path.empty()) break; + + auto predicted_object_vehicle = convertToPredictedObject(transformed_object); + predicted_object_vehicle.kinematics.predicted_paths.push_back(predicted_path); + output.objects.push_back(predicted_object_vehicle); + break; } - auto predicted_object = convertToPredictedObject(transformed_object); - predicted_object.kinematics.predicted_paths.push_back(predicted_path); - output.objects.push_back(predicted_object); - continue; - } - - // Get Predicted Reference Path for Each Maneuver and current lanelets - // return: - const auto ref_paths = - getPredictedReferencePath(transformed_object, current_lanelets, objects_detected_time); - - // If predicted reference path is empty, assume this object is out of the lane - if (ref_paths.empty()) { - PredictedPath predicted_path = - path_generator_->generatePathForLowSpeedVehicle(transformed_object); - predicted_path.confidence = 1.0; - if (predicted_path.path.empty()) { - continue; + // For too-slow vehicle + const double abs_obj_speed = std::hypot( + transformed_object.kinematics.twist_with_covariance.twist.linear.x, + transformed_object.kinematics.twist_with_covariance.twist.linear.y); + if (std::fabs(abs_obj_speed) < min_velocity_for_map_based_prediction_) { + PredictedPath predicted_path = + path_generator_->generatePathForLowSpeedVehicle(transformed_object); + predicted_path.confidence = 1.0; + if (predicted_path.path.empty()) break; + + auto predicted_slow_object = convertToPredictedObject(transformed_object); + predicted_slow_object.kinematics.predicted_paths.push_back(predicted_path); + output.objects.push_back(predicted_slow_object); + break; } - auto predicted_object = convertToPredictedObject(transformed_object); - predicted_object.kinematics.predicted_paths.push_back(predicted_path); - output.objects.push_back(predicted_object); - continue; - } + // Get Predicted Reference Path for Each Maneuver and current lanelets + // return: + const auto ref_paths = + getPredictedReferencePath(transformed_object, current_lanelets, objects_detected_time); + + // If predicted reference path is empty, assume this object is out of the lane + if (ref_paths.empty()) { + PredictedPath predicted_path = + path_generator_->generatePathForLowSpeedVehicle(transformed_object); + predicted_path.confidence = 1.0; + if (predicted_path.path.empty()) break; + + auto predicted_object_out_of_lane = convertToPredictedObject(transformed_object); + predicted_object_out_of_lane.kinematics.predicted_paths.push_back(predicted_path); + output.objects.push_back(predicted_object_out_of_lane); + break; + } - // Get Debug Marker for On Lane Vehicles - const auto max_prob_path = std::max_element( - ref_paths.begin(), ref_paths.end(), - [](const PredictedRefPath & a, const PredictedRefPath & b) { - return a.probability < b.probability; - }); - const auto debug_marker = - getDebugMarker(object, max_prob_path->maneuver, debug_markers.markers.size()); - debug_markers.markers.push_back(debug_marker); - - // Generate Predicted Path - std::vector predicted_paths; - for (const auto & ref_path : ref_paths) { - PredictedPath predicted_path = - path_generator_->generatePathForOnLaneVehicle(transformed_object, ref_path.path); - if (predicted_path.path.empty()) { - continue; + // Get Debug Marker for On Lane Vehicles + const auto max_prob_path = std::max_element( + ref_paths.begin(), ref_paths.end(), + [](const PredictedRefPath & a, const PredictedRefPath & b) { + return a.probability < b.probability; + }); + const auto debug_marker = + getDebugMarker(object, max_prob_path->maneuver, debug_markers.markers.size()); + debug_markers.markers.push_back(debug_marker); + + // Generate Predicted Path + std::vector predicted_paths; + for (const auto & ref_path : ref_paths) { + PredictedPath predicted_path = + path_generator_->generatePathForOnLaneVehicle(transformed_object, ref_path.path); + if (predicted_path.path.empty()) { + continue; + } + predicted_path.confidence = ref_path.probability; + predicted_paths.push_back(predicted_path); } - predicted_path.confidence = ref_path.probability; - predicted_paths.push_back(predicted_path); - } + // Normalize Path Confidence and output the predicted object - // Normalize Path Confidence and output the predicted object - { float sum_confidence = 0.0; for (const auto & predicted_path : predicted_paths) { sum_confidence += predicted_path.confidence; @@ -896,25 +950,25 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt const float min_sum_confidence_value = 1e-3; sum_confidence = std::max(sum_confidence, min_sum_confidence_value); + auto predicted_object = convertToPredictedObject(transformed_object); + for (auto & predicted_path : predicted_paths) { predicted_path.confidence = predicted_path.confidence / sum_confidence; - } - - auto predicted_object = convertToPredictedObject(transformed_object); - for (const auto & predicted_path : predicted_paths) { predicted_object.kinematics.predicted_paths.push_back(predicted_path); } output.objects.push_back(predicted_object); + break; } - // For unknown object - } else { - auto predicted_object = convertToPredictedObject(transformed_object); - PredictedPath predicted_path = - path_generator_->generatePathForNonVehicleObject(transformed_object); - predicted_path.confidence = 1.0; + default: { + auto predicted_unknown_object = convertToPredictedObject(transformed_object); + PredictedPath predicted_path = + path_generator_->generatePathForNonVehicleObject(transformed_object); + predicted_path.confidence = 1.0; - predicted_object.kinematics.predicted_paths.push_back(predicted_path); - output.objects.push_back(predicted_object); + predicted_unknown_object.kinematics.predicted_paths.push_back(predicted_path); + output.objects.push_back(predicted_unknown_object); + break; + } } } @@ -925,6 +979,46 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt pub_calculation_time_->publish(calculation_time_msg); } +bool MapBasedPredictionNode::doesPathCrossAnyFence(const PredictedPath & predicted_path) +{ + const lanelet::ConstLineStrings3d & all_fences = + lanelet::utils::query::getAllFences(lanelet_map_ptr_); + for (const auto & fence_line : all_fences) { + if (doesPathCrossFence(predicted_path, fence_line)) { + return true; + } + } + return false; +} + +bool MapBasedPredictionNode::doesPathCrossFence( + const PredictedPath & predicted_path, const lanelet::ConstLineString3d & fence_line) +{ + // check whether the predicted path cross with fence + for (size_t i = 0; i < predicted_path.path.size(); ++i) { + for (size_t j = 0; j < fence_line.size() - 1; ++j) { + if (isIntersecting( + predicted_path.path[i].position, predicted_path.path[i + 1].position, fence_line[j], + fence_line[j + 1])) { + return true; + } + } + } + return false; +} + +bool MapBasedPredictionNode::isIntersecting( + const geometry_msgs::msg::Point & point1, const geometry_msgs::msg::Point & point2, + const lanelet::ConstPoint3d & point3, const lanelet::ConstPoint3d & point4) +{ + const auto p1 = tier4_autoware_utils::createPoint(point1.x, point1.y, 0.0); + const auto p2 = tier4_autoware_utils::createPoint(point2.x, point2.y, 0.0); + const auto p3 = tier4_autoware_utils::createPoint(point3.x(), point3.y(), 0.0); + const auto p4 = tier4_autoware_utils::createPoint(point4.x(), point4.y(), 0.0); + const auto intersection = tier4_autoware_utils::intersect(p1, p2, p3, p4); + return intersection.has_value(); +} + PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser( const TrackedObject & object) { @@ -944,27 +1038,32 @@ PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser( } } + // If the object is in the crosswalk, generate path to the crosswalk edge if (crossing_crosswalk) { - const auto entry_point = getCrosswalkEntryPoint(crossing_crosswalk.get()); + const auto edge_points = getCrosswalkEdgePoints(crossing_crosswalk.get()); if (hasPotentialToReach( - object, entry_point.first, std::numeric_limits::max(), - min_crosswalk_user_velocity_)) { + object, edge_points.front_center_point, edge_points.front_right_point, + edge_points.front_left_point, std::numeric_limits::max(), + min_crosswalk_user_velocity_, max_crosswalk_user_delta_yaw_threshold_for_lanelet_)) { PredictedPath predicted_path = - path_generator_->generatePathToTargetPoint(object, entry_point.first); + path_generator_->generatePathToTargetPoint(object, edge_points.front_center_point); predicted_path.confidence = 1.0; predicted_object.kinematics.predicted_paths.push_back(predicted_path); } if (hasPotentialToReach( - object, entry_point.second, std::numeric_limits::max(), - min_crosswalk_user_velocity_)) { + object, edge_points.back_center_point, edge_points.back_right_point, + edge_points.back_left_point, std::numeric_limits::max(), + min_crosswalk_user_velocity_, max_crosswalk_user_delta_yaw_threshold_for_lanelet_)) { PredictedPath predicted_path = - path_generator_->generatePathToTargetPoint(object, entry_point.second); + path_generator_->generatePathToTargetPoint(object, edge_points.back_center_point); predicted_path.confidence = 1.0; predicted_object.kinematics.predicted_paths.push_back(predicted_path); } + // If the object is not crossing the crosswalk, in the road lanelets, try to find the closest + // crosswalk and generate path to the crosswalk edge } else if (withinRoadLanelet(object, lanelet_map_ptr_)) { lanelet::ConstLanelet closest_crosswalk{}; const auto & obj_pose = object.kinematics.pose_with_covariance.pose; @@ -972,42 +1071,50 @@ PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser( lanelet::utils::query::getClosestLanelet(crosswalks_, obj_pose, &closest_crosswalk); if (found_closest_crosswalk) { - const auto entry_point = getCrosswalkEntryPoint(closest_crosswalk); + const auto edge_points = getCrosswalkEdgePoints(closest_crosswalk); if (hasPotentialToReach( - object, entry_point.first, prediction_time_horizon_ * 2.0, - min_crosswalk_user_velocity_)) { + object, edge_points.front_center_point, edge_points.front_right_point, + edge_points.front_left_point, prediction_time_horizon_ * 2.0, + min_crosswalk_user_velocity_, max_crosswalk_user_delta_yaw_threshold_for_lanelet_)) { PredictedPath predicted_path = - path_generator_->generatePathToTargetPoint(object, entry_point.first); + path_generator_->generatePathToTargetPoint(object, edge_points.front_center_point); predicted_path.confidence = 1.0; predicted_object.kinematics.predicted_paths.push_back(predicted_path); } if (hasPotentialToReach( - object, entry_point.second, prediction_time_horizon_ * 2.0, - min_crosswalk_user_velocity_)) { + object, edge_points.back_center_point, edge_points.back_right_point, + edge_points.back_left_point, prediction_time_horizon_ * 2.0, + min_crosswalk_user_velocity_, max_crosswalk_user_delta_yaw_threshold_for_lanelet_)) { PredictedPath predicted_path = - path_generator_->generatePathToTargetPoint(object, entry_point.second); + path_generator_->generatePathToTargetPoint(object, edge_points.back_center_point); predicted_path.confidence = 1.0; predicted_object.kinematics.predicted_paths.push_back(predicted_path); } } + // If the object is not crossing the crosswalk, not in the road lanelets, try to find the edge + // points for all crosswalks and generate path to the crosswalk edge } else { for (const auto & crosswalk : crosswalks_) { - const auto entry_point = getCrosswalkEntryPoint(crosswalk); + const auto edge_points = getCrosswalkEdgePoints(crosswalk); const auto reachable_first = hasPotentialToReach( - object, entry_point.first, prediction_time_horizon_, min_crosswalk_user_velocity_); + object, edge_points.front_center_point, edge_points.front_right_point, + edge_points.front_left_point, prediction_time_horizon_, min_crosswalk_user_velocity_, + max_crosswalk_user_delta_yaw_threshold_for_lanelet_); const auto reachable_second = hasPotentialToReach( - object, entry_point.second, prediction_time_horizon_, min_crosswalk_user_velocity_); + object, edge_points.back_center_point, edge_points.back_right_point, + edge_points.back_left_point, prediction_time_horizon_, min_crosswalk_user_velocity_, + max_crosswalk_user_delta_yaw_threshold_for_lanelet_); if (!reachable_first && !reachable_second) { continue; } - const auto reachable_crosswalk = isReachableEntryPoint( - object, entry_point, lanelet_map_ptr_, prediction_time_horizon_, + const auto reachable_crosswalk = isReachableCrosswalkEdgePoints( + object, edge_points, lanelet_map_ptr_, prediction_time_horizon_, min_crosswalk_user_velocity_); if (!reachable_crosswalk) { @@ -1021,6 +1128,10 @@ PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser( if (predicted_path.path.empty()) { continue; } + // If the predicted path to the crosswalk is crossing the fence, don't use it + if (doesPathCrossAnyFence(predicted_path)) { + continue; + } predicted_object.kinematics.predicted_paths.push_back(predicted_path); } @@ -1048,25 +1159,29 @@ void MapBasedPredictionNode::updateObjectData(TrackedObject & object) const auto future_object_pose = tier4_autoware_utils::calcOffsetPose( object_pose, object_twist.linear.x * 0.1, object_twist.linear.y * 0.1, 0.0); - if (object.kinematics.twist_with_covariance.twist.linear.x < 0.0) { - if ( - object.kinematics.orientation_availability == - autoware_auto_perception_msgs::msg::DetectedObjectKinematics::SIGN_UNKNOWN) { + // assumption: the object vx is much larger than vy + if (object.kinematics.twist_with_covariance.twist.linear.x >= 0.0) return; + + switch (object.kinematics.orientation_availability) { + case autoware_auto_perception_msgs::msg::DetectedObjectKinematics::SIGN_UNKNOWN: { const auto original_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); // flip the angle object.kinematics.pose_with_covariance.pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(tier4_autoware_utils::pi + original_yaw); - } else { + break; + } + default: { const auto updated_object_yaw = tier4_autoware_utils::calcAzimuthAngle(object_pose.position, future_object_pose.position); object.kinematics.pose_with_covariance.pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(updated_object_yaw); + break; } - - object.kinematics.twist_with_covariance.twist.linear.x *= -1.0; } + object.kinematics.twist_with_covariance.twist.linear.x *= -1.0; + object.kinematics.twist_with_covariance.twist.linear.y *= -1.0; return; } @@ -1184,7 +1299,7 @@ LaneletsData MapBasedPredictionNode::getCurrentLanelets(const TrackedObject & ob for (const auto & lanelet : surrounding_opposite_lanelets) { // Check if the close lanelets meet the necessary condition for start lanelets // except for distance checking - if (!checkCloseLaneletCondition(lanelet, object, false)) { + if (!checkCloseLaneletCondition(lanelet, object)) { continue; } @@ -1204,8 +1319,7 @@ LaneletsData MapBasedPredictionNode::getCurrentLanelets(const TrackedObject & ob } bool MapBasedPredictionNode::checkCloseLaneletCondition( - const std::pair & lanelet, const TrackedObject & object, - const bool check_distance) + const std::pair & lanelet, const TrackedObject & object) { // Step1. If we only have one point in the centerline, we will ignore the lanelet if (lanelet.second.centerline().size() <= 1) { @@ -1240,7 +1354,7 @@ bool MapBasedPredictionNode::checkCloseLaneletCondition( const double object_vel = object.kinematics.twist_with_covariance.twist.linear.x; const bool is_yaw_reversed = M_PI - delta_yaw_threshold_for_searching_lanelet_ < abs_norm_delta && object_vel < 0.0; - if (check_distance && dist_threshold_for_searching_lanelet_ < lanelet.first) { + if (dist_threshold_for_searching_lanelet_ < lanelet.first) { return false; } if (!is_yaw_reversed && delta_yaw_threshold_for_searching_lanelet_ < abs_norm_delta) { @@ -1328,7 +1442,9 @@ std::vector MapBasedPredictionNode::getPredictedReferencePath( const TrackedObject & object, const LaneletsData & current_lanelets_data, const double object_detected_time) { - const double obj_vel = std::fabs(object.kinematics.twist_with_covariance.twist.linear.x); + const double obj_vel = std::hypot( + object.kinematics.twist_with_covariance.twist.linear.x, + object.kinematics.twist_with_covariance.twist.linear.y); std::vector all_ref_paths; for (const auto & current_lanelet_data : current_lanelets_data) { @@ -1669,21 +1785,22 @@ geometry_msgs::msg::Pose MapBasedPredictionNode::compensateTimeDelay( /* == Nonlinear model == * - * x_{k+1} = x_k + vx_k * cos(yaw_k) * dt - * y_{k+1} = y_k + vx_k * sin(yaw_k) * dt + * x_{k+1} = x_k + vx_k * cos(yaw_k) * dt - vy_k * sin(yaw_k) * dt + * y_{k+1} = y_k + vx_k * sin(yaw_k) * dt + vy_k * cos(yaw_k) * dt * yaw_{k+1} = yaw_k + (wz_k) * dt * */ const double vx = twist.linear.x; + const double vy = twist.linear.y; const double wz = twist.angular.z; const double prev_yaw = tf2::getYaw(delayed_pose.orientation); const double prev_x = delayed_pose.position.x; const double prev_y = delayed_pose.position.y; const double prev_z = delayed_pose.position.z; - const double curr_x = prev_x + vx * std::cos(prev_yaw) * dt; - const double curr_y = prev_y + vx * std::sin(prev_yaw) * dt; + const double curr_x = prev_x + vx * std::cos(prev_yaw) * dt - vy * std::sin(prev_yaw) * dt; + const double curr_y = prev_y + vx * std::sin(prev_yaw) * dt + vy * std::cos(prev_yaw) * dt; const double curr_z = prev_z; const double curr_yaw = prev_yaw + wz * dt; @@ -1795,16 +1912,10 @@ ManeuverProbability MapBasedPredictionNode::calculateManeuverProbability( constexpr float LC_PROB = 1.0; // probability for left lane change constexpr float RC_PROB = 1.0; // probability for right lane change lane_follow_probability = 0.0; - if (!left_paths.empty() && right_paths.empty()) { - left_lane_change_probability = LC_PROB; - right_lane_change_probability = 0.0; - } else if (left_paths.empty() && !right_paths.empty()) { - left_lane_change_probability = 0.0; - right_lane_change_probability = RC_PROB; - } else { - left_lane_change_probability = LC_PROB; - right_lane_change_probability = RC_PROB; - } + + // If the given lane is empty, the probability goes to 0 + left_lane_change_probability = left_paths.empty() ? 0.0 : LC_PROB; + right_lane_change_probability = right_paths.empty() ? 0.0 : RC_PROB; } const float MIN_PROBABILITY = 1e-3; diff --git a/perception/map_based_prediction/src/path_generator.cpp b/perception/map_based_prediction/src/path_generator.cpp index 746a379a2d93e..5cb7e186b7cc4 100644 --- a/perception/map_based_prediction/src/path_generator.cpp +++ b/perception/map_based_prediction/src/path_generator.cpp @@ -15,17 +15,18 @@ #include "map_based_prediction/path_generator.hpp" #include -#include -#include +#include +#include #include namespace map_based_prediction { PathGenerator::PathGenerator( - const double time_horizon, const double sampling_time_interval, + const double time_horizon, const double lateral_time_horizon, const double sampling_time_interval, const double min_crosswalk_user_velocity) : time_horizon_(time_horizon), + lateral_time_horizon_(lateral_time_horizon), sampling_time_interval_(sampling_time_interval), min_crosswalk_user_velocity_(min_crosswalk_user_velocity) { @@ -70,7 +71,7 @@ PredictedPath PathGenerator::generatePathToTargetPoint( } PredictedPath PathGenerator::generatePathForCrosswalkUser( - const TrackedObject & object, const EntryPoint & reachable_crosswalk) const + const TrackedObject & object, const CrosswalkEdgePoints & reachable_crosswalk) const { PredictedPath predicted_path{}; const double ep = 0.001; @@ -79,10 +80,11 @@ PredictedPath PathGenerator::generatePathForCrosswalkUser( const auto & obj_vel = object.kinematics.twist_with_covariance.twist.linear; const Eigen::Vector2d pedestrian_to_entry_point( - reachable_crosswalk.first.x() - obj_pos.x, reachable_crosswalk.first.y() - obj_pos.y); + reachable_crosswalk.front_center_point.x() - obj_pos.x, + reachable_crosswalk.front_center_point.y() - obj_pos.y); const Eigen::Vector2d entry_to_exit_point( - reachable_crosswalk.second.x() - reachable_crosswalk.first.x(), - reachable_crosswalk.second.y() - reachable_crosswalk.first.y()); + reachable_crosswalk.back_center_point.x() - reachable_crosswalk.front_center_point.x(), + reachable_crosswalk.back_center_point.y() - reachable_crosswalk.front_center_point.y()); const auto velocity = std::max(std::hypot(obj_vel.x, obj_vel.y), min_crosswalk_user_velocity_); const auto arrival_time = pedestrian_to_entry_point.norm() / velocity; @@ -98,10 +100,10 @@ PredictedPath PathGenerator::generatePathForCrosswalkUser( predicted_path.path.push_back(world_frame_pose); } else { world_frame_pose.position.x = - reachable_crosswalk.first.x() + + reachable_crosswalk.front_center_point.x() + velocity * entry_to_exit_point.normalized().x() * (dt - arrival_time); world_frame_pose.position.y = - reachable_crosswalk.first.y() + + reachable_crosswalk.front_center_point.y() + velocity * entry_to_exit_point.normalized().y() * (dt - arrival_time); world_frame_pose.position.z = obj_pos.z; world_frame_pose.orientation = object.kinematics.pose_with_covariance.pose.orientation; @@ -212,18 +214,25 @@ FrenetPath PathGenerator::generateFrenetPath( { FrenetPath path; const double duration = time_horizon_; + const double lateral_duration = lateral_time_horizon_; // Compute Lateral and Longitudinal Coefficients to generate the trajectory - const Eigen::Vector3d lat_coeff = calcLatCoefficients(current_point, target_point, duration); + const Eigen::Vector3d lat_coeff = + calcLatCoefficients(current_point, target_point, lateral_duration); const Eigen::Vector2d lon_coeff = calcLonCoefficients(current_point, target_point, duration); path.reserve(static_cast(duration / sampling_time_interval_)); for (double t = 0.0; t <= duration; t += sampling_time_interval_) { - const double d_next = current_point.d + current_point.d_vel * t + 0 * 2 * std::pow(t, 2) + - lat_coeff(0) * std::pow(t, 3) + lat_coeff(1) * std::pow(t, 4) + - lat_coeff(2) * std::pow(t, 5); - const double s_next = current_point.s + current_point.s_vel * t + 2 * 0 * std::pow(t, 2) + - lon_coeff(0) * std::pow(t, 3) + lon_coeff(1) * std::pow(t, 4); + const double current_acc = + 0.0; // Currently we assume the object is traveling at a constant speed + const double d_next_ = current_point.d + current_point.d_vel * t + + current_acc * 2.0 * std::pow(t, 2) + lat_coeff(0) * std::pow(t, 3) + + lat_coeff(1) * std::pow(t, 4) + lat_coeff(2) * std::pow(t, 5); + // t > lateral_duration: 0.0, else d_next_ + const double d_next = t > lateral_duration ? 0.0 : d_next_; + const double s_next = current_point.s + current_point.s_vel * t + + 2.0 * current_acc * std::pow(t, 2) + lon_coeff(0) * std::pow(t, 3) + + lon_coeff(1) * std::pow(t, 4); if (s_next > max_length) { break; } 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 f115d8c07f72f..d2b6ee5de475e 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 @@ -21,14 +21,16 @@ #include #include -#include #include #include #include #include +#include #include +#include + #include #include #include diff --git a/perception/multi_object_tracker/models.md b/perception/multi_object_tracker/models.md index 0f7fc50ed535f..f74967943be32 100644 --- a/perception/multi_object_tracker/models.md +++ b/perception/multi_object_tracker/models.md @@ -2,6 +2,8 @@ ## Tracking model + + ### CTRV model [1] CTRV model is a model that assumes constant turn rate and velocity magnitude. @@ -65,6 +67,18 @@ $$ \end{array}\right] $$ +#### remarks on the output twist + +Remarks that the velocity $v_{k}$ is the norm of velocity of vehicle, not the longitudinal velocity. +So the output twist in the object coordinate $(x,y)$ is calculated as follows. + +$$ +\begin{aligned} +v_{x} &= v_{k} \cos \left(\beta_{k}\right) \\ +v_{y} &= v_{k} \sin \left(\beta_{k}\right) +\end{aligned} +$$ + ## Anchor point based estimation To separate the estimation of the position and the shape, we use anchor point based position estimation. @@ -94,5 +108,10 @@ In other words, the offset value must be adjusted so that the input BBOX and the ## References -[1] Schubert, Robin & Richter, Eric & Wanielik, Gerd. (2008). Comparison and evaluation of advanced motion models for vehicle tracking. 1 - 6. 10.1109/ICIF.2008.4632283. + + +[1] Schubert, Robin & Richter, Eric & Wanielik, Gerd. (2008). Comparison and evaluation of advanced motion models for vehicle tracking. 1 - 6. 10.1109/ICIF.2008.4632283. + + + [2] Kong, Jason & Pfeiffer, Mark & Schildbach, Georg & Borrelli, Francesco. (2015). Kinematic and dynamic vehicle models for autonomous driving control design. 1094-1099. 10.1109/IVS.2015.7225830. diff --git a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp index 2b0fff35729d0..6571e70b8c123 100644 --- a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp @@ -20,7 +20,9 @@ #include "multi_object_tracker/utils/utils.hpp" -#include +#include +#include +#include #include #include @@ -427,7 +429,8 @@ bool BicycleTracker::getTrackedObject( pose_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = P(IDX::YAW, IDX::YAW); // twist - twist_with_cov.twist.linear.x = X_t(IDX::VX); + twist_with_cov.twist.linear.x = X_t(IDX::VX) * std::cos(X_t(IDX::SLIP)); + twist_with_cov.twist.linear.y = X_t(IDX::VX) * std::sin(X_t(IDX::SLIP)); twist_with_cov.twist.angular.z = X_t(IDX::VX) / lr_ * std::sin(X_t(IDX::SLIP)); // yaw_rate = vx_k / l_r * sin(slip_k) // twist covariance 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 73916742f17bb..97d6d48c35d1b 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 @@ -34,7 +34,9 @@ #include #include -#include +#include +#include +#include using Label = autoware_auto_perception_msgs::msg::ObjectClassification; @@ -478,7 +480,8 @@ bool BigVehicleTracker::getTrackedObject( pose_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = P(IDX::YAW, IDX::YAW); // twist - twist_with_cov.twist.linear.x = X_t(IDX::VX); + twist_with_cov.twist.linear.x = X_t(IDX::VX) * std::cos(X_t(IDX::SLIP)); + twist_with_cov.twist.linear.y = X_t(IDX::VX) * std::sin(X_t(IDX::SLIP)); twist_with_cov.twist.angular.z = X_t(IDX::VX) / lr_ * std::sin(X_t(IDX::SLIP)); // yaw_rate = vx_k / l_r * sin(slip_k) // twist covariance 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 d9137bc67ef55..51adca7e69b56 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 @@ -18,8 +18,6 @@ #include "multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp" -#include - using Label = autoware_auto_perception_msgs::msg::ObjectClassification; MultipleVehicleTracker::MultipleVehicleTracker( 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 27c2a7b821eb6..aa3f7b1c30d01 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 @@ -34,7 +34,9 @@ #include #include -#include +#include +#include +#include using Label = autoware_auto_perception_msgs::msg::ObjectClassification; @@ -494,7 +496,8 @@ bool NormalVehicleTracker::getTrackedObject( pose_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = P(IDX::YAW, IDX::YAW); // twist - twist_with_cov.twist.linear.x = X_t(IDX::VX); + twist_with_cov.twist.linear.x = X_t(IDX::VX) * std::cos(X_t(IDX::SLIP)); + twist_with_cov.twist.linear.y = X_t(IDX::VX) * std::sin(X_t(IDX::SLIP)); twist_with_cov.twist.angular.z = X_t(IDX::VX) / lr_ * std::sin(X_t(IDX::SLIP)); // yaw_rate = vx_k / l_r * sin(slip_k) // twist covariance diff --git a/perception/multi_object_tracker/src/tracker/model/pass_through_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/pass_through_tracker.cpp index 00822a2776ec8..2084ac28e70f0 100644 --- a/perception/multi_object_tracker/src/tracker/model/pass_through_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/pass_through_tracker.cpp @@ -34,7 +34,6 @@ #include #include -#include PassThroughTracker::PassThroughTracker( const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object, diff --git a/perception/multi_object_tracker/src/tracker/model/pedestrian_and_bicycle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/pedestrian_and_bicycle_tracker.cpp index 87036c9b7665a..eed9d05359b77 100644 --- a/perception/multi_object_tracker/src/tracker/model/pedestrian_and_bicycle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/pedestrian_and_bicycle_tracker.cpp @@ -18,8 +18,6 @@ #include "multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp" -#include - using Label = autoware_auto_perception_msgs::msg::ObjectClassification; PedestrianAndBicycleTracker::PedestrianAndBicycleTracker( diff --git a/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp index d2f144a25f103..b168717042db3 100644 --- a/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp @@ -21,7 +21,9 @@ #include "multi_object_tracker/utils/utils.hpp" #include "object_recognition_utils/object_recognition_utils.hpp" -#include +#include +#include +#include #include #include diff --git a/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp index 474191f8926ec..897b858a6aabe 100644 --- a/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp @@ -30,7 +30,9 @@ #include #include -#include +#include +#include +#include UnknownTracker::UnknownTracker( const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object, diff --git a/perception/object_merger/include/object_association_merger/node.hpp b/perception/object_merger/include/object_association_merger/node.hpp index 1e5b9fad9c9ca..6815b59894083 100644 --- a/perception/object_merger/include/object_association_merger/node.hpp +++ b/perception/object_merger/include/object_association_merger/node.hpp @@ -59,14 +59,16 @@ class ObjectAssociationMergerNode : public rclcpp::Node tf2_ros::TransformListener tf_listener_; rclcpp::Publisher::SharedPtr merged_object_pub_; - message_filters::Subscriber object0_sub_; - message_filters::Subscriber object1_sub_; - typedef message_filters::sync_policies::ApproximateTime< + message_filters::Subscriber object0_sub_{}; + message_filters::Subscriber object1_sub_{}; + + using SyncPolicy = message_filters::sync_policies::ApproximateTime< autoware_auto_perception_msgs::msg::DetectedObjects, - autoware_auto_perception_msgs::msg::DetectedObjects> - SyncPolicy; - typedef message_filters::Synchronizer Sync; - Sync sync_; + autoware_auto_perception_msgs::msg::DetectedObjects>; + using Sync = message_filters::Synchronizer; + typename std::shared_ptr sync_ptr_; + + int sync_queue_size_; std::unique_ptr data_association_; std::string base_link_frame_id_; // associated with the base_link frame diff --git a/perception/object_merger/include/object_association_merger/utils/utils.hpp b/perception/object_merger/include/object_association_merger/utils/utils.hpp index dce5a48c5d2ce..bb4466bd4944d 100644 --- a/perception/object_merger/include/object_association_merger/utils/utils.hpp +++ b/perception/object_merger/include/object_association_merger/utils/utils.hpp @@ -19,8 +19,6 @@ #ifndef OBJECT_ASSOCIATION_MERGER__UTILS__UTILS_HPP_ #define OBJECT_ASSOCIATION_MERGER__UTILS__UTILS_HPP_ -#include - #include #include #include diff --git a/perception/object_merger/launch/object_association_merger.launch.xml b/perception/object_merger/launch/object_association_merger.launch.xml index 3418f7d5a5e61..5754c256fef3c 100644 --- a/perception/object_merger/launch/object_association_merger.launch.xml +++ b/perception/object_merger/launch/object_association_merger.launch.xml @@ -6,6 +6,7 @@ + @@ -14,6 +15,7 @@ + diff --git a/perception/object_merger/src/object_association_merger/node.cpp b/perception/object_merger/src/object_association_merger/node.cpp index 4f600ce8a4948..f776d2d940045 100644 --- a/perception/object_merger/src/object_association_merger/node.cpp +++ b/perception/object_merger/src/object_association_merger/node.cpp @@ -16,7 +16,7 @@ #include "object_association_merger/utils/utils.hpp" #include "object_recognition_utils/object_recognition_utils.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" #include @@ -77,20 +77,13 @@ ObjectAssociationMergerNode::ObjectAssociationMergerNode(const rclcpp::NodeOptio tf_buffer_(get_clock()), tf_listener_(tf_buffer_), object0_sub_(this, "input/object0", rclcpp::QoS{1}.get_rmw_qos_profile()), - object1_sub_(this, "input/object1", rclcpp::QoS{1}.get_rmw_qos_profile()), - sync_(SyncPolicy(10), object0_sub_, object1_sub_) + object1_sub_(this, "input/object1", rclcpp::QoS{1}.get_rmw_qos_profile()) { - // Create publishers and subscribers - using std::placeholders::_1; - using std::placeholders::_2; - sync_.registerCallback(std::bind(&ObjectAssociationMergerNode::objectsCallback, this, _1, _2)); - merged_object_pub_ = create_publisher( - "output/object", rclcpp::QoS{1}); - // Parameters base_link_frame_id_ = declare_parameter("base_link_frame_id", "base_link"); priority_mode_ = static_cast( declare_parameter("priority_mode", static_cast(PriorityMode::Confidence))); + sync_queue_size_ = declare_parameter("sync_queue_size", 20); remove_overlapped_unknown_objects_ = declare_parameter("remove_overlapped_unknown_objects", true); overlapped_judge_param_.precision_threshold = @@ -115,6 +108,16 @@ ObjectAssociationMergerNode::ObjectAssociationMergerNode(const rclcpp::NodeOptio const auto min_iou_matrix = this->declare_parameter>("min_iou_matrix"); data_association_ = std::make_unique( can_assign_matrix, max_dist_matrix, max_rad_matrix, min_iou_matrix); + + // Create publishers and subscribers + using std::placeholders::_1; + using std::placeholders::_2; + sync_ptr_ = std::make_shared(SyncPolicy(sync_queue_size_), object0_sub_, object1_sub_); + sync_ptr_->registerCallback( + std::bind(&ObjectAssociationMergerNode::objectsCallback, this, _1, _2)); + + merged_object_pub_ = create_publisher( + "output/object", rclcpp::QoS{1}); } void ObjectAssociationMergerNode::objectsCallback( diff --git a/perception/object_velocity_splitter/src/object_velocity_splitter_node/object_velocity_splitter_node.cpp b/perception/object_velocity_splitter/src/object_velocity_splitter_node/object_velocity_splitter_node.cpp index 775f612b8caeb..a5d54b63b3311 100644 --- a/perception/object_velocity_splitter/src/object_velocity_splitter_node/object_velocity_splitter_node.cpp +++ b/perception/object_velocity_splitter/src/object_velocity_splitter_node/object_velocity_splitter_node.cpp @@ -14,7 +14,7 @@ #include "object_velocity_splitter/object_velocity_splitter_node.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" #include #include diff --git a/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp b/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp index 72136c20fddb8..691c13a6d4701 100644 --- a/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp +++ b/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp @@ -15,7 +15,9 @@ #include "occupancy_grid_map_outlier_filter/occupancy_grid_map_outlier_filter_nodelet.hpp" #include -#include +#include +#include +#include #include diff --git a/perception/probabilistic_occupancy_grid_map/include/utils/utils.hpp b/perception/probabilistic_occupancy_grid_map/include/utils/utils.hpp index 4af7ef950272d..047b747c2861f 100644 --- a/perception/probabilistic_occupancy_grid_map/include/utils/utils.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/utils/utils.hpp @@ -18,7 +18,6 @@ #include #include #include -#include #include #include diff --git a/perception/probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.cpp b/perception/probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.cpp index e6f8ca17757c2..b4505eedddd21 100644 --- a/perception/probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.cpp @@ -18,7 +18,6 @@ #include "utils/utils.hpp" #include -#include #include diff --git a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.cpp b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.cpp index 4e71c4bda3f21..8652cfa34d96c 100644 --- a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.cpp @@ -56,7 +56,6 @@ #include #include -#include #include diff --git a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp index b07076de8342a..56aeea30e0773 100644 --- a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp @@ -19,7 +19,7 @@ #include #include -#include +#include #include diff --git a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.cpp b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.cpp index 9556b0a93cc93..20a5770e37fdb 100644 --- a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.cpp @@ -20,7 +20,7 @@ #include #include #include -#include +#include #include diff --git a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp index e8b53b91d81f7..f6369602b8890 100644 --- a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp @@ -20,7 +20,8 @@ #include "utils/utils.hpp" #include -#include +#include +#include #include diff --git a/perception/probabilistic_occupancy_grid_map/src/utils/utils.cpp b/perception/probabilistic_occupancy_grid_map/src/utils/utils.cpp index f2cd6203b3685..0060754cd875c 100644 --- a/perception/probabilistic_occupancy_grid_map/src/utils/utils.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/utils/utils.cpp @@ -14,6 +14,8 @@ #include "utils/utils.hpp" +#include + #include namespace utils diff --git a/perception/probabilistic_occupancy_grid_map/test/test_utils.cpp b/perception/probabilistic_occupancy_grid_map/test/test_utils.cpp index 3b1edca14e146..367dcee807e7b 100644 --- a/perception/probabilistic_occupancy_grid_map/test/test_utils.cpp +++ b/perception/probabilistic_occupancy_grid_map/test/test_utils.cpp @@ -24,8 +24,6 @@ // autoware #include "utils/utils.hpp" -#include - // create pointcloud function pcl::PointCloud createPCLPointCloudWithIteratedHeight(const size_t width) { diff --git a/perception/radar_crossing_objects_noise_filter/src/radar_crossing_objects_noise_filter_node/radar_crossing_objects_noise_filter_node.cpp b/perception/radar_crossing_objects_noise_filter/src/radar_crossing_objects_noise_filter_node/radar_crossing_objects_noise_filter_node.cpp index c530ee0b2b851..947856e5d2a4d 100644 --- a/perception/radar_crossing_objects_noise_filter/src/radar_crossing_objects_noise_filter_node/radar_crossing_objects_noise_filter_node.cpp +++ b/perception/radar_crossing_objects_noise_filter/src/radar_crossing_objects_noise_filter_node/radar_crossing_objects_noise_filter_node.cpp @@ -14,7 +14,8 @@ #include "radar_crossing_objects_noise_filter/radar_crossing_objects_noise_filter_node.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" +#include "tier4_autoware_utils/math/normalization.hpp" #include diff --git a/perception/radar_fusion_to_detected_object/README.md b/perception/radar_fusion_to_detected_object/README.md index b22938cbf0e5a..defc716a744d9 100644 --- a/perception/radar_fusion_to_detected_object/README.md +++ b/perception/radar_fusion_to_detected_object/README.md @@ -13,11 +13,11 @@ The document of core algorithm is [here](docs/algorithm.md) ### Parameters for sensor fusion -| Name | Type | Description | Default value | -| :----------------------- | :----- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | -| bounding_box_margin | double | The distance to extend the 2D bird's-eye view Bounding Box on each side. This distance is used as a threshold to find radar centroids falling inside the extended box. [m] | 2.0 | -| split_threshold_velocity | double | The object's velocity threshold to decide to split for two objects from radar information (currently not implemented) [m/s] | 5.0 | -| threshold_yaw_diff | double | The yaw orientation threshold. If $ \vert \theta _{ob} - \theta_ {ra} \vert < threshold*yaw_diff $ attached to radar information include estimated velocity, where $ \theta*{ob} $ is yaw angle from 3d detected object, $ \theta\_ {ra} $ is yaw angle from radar object. [rad] | 0.35 | +| Name | Type | Description | Default value | +| :----------------------- | :----- | :----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | +| bounding_box_margin | double | The distance to extend the 2D bird's-eye view Bounding Box on each side. This distance is used as a threshold to find radar centroids falling inside the extended box. [m] | 2.0 | +| split_threshold_velocity | double | The object's velocity threshold to decide to split for two objects from radar information (currently not implemented) [m/s] | 5.0 | +| threshold_yaw_diff | double | The yaw orientation threshold. If ∣ θ_ob − θ_ra ∣ < threshold × yaw_diff attached to radar information include estimated velocity, where*θob*is yaw angle from 3d detected object,\*θ_ra is yaw angle from radar object. [rad] | 0.35 | ### Weight parameters for velocity estimation diff --git a/perception/radar_fusion_to_detected_object/include/radar_fusion_to_detected_object.hpp b/perception/radar_fusion_to_detected_object/include/radar_fusion_to_detected_object.hpp index e51024ba4e205..b5f2005a84baf 100644 --- a/perception/radar_fusion_to_detected_object/include/radar_fusion_to_detected_object.hpp +++ b/perception/radar_fusion_to_detected_object/include/radar_fusion_to_detected_object.hpp @@ -16,8 +16,7 @@ #define RADAR_FUSION_TO_DETECTED_OBJECT_HPP_ #include "rclcpp/logger.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" - +#include "tier4_autoware_utils/geometry/boost_geometry.hpp" #define EIGEN_MPL2_ONLY #include #include diff --git a/perception/radar_fusion_to_detected_object/schema/radar_fusion_to_detected_object.schema.json b/perception/radar_fusion_to_detected_object/schema/radar_fusion_to_detected_object.schema.json new file mode 100644 index 0000000000000..688414df56c1e --- /dev/null +++ b/perception/radar_fusion_to_detected_object/schema/radar_fusion_to_detected_object.schema.json @@ -0,0 +1,114 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Radar Fusion to Detected Object Node", + "type": "object", + "definitions": { + "radar_fusion_to_detected_object": { + "type": "object", + "properties": { + "node_params": { + "type": "object", + "properties": { + "update_rate_hz": { + "type": "number", + "description": "Update rate for parameters. [hz]", + "default": "10.0", + "minimum": 0.0 + } + }, + "required": ["update_rate_hz"] + }, + "core_params": { + "type": "object", + "properties": { + "bounding_box_margin": { + "type": "number", + "description": "The distance to extend the 2D bird's-eye view Bounding Box on each side. This distance is used as a threshold to find radar centroids falling inside the extended box. [m]", + "default": "2.0", + "minimum": 0.0 + }, + "split_threshold_velocity": { + "type": "number", + "description": "The object's velocity threshold to decide to split for two objects from radar information (currently not implemented) [m/s]", + "default": "5.0", + "minimum": 0.0 + }, + "threshold_yaw_diff": { + "type": "number", + "description": "The yaw orientation threshold. If ∣θob−θra∣ +#include + #include #include @@ -40,7 +43,7 @@ void RadarFusionToDetectedObject::setParam(const Param & param) // Radar fusion param param_.bounding_box_margin = param.bounding_box_margin; param_.split_threshold_velocity = param.split_threshold_velocity; - param_.threshold_yaw_diff = param.split_threshold_velocity; + param_.threshold_yaw_diff = param.threshold_yaw_diff; // Normalize weight param double sum_weight = param.velocity_weight_median + param.velocity_weight_min_distance + @@ -293,7 +296,7 @@ TwistWithCovariance RadarFusionToDetectedObject::estimateTwist( bool RadarFusionToDetectedObject::isQualified( const DetectedObject & object, std::shared_ptr> & radars) { - if (object.classification[0].probability > param_.threshold_probability) { + if (object.existence_probability > param_.threshold_probability) { return true; } else { if (!radars || !(*radars).empty()) { diff --git a/perception/radar_fusion_to_detected_object/src/radar_object_fusion_to_detected_object_node/radar_object_fusion_to_detected_object_node.cpp b/perception/radar_fusion_to_detected_object/src/radar_object_fusion_to_detected_object_node/radar_object_fusion_to_detected_object_node.cpp index 8e909e4fc08c8..b233bdf31adc6 100644 --- a/perception/radar_fusion_to_detected_object/src/radar_object_fusion_to_detected_object_node/radar_object_fusion_to_detected_object_node.cpp +++ b/perception/radar_fusion_to_detected_object/src/radar_object_fusion_to_detected_object_node/radar_object_fusion_to_detected_object_node.cpp @@ -61,31 +61,29 @@ RadarObjectFusionToDetectedObjectNode::RadarObjectFusionToDetectedObjectNode( std::bind(&RadarObjectFusionToDetectedObjectNode::onSetParam, this, _1)); // Node Parameter - node_param_.update_rate_hz = declare_parameter("node_params.update_rate_hz", 10.0); + node_param_.update_rate_hz = declare_parameter("node_params.update_rate_hz"); // Core Parameter - core_param_.bounding_box_margin = - declare_parameter("core_params.bounding_box_margin", 0.5); + core_param_.bounding_box_margin = declare_parameter("core_params.bounding_box_margin"); core_param_.split_threshold_velocity = - declare_parameter("core_params.split_threshold_velocity", 0.0); - core_param_.threshold_yaw_diff = - declare_parameter("core_params.threshold_yaw_diff", 0.35); + declare_parameter("core_params.split_threshold_velocity"); + core_param_.threshold_yaw_diff = declare_parameter("core_params.threshold_yaw_diff"); core_param_.velocity_weight_min_distance = - declare_parameter("core_params.velocity_weight_min_distance", 1.0); + declare_parameter("core_params.velocity_weight_min_distance"); core_param_.velocity_weight_average = - declare_parameter("core_params.velocity_weight_average", 0.0); + declare_parameter("core_params.velocity_weight_average"); core_param_.velocity_weight_median = - declare_parameter("core_params.velocity_weight_median", 0.0); + declare_parameter("core_params.velocity_weight_median"); core_param_.velocity_weight_target_value_average = - declare_parameter("core_params.velocity_weight_target_value_average", 0.0); + declare_parameter("core_params.velocity_weight_target_value_average"); core_param_.velocity_weight_target_value_top = - declare_parameter("core_params.velocity_weight_target_value_top", 1.0); + declare_parameter("core_params.velocity_weight_target_value_top"); core_param_.convert_doppler_to_twist = - declare_parameter("core_params.convert_doppler_to_twist", false); + declare_parameter("core_params.convert_doppler_to_twist"); core_param_.threshold_probability = - declare_parameter("core_params.threshold_probability", 0.0); + declare_parameter("core_params.threshold_probability"); core_param_.compensate_probability = - declare_parameter("core_params.compensate_probability", false); + declare_parameter("core_params.compensate_probability"); // Core radar_fusion_to_detected_object_ = std::make_unique(get_logger()); @@ -97,7 +95,7 @@ RadarObjectFusionToDetectedObjectNode::RadarObjectFusionToDetectedObjectNode( using std::placeholders::_1; using std::placeholders::_2; - sync_ptr_ = std::make_shared(SyncPolicy(10), sub_object_, sub_radar_); + sync_ptr_ = std::make_shared(SyncPolicy(20), sub_object_, sub_radar_); sync_ptr_->registerCallback( std::bind(&RadarObjectFusionToDetectedObjectNode::onData, this, _1, _2)); diff --git a/perception/radar_object_clustering/include/radar_object_clustering/radar_object_clustering_node.hpp b/perception/radar_object_clustering/include/radar_object_clustering/radar_object_clustering_node.hpp index 0e82c5388d297..c762996f8e0a7 100644 --- a/perception/radar_object_clustering/include/radar_object_clustering/radar_object_clustering_node.hpp +++ b/perception/radar_object_clustering/include/radar_object_clustering/radar_object_clustering_node.hpp @@ -16,7 +16,6 @@ #define RADAR_OBJECT_CLUSTERING__RADAR_OBJECT_CLUSTERING_NODE_HPP_ #include "rclcpp/rclcpp.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" #include "autoware_auto_perception_msgs/msg/detected_objects.hpp" diff --git a/perception/radar_object_clustering/src/radar_object_clustering_node/radar_object_clustering_node.cpp b/perception/radar_object_clustering/src/radar_object_clustering_node/radar_object_clustering_node.cpp index 7f9879fc357a5..74e85bde21385 100644 --- a/perception/radar_object_clustering/src/radar_object_clustering_node/radar_object_clustering_node.cpp +++ b/perception/radar_object_clustering/src/radar_object_clustering_node/radar_object_clustering_node.cpp @@ -15,6 +15,8 @@ #include "radar_object_clustering/radar_object_clustering_node.hpp" #include "object_recognition_utils/object_recognition_utils.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" +#include "tier4_autoware_utils/math/unit_conversion.hpp" #include diff --git a/perception/radar_object_tracker/README.md b/perception/radar_object_tracker/README.md index 9f11d50ce2b3f..739a4a745ff6c 100644 --- a/perception/radar_object_tracker/README.md +++ b/perception/radar_object_tracker/README.md @@ -26,9 +26,10 @@ See more details in the [models.md](models.md). ### Input -| Name | Type | Description | -| --------- | ----------------------------------------------------- | ---------------- | -| `~/input` | `autoware_auto_perception_msgs::msg::DetectedObjects` | Detected objects | +| Name | Type | Description | +| ------------- | ----------------------------------------------------- | ---------------- | +| `~/input` | `autoware_auto_perception_msgs::msg::DetectedObjects` | Detected objects | +| `/vector/map` | `autoware_auto_msgs::msg::HADMapBin` | Map data | ### Output @@ -40,20 +41,29 @@ See more details in the [models.md](models.md). ### Node Parameters -| Name | Type | Default Value | Description | -| --------------------------- | ------ | ----------------------------- | ----------------------------------------------------------- | -| `publish_rate` | double | 30.0 | The rate at which to publish the output messages | -| `world_frame_id` | string | "world" | The frame ID of the world coordinate system | -| `enable_delay_compensation` | bool | false | Whether to enable delay compensation | -| `tracking_config_directory` | string | "" | The directory containing the tracking configuration files | -| `enable_logging` | bool | false | Whether to enable logging | -| `logging_file_path` | string | "~/.ros/association_log.json" | The path to the file where logs should be written | -| `can_assign_matrix` | array | | An array of integers used in the data association algorithm | -| `max_dist_matrix` | array | | An array of doubles used in the data association algorithm | -| `max_area_matrix` | array | | An array of doubles used in the data association algorithm | -| `min_area_matrix` | array | | An array of doubles used in the data association algorithm | -| `max_rad_matrix` | array | | An array of doubles used in the data association algorithm | -| `min_iou_matrix` | array | | An array of doubles used in the data association algorithm | +| Name | Type | Default Value | Description | +| ------------------------------------ | ------ | --------------------------- | --------------------------------------------------------------------------------------------------------------- | +| `publish_rate` | double | 10.0 | The rate at which to publish the output messages | +| `world_frame_id` | string | "map" | The frame ID of the world coordinate system | +| `enable_delay_compensation` | bool | false | Whether to enable delay compensation. If set to `true`, output topic is published by timer with `publish_rate`. | +| `tracking_config_directory` | string | "" | The directory containing the tracking configuration files | +| `enable_logging` | bool | false | Whether to enable logging | +| `logging_file_path` | string | "/tmp/association_log.json" | The path to the file where logs should be written | +| `tracker_lifetime` | double | 1.0 | The lifetime of the tracker in seconds | +| `use_distance_based_noise_filtering` | bool | true | Whether to use distance based filtering | +| `minimum_range_threshold` | double | 70.0 | Minimum distance threshold for filtering in meters | +| `use_map_based_noise_filtering` | bool | true | Whether to use map based filtering | +| `max_distance_from_lane` | double | 5.0 | Maximum distance from lane for filtering in meters | +| `max_angle_diff_from_lane` | double | 0.785398 | Maximum angle difference from lane for filtering in radians | +| `max_lateral_velocity` | double | 5.0 | Maximum lateral velocity for filtering in m/s | +| `can_assign_matrix` | array | | An array of integers used in the data association algorithm | +| `max_dist_matrix` | array | | An array of doubles used in the data association algorithm | +| `max_area_matrix` | array | | An array of doubles used in the data association algorithm | +| `min_area_matrix` | array | | An array of doubles used in the data association algorithm | +| `max_rad_matrix` | array | | An array of doubles used in the data association algorithm | +| `min_iou_matrix` | array | | An array of doubles used in the data association algorithm | + +See more details in the [models.md](models.md). ## Assumptions / Known limits diff --git a/perception/radar_object_tracker/config/radar_object_tracker.param.yaml b/perception/radar_object_tracker/config/radar_object_tracker.param.yaml new file mode 100644 index 0000000000000..f80adffb41090 --- /dev/null +++ b/perception/radar_object_tracker/config/radar_object_tracker.param.yaml @@ -0,0 +1,27 @@ +# general parameters for radar_object_tracker node +/**: + ros__parameters: + # basic settings + world_frame_id: "map" + tracker_lifetime: 1.0 # [sec] + # if empty, use default config declared in this package + tracking_config_directory: "" + + # delay compensate parameters + publish_rate: 10.0 + enable_delay_compensation: false + + # logging + enable_logging: false + logging_file_path: "/tmp/association_log.json" + + # filtering + ## 1. distance based filtering: remove closer objects than this threshold + use_distance_based_noise_filtering: true + minimum_range_threshold: 70.0 # [m] + + ## 2. lanelet map based filtering + use_map_based_noise_filtering: true + max_distance_from_lane: 5.0 # [m] + max_angle_diff_from_lane: 0.785398 # [rad] (45 deg) + max_lateral_velocity: 5.0 # [m/s] diff --git a/perception/radar_object_tracker/image/noise_filtering.drawio.svg b/perception/radar_object_tracker/image/noise_filtering.drawio.svg new file mode 100644 index 0000000000000..cd044cdfa1379 --- /dev/null +++ b/perception/radar_object_tracker/image/noise_filtering.drawio.svg @@ -0,0 +1,165 @@ + + + + + + + + + + + +
+
+
self vehicle
+
+
+ + self vehicle + + + + + + + +
+
+
removed
+
+
+ + removed + + + + + + + +
+
+
distance threshold
+
+
+ + distance threshold + + + + + + + +
+
+
+ filtered object +
+
+
+ + filtered object + + + + + + + + + + + +
+
+
removed
+
+
+ + removed + + + + + + + + + +
+
+
+ lane distance +
+ threshold +
+
+
+ + lane distance... + + + + + + + + + + + +
+
+
+ removed +
+ with +
+ angle threshold +
+
+
+ + removed... + + + + + + + + + +
+
+
+ removed by +
+ high +
+ lateral velocity +
+
+
+ + removed by... + + + + + + + Text is not SVG - cannot display + + + diff --git a/perception/radar_object_tracker/include/radar_object_tracker/radar_object_tracker_node/radar_object_tracker_node.hpp b/perception/radar_object_tracker/include/radar_object_tracker/radar_object_tracker_node/radar_object_tracker_node.hpp index 898a7855aabae..415ff24b34cc3 100644 --- a/perception/radar_object_tracker/include/radar_object_tracker/radar_object_tracker_node/radar_object_tracker_node.hpp +++ b/perception/radar_object_tracker/include/radar_object_tracker/radar_object_tracker_node/radar_object_tracker_node.hpp @@ -33,6 +33,16 @@ #include #endif +#include +#include +#include + +#include +#include +#include +#include +#include +#include #include #include @@ -41,8 +51,11 @@ #include #include +using autoware_auto_mapping_msgs::msg::HADMapBin; using autoware_auto_perception_msgs::msg::DetectedObject; using autoware_auto_perception_msgs::msg::DetectedObjects; +using autoware_auto_perception_msgs::msg::TrackedObject; +using autoware_auto_perception_msgs::msg::TrackedObjects; class RadarObjectTrackerNode : public rclcpp::Node { @@ -50,20 +63,23 @@ class RadarObjectTrackerNode : public rclcpp::Node explicit RadarObjectTrackerNode(const rclcpp::NodeOptions & node_options); private: + // pub-sub rclcpp::Publisher::SharedPtr tracked_objects_pub_; rclcpp::Subscription::SharedPtr detected_object_sub_; - rclcpp::TimerBase::SharedPtr publish_timer_; // publish timer + rclcpp::TimerBase::SharedPtr publish_timer_; // publish timer + rclcpp::Subscription::SharedPtr sub_map_; // map subscriber tf2_ros::Buffer tf_buffer_; tf2_ros::TransformListener tf_listener_; - + float tracker_lifetime_; std::map tracker_map_; void onMeasurement( const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr input_objects_msg); void onTimer(); + void onMap(const HADMapBin::ConstSharedPtr map_msg); std::string world_frame_id_; // tracking frame std::string tracker_config_directory_; @@ -77,11 +93,36 @@ class RadarObjectTrackerNode : public rclcpp::Node std::string path; } logging_; + // noise reduction + bool use_distance_based_noise_filtering_; + bool use_map_based_noise_filtering_; + + // distance based noise reduction + double minimum_range_threshold_; + std::string sensor_frame_ = "base_link"; + + // map based noise reduction + bool map_is_loaded_ = false; + double max_distance_from_lane_; + double max_lateral_velocity_; + double max_angle_diff_from_lane_; + // Lanelet Map Pointers + std::shared_ptr lanelet_map_ptr_; + std::shared_ptr routing_graph_ptr_; + std::shared_ptr traffic_rules_ptr_; + // Crosswalk Entry Points + // lanelet::ConstLanelets crosswalks_; + void checkTrackerLifeCycle( std::list> & list_tracker, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform); void sanitizeTracker( std::list> & list_tracker, const rclcpp::Time & time); + void mapBasedNoiseFilter( + std::list> & list_tracker, const rclcpp::Time & time); + void distanceBasedNoiseFilter( + std::list> & list_tracker, const rclcpp::Time & time, + const geometry_msgs::msg::Transform & self_transform); std::shared_ptr createNewTracker( const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform) const; diff --git a/perception/radar_object_tracker/include/radar_object_tracker/utils/utils.hpp b/perception/radar_object_tracker/include/radar_object_tracker/utils/utils.hpp index 3fe9172ab5ceb..c43792908aedf 100644 --- a/perception/radar_object_tracker/include/radar_object_tracker/utils/utils.hpp +++ b/perception/radar_object_tracker/include/radar_object_tracker/utils/utils.hpp @@ -22,7 +22,6 @@ #include #include #include -#include #include #include diff --git a/perception/radar_object_tracker/launch/radar_object_tracker.launch.xml b/perception/radar_object_tracker/launch/radar_object_tracker.launch.xml index 7ef3e58a64161..e2d30c1b69d19 100644 --- a/perception/radar_object_tracker/launch/radar_object_tracker.launch.xml +++ b/perception/radar_object_tracker/launch/radar_object_tracker.launch.xml @@ -7,16 +7,18 @@ + + + + + + - - - - diff --git a/perception/radar_object_tracker/models.md b/perception/radar_object_tracker/models.md index a4c53db156f0f..f4e4c6a02edaf 100644 --- a/perception/radar_object_tracker/models.md +++ b/perception/radar_object_tracker/models.md @@ -74,3 +74,33 @@ v_{k+1} &= v_k \\ \omega_{k+1} &= \omega_k \end{align} $$ + +## Noise filtering + +Radar sensors often have noisy measurement. So we use the following filter to reduce the false positive objects. + +The figure below shows the current noise filtering process. + +![noise_filter](image/noise_filtering.drawio.svg) + +### minimum range filter + +In most cases, Radar sensors are used with other sensors such as LiDAR and Camera, and Radar sensors are used to detect objects far away. So we can filter out objects that are too close to the sensor. + +`use_distance_based_noise_filtering` parameter is used to enable/disable this filter, and `minimum_range_threshold` parameter is used to set the threshold. + +### lanelet based filter + +With lanelet map information, We can filter out false positive objects that are not likely important obstacles. + +We filter out objects that satisfy the following conditions: + +- too large lateral distance from lane +- velocity direction is too different from lane direction +- too large lateral velocity + +Each condition can be set by the following parameters: + +- `max_distance_from_lane` +- `max_angle_diff_from_lane` +- `max_lateral_velocity` diff --git a/perception/radar_object_tracker/package.xml b/perception/radar_object_tracker/package.xml index 46fb0826e30e1..ad71e613c1a18 100644 --- a/perception/radar_object_tracker/package.xml +++ b/perception/radar_object_tracker/package.xml @@ -16,7 +16,9 @@ autoware_auto_perception_msgs eigen kalman_filter + lanelet2_extension mussp + nlohmann-json-dev object_recognition_utils rclcpp rclcpp_components diff --git a/perception/radar_object_tracker/src/radar_object_tracker_node/radar_object_tracker_node.cpp b/perception/radar_object_tracker/src/radar_object_tracker_node/radar_object_tracker_node.cpp index f32084493f34e..6b02836e1f933 100644 --- a/perception/radar_object_tracker/src/radar_object_tracker_node/radar_object_tracker_node.cpp +++ b/perception/radar_object_tracker/src/radar_object_tracker_node/radar_object_tracker_node.cpp @@ -60,6 +60,131 @@ boost::optional getTransformAnonymous( } } +// check if lanelet is close enough to the target lanelet +bool isDuplicated( + const std::pair & target_lanelet, + const lanelet::ConstLanelets & lanelets) +{ + const double CLOSE_LANELET_THRESHOLD = 0.1; + for (const auto & lanelet : lanelets) { + const auto target_lanelet_end_p = target_lanelet.second.centerline2d().back(); + const auto lanelet_end_p = lanelet.centerline2d().back(); + const double dist = std::hypot( + target_lanelet_end_p.x() - lanelet_end_p.x(), target_lanelet_end_p.y() - lanelet_end_p.y()); + if (dist < CLOSE_LANELET_THRESHOLD) { + return true; + } + } + + return false; +} + +// check if the lanelet is valid for object tracking +bool checkCloseLaneletCondition( + const std::pair & lanelet, const TrackedObject & object, + const double max_distance_from_lane, const double max_angle_diff_from_lane) +{ + // Step1. If we only have one point in the centerline, we will ignore the lanelet + if (lanelet.second.centerline().size() <= 1) { + return false; + } + + // Step2. Check if the obstacle is inside or close enough to the lanelet + lanelet::BasicPoint2d search_point( + object.kinematics.pose_with_covariance.pose.position.x, + object.kinematics.pose_with_covariance.pose.position.y); + if (!lanelet::geometry::inside(lanelet.second, search_point)) { + const auto distance = lanelet.first; + if (distance > max_distance_from_lane) { + return false; + } + } + + // Step3. Calculate the angle difference between the lane angle and obstacle angle + const double object_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); + const double lane_yaw = lanelet::utils::getLaneletAngle( + lanelet.second, object.kinematics.pose_with_covariance.pose.position); + const double delta_yaw = object_yaw - lane_yaw; + const double normalized_delta_yaw = tier4_autoware_utils::normalizeRadian(delta_yaw); + const double abs_norm_delta = std::fabs(normalized_delta_yaw); + + // Step4. Check if the closest lanelet is valid, and add all + // of the lanelets that are below max_dist and max_delta_yaw + const double object_vel = object.kinematics.twist_with_covariance.twist.linear.x; + const bool is_yaw_reversed = M_PI - max_angle_diff_from_lane < abs_norm_delta && object_vel < 0.0; + if (is_yaw_reversed || abs_norm_delta < max_angle_diff_from_lane) { + return true; + } + + return false; +} + +lanelet::ConstLanelets getClosestValidLanelets( + const TrackedObject & object, const lanelet::LaneletMapPtr & lanelet_map_ptr, + const double max_distance_from_lane, const double max_angle_diff_from_lane) +{ + // obstacle point + lanelet::BasicPoint2d search_point( + object.kinematics.pose_with_covariance.pose.position.x, + object.kinematics.pose_with_covariance.pose.position.y); + + // nearest lanelet + std::vector> surrounding_lanelets = + lanelet::geometry::findNearest(lanelet_map_ptr->laneletLayer, search_point, 10); + + // No Closest Lanelets + if (surrounding_lanelets.empty()) { + return {}; + } + + lanelet::ConstLanelets closest_lanelets; + for (const auto & lanelet : surrounding_lanelets) { + // Check if the close lanelets meet the necessary condition for start lanelets and + // Check if similar lanelet is inside the closest lanelet + if ( + !checkCloseLaneletCondition( + lanelet, object, max_distance_from_lane, max_angle_diff_from_lane) || + isDuplicated(lanelet, closest_lanelets)) { + continue; + } + + closest_lanelets.push_back(lanelet.second); + } + + return closest_lanelets; +} + +bool hasValidVelocityDirectionToLanelet( + const TrackedObject & object, const lanelet::ConstLanelets & lanelets, + const double max_lateral_velocity) +{ + // get object velocity direction + const double object_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); + const double object_vel_x = object.kinematics.twist_with_covariance.twist.linear.x; + const double object_vel_y = object.kinematics.twist_with_covariance.twist.linear.y; + const double object_vel_yaw = std::atan2(object_vel_y, object_vel_x); // local frame + const double object_vel_yaw_global = + tier4_autoware_utils::normalizeRadian(object_yaw + object_vel_yaw); + const double object_vel = std::hypot(object_vel_x, object_vel_y); + + for (const auto & lanelet : lanelets) { + // get lanelet angle + const double lane_yaw = lanelet::utils::getLaneletAngle( + lanelet, object.kinematics.pose_with_covariance.pose.position); + const double delta_yaw = object_vel_yaw_global - lane_yaw; + const double normalized_delta_yaw = tier4_autoware_utils::normalizeRadian(delta_yaw); + + // get lateral velocity + const double lane_vel = object_vel * std::sin(normalized_delta_yaw); + // std::cout << "lane_vel: " << lane_vel << " , delta yaw:" << normalized_delta_yaw << " , + // object_vel " << object_vel <("output", rclcpp::QoS{1}); + sub_map_ = this->create_subscription( + "/vector_map", rclcpp::QoS{1}.transient_local(), + std::bind(&RadarObjectTrackerNode::onMap, this, std::placeholders::_1)); // Parameters - double publish_rate = declare_parameter("publish_rate", 30.0); - world_frame_id_ = declare_parameter("world_frame_id", "world"); - bool enable_delay_compensation{declare_parameter("enable_delay_compensation", false)}; - tracker_config_directory_ = declare_parameter("tracking_config_directory", ""); - logging_.enable = declare_parameter("enable_logging", false); - logging_.path = - declare_parameter("logging_file_path", "~/.ros/association_log.json"); + tracker_lifetime_ = declare_parameter("tracker_lifetime"); + double publish_rate = declare_parameter("publish_rate"); + world_frame_id_ = declare_parameter("world_frame_id"); + bool enable_delay_compensation{declare_parameter("enable_delay_compensation")}; + tracker_config_directory_ = declare_parameter("tracking_config_directory"); + logging_.enable = declare_parameter("enable_logging"); + logging_.path = declare_parameter("logging_file_path"); + + // noise filter + use_distance_based_noise_filtering_ = + declare_parameter("use_distance_based_noise_filtering"); + use_map_based_noise_filtering_ = declare_parameter("use_map_based_noise_filtering"); + minimum_range_threshold_ = declare_parameter("minimum_range_threshold"); + max_distance_from_lane_ = declare_parameter("max_distance_from_lane"); + max_angle_diff_from_lane_ = declare_parameter("max_angle_diff_from_lane"); + max_lateral_velocity_ = declare_parameter("max_lateral_velocity"); // Load tracking config file if (tracker_config_directory_.empty()) { @@ -132,6 +269,17 @@ RadarObjectTrackerNode::RadarObjectTrackerNode(const rclcpp::NodeOptions & node_ std::make_pair(Label::MOTORCYCLE, this->declare_parameter("motorcycle_tracker"))); } +// load map information to node parameter +void RadarObjectTrackerNode::onMap(const HADMapBin::ConstSharedPtr msg) +{ + RCLCPP_INFO(get_logger(), "[Radar Object Tracker]: Start loading lanelet"); + lanelet_map_ptr_ = std::make_shared(); + lanelet::utils::conversion::fromBinMsg( + *msg, lanelet_map_ptr_, &traffic_rules_ptr_, &routing_graph_ptr_); + RCLCPP_INFO(get_logger(), "[Radar Object Tracker]: Map is loaded"); + map_is_loaded_ = true; +} + void RadarObjectTrackerNode::onMeasurement( const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr input_objects_msg) { @@ -176,6 +324,14 @@ void RadarObjectTrackerNode::onMeasurement( /* life cycle check */ checkTrackerLifeCycle(list_tracker_, measurement_time, *self_transform); + /* map based noise filter */ + if (map_is_loaded_ && use_map_based_noise_filtering_) { + mapBasedNoiseFilter(list_tracker_, measurement_time); + } + /* distance based noise filter */ + if (use_distance_based_noise_filtering_) { + distanceBasedNoiseFilter(list_tracker_, measurement_time, *self_transform); + } /* sanitize trackers */ sanitizeTracker(list_tracker_, measurement_time); @@ -227,6 +383,16 @@ void RadarObjectTrackerNode::onTimer() /* life cycle check */ checkTrackerLifeCycle(list_tracker_, current_time, *self_transform); + + /* map based noise filter */ + if (map_is_loaded_ && use_map_based_noise_filtering_) { + mapBasedNoiseFilter(list_tracker_, current_time); + } + /* distance based noise filter */ + if (use_distance_based_noise_filtering_) { + distanceBasedNoiseFilter(list_tracker_, current_time, *self_transform); + } + /* sanitize trackers */ sanitizeTracker(list_tracker_, current_time); @@ -238,12 +404,9 @@ void RadarObjectTrackerNode::checkTrackerLifeCycle( std::list> & list_tracker, const rclcpp::Time & time, [[maybe_unused]] const geometry_msgs::msg::Transform & self_transform) { - /* params */ - constexpr float max_elapsed_time = 1.0; - /* delete tracker */ for (auto itr = list_tracker.begin(); itr != list_tracker.end(); ++itr) { - const bool is_old = max_elapsed_time < (*itr)->getElapsedTimeFromLastUpdate(time); + const bool is_old = tracker_lifetime_ < (*itr)->getElapsedTimeFromLastUpdate(time); if (is_old) { auto erase_itr = itr; --itr; @@ -252,6 +415,51 @@ void RadarObjectTrackerNode::checkTrackerLifeCycle( } } +// remove objects by lanelet information +void RadarObjectTrackerNode::mapBasedNoiseFilter( + std::list> & list_tracker, const rclcpp::Time & time) +{ + for (auto itr = list_tracker.begin(); itr != list_tracker.end(); ++itr) { + autoware_auto_perception_msgs::msg::TrackedObject object; + (*itr)->getTrackedObject(time, object); + const auto closest_lanelets = getClosestValidLanelets( + object, lanelet_map_ptr_, max_distance_from_lane_, max_angle_diff_from_lane_); + + // 1. If the object is not close to any lanelet, delete the tracker + const bool no_closest_lanelet = closest_lanelets.empty(); + // 2. If the object velocity direction is not close to the lanelet direction, delete the tracker + const bool is_velocity_direction_close_to_lanelet = + hasValidVelocityDirectionToLanelet(object, closest_lanelets, max_lateral_velocity_); + if (no_closest_lanelet || !is_velocity_direction_close_to_lanelet) { + // std::cout << "object removed due to map based noise filter" << " no close lanelet: " << + // no_closest_lanelet << " velocity direction flag:" << is_velocity_direction_close_to_lanelet + // << std::endl; + itr = list_tracker.erase(itr); + --itr; + } + } +} + +// remove objects by distance +void RadarObjectTrackerNode::distanceBasedNoiseFilter( + std::list> & list_tracker, const rclcpp::Time & time, + const geometry_msgs::msg::Transform & self_transform) +{ + // remove objects that are too close + for (auto itr = list_tracker.begin(); itr != list_tracker.end(); ++itr) { + autoware_auto_perception_msgs::msg::TrackedObject object; + (*itr)->getTrackedObject(time, object); + const double distance = std::hypot( + object.kinematics.pose_with_covariance.pose.position.x - self_transform.translation.x, + object.kinematics.pose_with_covariance.pose.position.y - self_transform.translation.y); + if (distance < minimum_range_threshold_) { + // std::cout << "object removed due to small distance. distance: " << distance << std::endl; + itr = list_tracker.erase(itr); + --itr; + } + } +} + void RadarObjectTrackerNode::sanitizeTracker( std::list> & list_tracker, const rclcpp::Time & time) { diff --git a/perception/radar_object_tracker/src/tracker/model/linear_motion_tracker.cpp b/perception/radar_object_tracker/src/tracker/model/linear_motion_tracker.cpp index bfd84159706fb..f6814ceb8c246 100644 --- a/perception/radar_object_tracker/src/tracker/model/linear_motion_tracker.cpp +++ b/perception/radar_object_tracker/src/tracker/model/linear_motion_tracker.cpp @@ -20,7 +20,8 @@ #include "radar_object_tracker/utils/utils.hpp" -#include +#include +#include #include #include @@ -91,15 +92,15 @@ LinearMotionTracker::LinearMotionTracker( R << cos_yaw, -sin_yaw, sin_yaw, cos_yaw; // covariance matrix in the target vehicle coordinate system - Eigen::Matrix2d P_xy_local, P_vxy_local, P_axy_local; + Eigen::Matrix2d P_xy_local, P_v_xy_local, P_a_xy_local; P_xy_local << ekf_params_.p0_cov_x, 0.0, 0.0, ekf_params_.p0_cov_y; - P_vxy_local << ekf_params_.p0_cov_vx, 0.0, 0.0, ekf_params_.p0_cov_vy; - P_axy_local << ekf_params_.p0_cov_ax, 0.0, 0.0, ekf_params_.p0_cov_ay; + P_v_xy_local << ekf_params_.p0_cov_vx, 0.0, 0.0, ekf_params_.p0_cov_vy; + P_a_xy_local << ekf_params_.p0_cov_ax, 0.0, 0.0, ekf_params_.p0_cov_ay; // Rotated covariance matrix // covariance is rotated by 2D rotation matrix R // P′=R P RT - Eigen::Matrix2d P_xy, P_vxy, P_axy; + Eigen::Matrix2d P_xy, P_v_xy, P_a_xy; // Rotate the covariance matrix according to the vehicle yaw // because p0_cov_x and y are in the vehicle coordinate system. @@ -117,12 +118,12 @@ LinearMotionTracker::LinearMotionTracker( if (object.kinematics.has_twist_covariance) { const auto vx_cov = object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; const auto vy_cov = object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y]; - P_vxy_local << vx_cov, 0.0, 0.0, vy_cov; - P_vxy = R * P_vxy_local * R.transpose(); + P_v_xy_local << vx_cov, 0.0, 0.0, vy_cov; + P_v_xy = R * P_v_xy_local * R.transpose(); } else { - P_vxy = R * P_vxy_local * R.transpose(); + P_v_xy = R * P_v_xy_local * R.transpose(); } - // acceleration covariance often writen in object frame + // acceleration covariance often written in object frame const bool has_acceleration_covariance = false; // currently message does not have acceleration covariance if (has_acceleration_covariance) { @@ -132,18 +133,18 @@ LinearMotionTracker::LinearMotionTracker( // const auto ay_cov = // object.kinematics.acceleration_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y]; // This // is future update - // Eigen::Matrix2d P_axy_local; - // P_axy_local << ax_cov, 0.0, 0.0, ay_cov; - P_axy = R * P_axy_local * R.transpose(); + // Eigen::Matrix2d P_a_xy_local; + // P_a_xy_local << ax_cov, 0.0, 0.0, ay_cov; + P_a_xy = R * P_a_xy_local * R.transpose(); } else { - P_axy = R * P_axy_local * R.transpose(); + P_a_xy = R * P_a_xy_local * R.transpose(); } // put value in P matrix // use block description. This assume x,y,vx,vy,ax,ay in this order P.block<2, 2>(IDX::X, IDX::X) = P_xy; - P.block<2, 2>(IDX::VX, IDX::VX) = P_vxy; - P.block<2, 2>(IDX::AX, IDX::AX) = P_axy; + P.block<2, 2>(IDX::VX, IDX::VX) = P_v_xy; + P.block<2, 2>(IDX::AX, IDX::AX) = P_a_xy; // init shape if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { @@ -292,8 +293,8 @@ bool LinearMotionTracker::predict(const double dt, KalmanFilter & ekf) const // system noise in local coordinate // we assume acceleration random walk model // - // Q_local = [dt^3/6 0 dt^2/2 0 dt 0] ^ T qcov_ax [dt^3/6 0 dt^2/2 0 dt 0] - // + [0 dt^3/6 0 dt^2/2 0 dt] ^ T qcov_ay [0 dt^3/6 0 dt^2/2 0 dt] + // Q_local = [dt^3/6 0 dt^2/2 0 dt 0] ^ T q_cov_ax [dt^3/6 0 dt^2/2 0 dt 0] + // + [0 dt^3/6 0 dt^2/2 0 dt] ^ T q_cov_ay [0 dt^3/6 0 dt^2/2 0 dt] // Eigen::MatrixXd qx = Eigen::MatrixXd::Zero(ekf_params_.dim_x, 1); // Eigen::MatrixXd qy = Eigen::MatrixXd::Zero(ekf_params_.dim_x, 1); // qx << dt * dt * dt / 6, 0, dt * dt / 2, 0, dt, 0; @@ -386,10 +387,10 @@ bool LinearMotionTracker::measureWithPose( // 2. add linear velocity measurement const bool enable_velocity_measurement = object.kinematics.has_twist; if (enable_velocity_measurement) { - Eigen::MatrixXd Cvxvy = Eigen::MatrixXd::Zero(2, ekf_params_.dim_x); - Cvxvy(0, IDX::VX) = 1; - Cvxvy(1, IDX::VY) = 1; - C_list.push_back(Cvxvy); + Eigen::MatrixXd C_vx_vy = Eigen::MatrixXd::Zero(2, ekf_params_.dim_x); + C_vx_vy(0, IDX::VX) = 1; + C_vx_vy(1, IDX::VY) = 1; + C_list.push_back(C_vx_vy); // velocity is in the target vehicle coordinate system Eigen::MatrixXd Vxy_local = Eigen::MatrixXd::Zero(2, 1); @@ -399,19 +400,19 @@ bool LinearMotionTracker::measureWithPose( Vxy = RotationYaw * Vxy_local; Y_list.push_back(Vxy); - Eigen::Matrix2d Rvxy_local = Eigen::MatrixXd::Zero(2, 2); + Eigen::Matrix2d R_v_xy_local = Eigen::MatrixXd::Zero(2, 2); if (!object.kinematics.has_twist_covariance) { - Rvxy_local << ekf_params_.r_cov_vx, 0, 0, ekf_params_.r_cov_vy; + R_v_xy_local << ekf_params_.r_cov_vx, 0, 0, ekf_params_.r_cov_vy; } else { - Rvxy_local << object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X], 0, - 0, object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y]; + R_v_xy_local << object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X], + 0, 0, object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y]; } - Eigen::MatrixXd Rvxy = Eigen::MatrixXd::Zero(2, 2); - Rvxy = RotationYaw * Rvxy_local * RotationYaw.transpose(); - R_block_list.push_back(Rvxy); + Eigen::MatrixXd R_v_xy = Eigen::MatrixXd::Zero(2, 2); + R_v_xy = RotationYaw * R_v_xy_local * RotationYaw.transpose(); + R_block_list.push_back(R_v_xy); } - // 3. sumup matrices + // 3. sum up matrices const auto row_number = std::accumulate( C_list.begin(), C_list.end(), 0, [](const auto & sum, const auto & C) { return sum + C.rows(); }); @@ -522,9 +523,9 @@ bool LinearMotionTracker::getTrackedObject( auto & twist_with_cov = object.kinematics.twist_with_covariance; auto & acceleration_with_cov = object.kinematics.acceleration_with_covariance; // rotation matrix with yaw_ - Eigen::Matrix2d Ryaw = Eigen::Matrix2d::Zero(); - Ryaw << std::cos(yaw_), -std::sin(yaw_), std::sin(yaw_), std::cos(yaw_); - const Eigen::Matrix2d Ryaw_inv = Ryaw.transpose(); + Eigen::Matrix2d R_yaw = Eigen::Matrix2d::Zero(); + R_yaw << std::cos(yaw_), -std::sin(yaw_), std::sin(yaw_), std::cos(yaw_); + const Eigen::Matrix2d R_yaw_inv = R_yaw.transpose(); // position pose_with_cov.pose.position.x = X_t(IDX::X); @@ -550,41 +551,41 @@ bool LinearMotionTracker::getTrackedObject( const auto vx = X_t(IDX::VX); const auto vy = X_t(IDX::VY); // rotate this vector with -yaw - Eigen::Vector2d vxy = Ryaw_inv * Eigen::Vector2d(vx, vy); - twist_with_cov.twist.linear.x = vxy(0); - twist_with_cov.twist.linear.y = vxy(1); + Eigen::Vector2d v_local = R_yaw_inv * Eigen::Vector2d(vx, vy); + twist_with_cov.twist.linear.x = v_local(0); + twist_with_cov.twist.linear.y = v_local(1); // acceleration // acceleration need to converted to local coordinate const auto ax = X_t(IDX::AX); const auto ay = X_t(IDX::AY); // rotate this vector with -yaw - Eigen::Vector2d axy = Ryaw_inv * Eigen::Vector2d(ax, ay); - acceleration_with_cov.accel.linear.x = axy(0); - acceleration_with_cov.accel.linear.y = axy(1); + Eigen::Vector2d a_local = R_yaw_inv * Eigen::Vector2d(ax, ay); + acceleration_with_cov.accel.linear.x = a_local(0); + acceleration_with_cov.accel.linear.y = a_local(1); // ===== covariance transformation ===== // since covariance in EKF is in map coordinate and output should be in object coordinate, // we need to transform covariance - Eigen::Matrix2d Pxy_map, Pvxy_map, Paxy_map; - Pxy_map << P(IDX::X, IDX::X), P(IDX::X, IDX::Y), P(IDX::Y, IDX::X), P(IDX::Y, IDX::Y); - Pvxy_map << P(IDX::VX, IDX::VX), P(IDX::VX, IDX::VY), P(IDX::VY, IDX::VX), P(IDX::VY, IDX::VY); - Paxy_map << P(IDX::AX, IDX::AX), P(IDX::AX, IDX::AY), P(IDX::AY, IDX::AX), P(IDX::AY, IDX::AY); + Eigen::Matrix2d P_xy_map, P_v_xy_map, P_a_xy_map; + P_xy_map << P(IDX::X, IDX::X), P(IDX::X, IDX::Y), P(IDX::Y, IDX::X), P(IDX::Y, IDX::Y); + P_v_xy_map << P(IDX::VX, IDX::VX), P(IDX::VX, IDX::VY), P(IDX::VY, IDX::VX), P(IDX::VY, IDX::VY); + P_a_xy_map << P(IDX::AX, IDX::AX), P(IDX::AX, IDX::AY), P(IDX::AY, IDX::AX), P(IDX::AY, IDX::AY); // rotate covariance with -yaw - Eigen::Matrix2d Pxy = Ryaw_inv * Pxy_map * Ryaw_inv.transpose(); - Eigen::Matrix2d Pvxy = Ryaw_inv * Pvxy_map * Ryaw_inv.transpose(); - Eigen::Matrix2d Paxy = Ryaw_inv * Paxy_map * Ryaw_inv.transpose(); + Eigen::Matrix2d P_xy = R_yaw_inv * P_xy_map * R_yaw_inv.transpose(); + Eigen::Matrix2d P_v_xy = R_yaw_inv * P_v_xy_map * R_yaw_inv.transpose(); + Eigen::Matrix2d P_a_xy = R_yaw_inv * P_a_xy_map * R_yaw_inv.transpose(); // position covariance constexpr double no_info_cov = 1e9; // no information constexpr double z_cov = 1. * 1.; // TODO(yukkysaito) Currently tentative constexpr double yaw_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - pose_with_cov.covariance[utils::MSG_COV_IDX::X_X] = Pxy(IDX::X, IDX::X); - pose_with_cov.covariance[utils::MSG_COV_IDX::X_Y] = Pxy(IDX::X, IDX::Y); - pose_with_cov.covariance[utils::MSG_COV_IDX::Y_X] = Pxy(IDX::Y, IDX::X); - pose_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = Pxy(IDX::Y, IDX::Y); + pose_with_cov.covariance[utils::MSG_COV_IDX::X_X] = P_xy(IDX::X, IDX::X); + pose_with_cov.covariance[utils::MSG_COV_IDX::X_Y] = P_xy(IDX::X, IDX::Y); + pose_with_cov.covariance[utils::MSG_COV_IDX::Y_X] = P_xy(IDX::Y, IDX::X); + pose_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = P_xy(IDX::Y, IDX::Y); pose_with_cov.covariance[utils::MSG_COV_IDX::Z_Z] = z_cov; pose_with_cov.covariance[utils::MSG_COV_IDX::ROLL_ROLL] = no_info_cov; pose_with_cov.covariance[utils::MSG_COV_IDX::PITCH_PITCH] = no_info_cov; @@ -594,20 +595,20 @@ bool LinearMotionTracker::getTrackedObject( constexpr double vz_cov = 0.2 * 0.2; // TODO(Yoshi Ri) Currently tentative constexpr double wz_cov = 0.2 * 0.2; // TODO(yukkysaito) Currently tentative - twist_with_cov.covariance[utils::MSG_COV_IDX::X_X] = Pvxy(IDX::X, IDX::X); - twist_with_cov.covariance[utils::MSG_COV_IDX::X_Y] = Pvxy(IDX::X, IDX::Y); - twist_with_cov.covariance[utils::MSG_COV_IDX::Y_X] = Pvxy(IDX::Y, IDX::X); - twist_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = Pvxy(IDX::Y, IDX::Y); + twist_with_cov.covariance[utils::MSG_COV_IDX::X_X] = P_v_xy(IDX::X, IDX::X); + twist_with_cov.covariance[utils::MSG_COV_IDX::X_Y] = P_v_xy(IDX::X, IDX::Y); + twist_with_cov.covariance[utils::MSG_COV_IDX::Y_X] = P_v_xy(IDX::Y, IDX::X); + twist_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = P_v_xy(IDX::Y, IDX::Y); twist_with_cov.covariance[utils::MSG_COV_IDX::Z_Z] = vz_cov; twist_with_cov.covariance[utils::MSG_COV_IDX::ROLL_ROLL] = no_info_cov; twist_with_cov.covariance[utils::MSG_COV_IDX::PITCH_PITCH] = no_info_cov; twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = wz_cov; // acceleration covariance - acceleration_with_cov.covariance[utils::MSG_COV_IDX::X_X] = Paxy(IDX::X, IDX::X); - acceleration_with_cov.covariance[utils::MSG_COV_IDX::X_Y] = Paxy(IDX::X, IDX::Y); - acceleration_with_cov.covariance[utils::MSG_COV_IDX::Y_X] = Paxy(IDX::Y, IDX::X); - acceleration_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = Paxy(IDX::Y, IDX::Y); + acceleration_with_cov.covariance[utils::MSG_COV_IDX::X_X] = P_a_xy(IDX::X, IDX::X); + acceleration_with_cov.covariance[utils::MSG_COV_IDX::X_Y] = P_a_xy(IDX::X, IDX::Y); + acceleration_with_cov.covariance[utils::MSG_COV_IDX::Y_X] = P_a_xy(IDX::Y, IDX::X); + acceleration_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = P_a_xy(IDX::Y, IDX::Y); acceleration_with_cov.covariance[utils::MSG_COV_IDX::Z_Z] = no_info_cov; acceleration_with_cov.covariance[utils::MSG_COV_IDX::ROLL_ROLL] = no_info_cov; acceleration_with_cov.covariance[utils::MSG_COV_IDX::PITCH_PITCH] = no_info_cov; diff --git a/perception/radar_tracks_msgs_converter/include/radar_tracks_msgs_converter/radar_tracks_msgs_converter_node.hpp b/perception/radar_tracks_msgs_converter/include/radar_tracks_msgs_converter/radar_tracks_msgs_converter_node.hpp index 62ebb2ec0146b..351410161d8b2 100644 --- a/perception/radar_tracks_msgs_converter/include/radar_tracks_msgs_converter/radar_tracks_msgs_converter_node.hpp +++ b/perception/radar_tracks_msgs_converter/include/radar_tracks_msgs_converter/radar_tracks_msgs_converter_node.hpp @@ -16,7 +16,7 @@ #define RADAR_TRACKS_MSGS_CONVERTER__RADAR_TRACKS_MSGS_CONVERTER_NODE_HPP_ #include "rclcpp/rclcpp.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" +#include "tier4_autoware_utils/ros/transform_listener.hpp" #include "autoware_auto_perception_msgs/msg/detected_objects.hpp" #include "autoware_auto_perception_msgs/msg/object_classification.hpp" 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 ab5e4eb5abe90..1122bf7b4b18e 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 @@ -14,7 +14,9 @@ #include "radar_tracks_msgs_converter/radar_tracks_msgs_converter_node.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" +#include "tier4_autoware_utils/math/unit_conversion.hpp" +#include "tier4_autoware_utils/ros/msg_covariance.hpp" #include @@ -212,8 +214,15 @@ TrackedObjects RadarTracksMsgsConverterNode::convertRadarTrackToTrackedObjects() kinematics.orientation_availability = TrackedObjectKinematics::AVAILABLE; kinematics.is_stationary = false; - // Twist conversion - geometry_msgs::msg::Vector3 compensated_velocity = radar_track.velocity; + geometry_msgs::msg::Vector3 compensated_velocity{}; + { + double rotate_yaw = tf2::getYaw(transform_->transform.rotation); + const geometry_msgs::msg::Vector3 & vel = radar_track.velocity; + compensated_velocity.x = vel.x * std::cos(rotate_yaw) - vel.y * std::sin(rotate_yaw); + compensated_velocity.y = vel.x * std::sin(rotate_yaw) + vel.y * std::cos(rotate_yaw); + compensated_velocity.z = radar_track.velocity.z; + } + if (node_param_.use_twist_compensation) { if (odometry_data_) { compensated_velocity.x += odometry_data_->twist.twist.linear.x; @@ -233,12 +242,12 @@ TrackedObjects RadarTracksMsgsConverterNode::convertRadarTrackToTrackedObjects() double yaw = tier4_autoware_utils::normalizeRadian( std::atan2(compensated_velocity.y, compensated_velocity.x)); - radar_pose_stamped.pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(yaw); geometry_msgs::msg::PoseStamped transformed_pose_stamped{}; tf2::doTransform(radar_pose_stamped, transformed_pose_stamped, *transform_); kinematics.pose_with_covariance.pose = transformed_pose_stamped.pose; - + kinematics.pose_with_covariance.pose.orientation = + tier4_autoware_utils::createQuaternionFromYaw(yaw); { auto & pose_cov = kinematics.pose_with_covariance.covariance; auto & radar_position_cov = radar_track.position_covariance; diff --git a/perception/shape_estimation/include/shape_estimation/corrector/utils.hpp b/perception/shape_estimation/include/shape_estimation/corrector/utils.hpp index b932c67133850..60ccb0b4ef02c 100644 --- a/perception/shape_estimation/include/shape_estimation/corrector/utils.hpp +++ b/perception/shape_estimation/include/shape_estimation/corrector/utils.hpp @@ -17,8 +17,6 @@ #include "shape_estimation/shape_estimator.hpp" -#include - #include #include diff --git a/perception/shape_estimation/include/shape_estimation/model/model_interface.hpp b/perception/shape_estimation/include/shape_estimation/model/model_interface.hpp index 7f9baa9a0f5e0..ccd04bae40698 100644 --- a/perception/shape_estimation/include/shape_estimation/model/model_interface.hpp +++ b/perception/shape_estimation/include/shape_estimation/model/model_interface.hpp @@ -15,8 +15,6 @@ #ifndef SHAPE_ESTIMATION__MODEL__MODEL_INTERFACE_HPP_ #define SHAPE_ESTIMATION__MODEL__MODEL_INTERFACE_HPP_ -#include - #include #include diff --git a/perception/shape_estimation/lib/corrector/utils.cpp b/perception/shape_estimation/lib/corrector/utils.cpp index e75dcc25f521a..e16f8bed8d36b 100644 --- a/perception/shape_estimation/lib/corrector/utils.cpp +++ b/perception/shape_estimation/lib/corrector/utils.cpp @@ -14,10 +14,11 @@ #include "shape_estimation/corrector/utils.hpp" +#include + #include #include #include - #ifdef ROS_DISTRO_GALACTIC #include #include diff --git a/perception/shape_estimation/src/node.cpp b/perception/shape_estimation/src/node.cpp index 9be64112bc32d..987cf8106c99e 100644 --- a/perception/shape_estimation/src/node.cpp +++ b/perception/shape_estimation/src/node.cpp @@ -15,7 +15,7 @@ #include "shape_estimation/shape_estimator.hpp" #include -#include +#include #include diff --git a/perception/simple_object_merger/include/simple_object_merger/simple_object_merger_node.hpp b/perception/simple_object_merger/include/simple_object_merger/simple_object_merger_node.hpp index 6e2daf9266c9c..0d041439e6092 100644 --- a/perception/simple_object_merger/include/simple_object_merger/simple_object_merger_node.hpp +++ b/perception/simple_object_merger/include/simple_object_merger/simple_object_merger_node.hpp @@ -16,7 +16,7 @@ #define SIMPLE_OBJECT_MERGER__SIMPLE_OBJECT_MERGER_NODE_HPP_ #include "rclcpp/rclcpp.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" +#include "tier4_autoware_utils/ros/transform_listener.hpp" #include "autoware_auto_perception_msgs/msg/detected_objects.hpp" diff --git a/perception/simple_object_merger/src/simple_object_merger_node/simple_object_merger_node.cpp b/perception/simple_object_merger/src/simple_object_merger_node/simple_object_merger_node.cpp index 506a40e672fb6..693fccb7e937c 100644 --- a/perception/simple_object_merger/src/simple_object_merger_node/simple_object_merger_node.cpp +++ b/perception/simple_object_merger/src/simple_object_merger_node/simple_object_merger_node.cpp @@ -14,6 +14,8 @@ #include "simple_object_merger/simple_object_merger_node.hpp" +#include + #include #include #include @@ -168,7 +170,7 @@ void SimpleObjectMergerNode::onTimer() output_objects.header.frame_id = node_param_.new_frame_id; for (size_t i = 0; i < input_topic_size; i++) { - double time_diff = (this->get_clock()->now()).seconds() - + double time_diff = rclcpp::Time(objects_data_.at(i)->header.stamp).seconds() - rclcpp::Time(objects_data_.at(0)->header.stamp).seconds(); if (std::abs(time_diff) < node_param_.timeout_threshold) { transform_ = transform_listener_->getTransform( diff --git a/perception/tensorrt_yolo/CMakeLists.txt b/perception/tensorrt_yolo/CMakeLists.txt index f97a65834fbf8..b059ab44d8ece 100755 --- a/perception/tensorrt_yolo/CMakeLists.txt +++ b/perception/tensorrt_yolo/CMakeLists.txt @@ -70,52 +70,6 @@ else() set(CUDNN_AVAIL OFF) endif() -# Download onnx -set(DATA_PATH "${CMAKE_CURRENT_SOURCE_DIR}/data") -if(NOT EXISTS "${DATA_PATH}") - execute_process(COMMAND mkdir -p ${DATA_PATH}) -endif() -function(download FILE_NAME FILE_HASH) - message(STATUS "Checking and downloading ${FILE_NAME}") - set(FILE_PATH ${DATA_PATH}/${FILE_NAME}) - set(STATUS_CODE 0) - message(STATUS "start ${FILE_NAME}") - if(EXISTS ${FILE_PATH}) - message(STATUS "found ${FILE_NAME}") - file(MD5 ${FILE_PATH} EXISTING_FILE_HASH) - if(${FILE_HASH} STREQUAL ${EXISTING_FILE_HASH}) - message(STATUS "same ${FILE_NAME}") - message(STATUS "File already exists.") - else() - message(STATUS "diff ${FILE_NAME}") - message(STATUS "File hash changes. Downloading now ...") - file(DOWNLOAD https://awf.ml.dev.web.auto/perception/models/${FILE_NAME} ${FILE_PATH} STATUS DOWNLOAD_STATUS TIMEOUT 3600) - list(GET DOWNLOAD_STATUS 0 STATUS_CODE) - list(GET DOWNLOAD_STATUS 1 ERROR_MESSAGE) - endif() - else() - message(STATUS "not found ${FILE_NAME}") - message(STATUS "File doesn't exists. Downloading now ...") - file(DOWNLOAD https://awf.ml.dev.web.auto/perception/models/${FILE_NAME} ${FILE_PATH} STATUS DOWNLOAD_STATUS TIMEOUT 3600) - list(GET DOWNLOAD_STATUS 0 STATUS_CODE) - list(GET DOWNLOAD_STATUS 1 ERROR_MESSAGE) - endif() - if(${STATUS_CODE} EQUAL 0) - message(STATUS "Download completed successfully!") - else() - message(FATAL_ERROR "Error occurred during download: ${ERROR_MESSAGE}") - endif() -endfunction() - -download(yolov3.onnx bde3b120f51db251d9a89cd25205e062) -download(yolov4.onnx 99ed8c5d18acad2ebb5ee80860d9cbaf) -download(yolov4-tiny.onnx d16dfd56dddfce75f25d5aaf4c8f4c40) -download(yolov5s.onnx 646b295db6089597e79163446d6eedca) -download(yolov5m.onnx 8fc91d70599ac59ae92c6490001fd84b) -download(yolov5l.onnx 99ff1fc966bda8efbc45c3b8bf00eea2) -download(yolov5x.onnx c51beebed276735cb16a18ca32959e9f) -download(coco.names 8fc50561361f8bcf96b0177086e7616c) - # create calib image directory for int8 calibration set(CALIB_IMAGE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/calib_image") if(NOT EXISTS "${CALIB_IMAGE_PATH}") @@ -186,7 +140,6 @@ if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL) ament_auto_package(INSTALL_TO_SHARE config - data launch ) @@ -204,7 +157,6 @@ else() message("TensorrtYolo won't be built, CUDA and/or TensorRT were not found.") ament_auto_package(INSTALL_TO_SHARE config - data launch ) endif() diff --git a/perception/tensorrt_yolo/README.md b/perception/tensorrt_yolo/README.md index afa9209c43bb2..58d4af0dfa83d 100644 --- a/perception/tensorrt_yolo/README.md +++ b/perception/tensorrt_yolo/README.md @@ -55,6 +55,7 @@ Jocher, G., et al. (2021). ultralytics/yolov5: v6.0 - YOLOv5n 'Nano' models, Rob | Name | Type | Default Value | Description | | ----------------------- | ------ | ------------- | ------------------------------------------------------------------ | +| `data_path` | string | "" | Packages data and artifacts directory path | | `onnx_file` | string | "" | The onnx file name for yolo model | | `engine_file` | string | "" | The tensorrt engine file name for yolo model | | `label_file` | string | "" | The label file with label names for detected objects written on it | @@ -71,7 +72,7 @@ This package includes multiple licenses. 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. +All models are downloaded during env preparation by ansible (as mention in [installation](https://autowarefoundation.github.io/autoware-documentation/main/installation/autoware/source-installation/)). It is also possible to download them manually, see [Manual downloading of artifacts](https://github.com/autowarefoundation/autoware/tree/main/ansible/roles/artifacts) . 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/perception/tensorrt_yolo/launch/tensorrt_yolo.launch.xml b/perception/tensorrt_yolo/launch/tensorrt_yolo.launch.xml index a548939f2cebe..b2656de0ab72e 100755 --- a/perception/tensorrt_yolo/launch/tensorrt_yolo.launch.xml +++ b/perception/tensorrt_yolo/launch/tensorrt_yolo.launch.xml @@ -3,7 +3,8 @@ - + + @@ -11,11 +12,11 @@ - + - + - + diff --git a/perception/tensorrt_yolox/CMakeLists.txt b/perception/tensorrt_yolox/CMakeLists.txt index 01ade9bac43d6..21860854ccd5f 100644 --- a/perception/tensorrt_yolox/CMakeLists.txt +++ b/perception/tensorrt_yolox/CMakeLists.txt @@ -24,49 +24,6 @@ if(OpenMP_FOUND) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") endif() -########## -# Download pretrained model -set(DATA_PATH "${CMAKE_CURRENT_SOURCE_DIR}/data") -if(NOT EXISTS "${DATA_PATH}") - execute_process(COMMAND mkdir -p ${DATA_PATH}) -endif() -function(download FILE_NAME FILE_HASH) - message(STATUS "Checking and downloading ${FILE_NAME}") - set(FILE_PATH ${DATA_PATH}/${FILE_NAME}) - set(STATUS_CODE 0) - message(STATUS "start ${FILE_NAME}") - if(EXISTS ${FILE_PATH}) - message(STATUS "found ${FILE_NAME}") - file(MD5 ${FILE_PATH} EXISTING_FILE_HASH) - if(${FILE_HASH} STREQUAL ${EXISTING_FILE_HASH}) - message(STATUS "same ${FILE_NAME}") - message(STATUS "File already exists.") - else() - message(STATUS "diff ${FILE_NAME}") - message(STATUS "File hash changes. Downloading now ...") - file(DOWNLOAD https://awf.ml.dev.web.auto/perception/models/${FILE_NAME} ${FILE_PATH} STATUS DOWNLOAD_STATUS TIMEOUT 3600) - list(GET DOWNLOAD_STATUS 0 STATUS_CODE) - list(GET DOWNLOAD_STATUS 1 ERROR_MESSAGE) - endif() - else() - message(STATUS "not found ${FILE_NAME}") - message(STATUS "File doesn't exists. Downloading now ...") - file(DOWNLOAD https://awf.ml.dev.web.auto/perception/models/${FILE_NAME} ${FILE_PATH} STATUS DOWNLOAD_STATUS TIMEOUT 3600) - list(GET DOWNLOAD_STATUS 0 STATUS_CODE) - list(GET DOWNLOAD_STATUS 1 ERROR_MESSAGE) - endif() - if(${STATUS_CODE} EQUAL 0) - message(STATUS "Download completed successfully!") - else() - message(FATAL_ERROR "Error occurred during download: ${ERROR_MESSAGE}") - endif() -endfunction() - -download(yolox-tiny.onnx 97028baf73ce55e115599c9c60651b08) -download(yolox-sPlus-opt.onnx bf3b0155351f90fcdca2626acbfd3bcf) -download(yolox-sPlus-opt.EntropyV2-calibration.table c6e6f1999d5724a017516a956096701f) -download(label.txt 9ceadca8b72b6169ee6aabb861fe3e1e) - ########## # tensorrt_yolox ament_auto_add_library(${PROJECT_NAME} SHARED @@ -157,5 +114,4 @@ endif() ament_auto_package(INSTALL_TO_SHARE launch - data ) diff --git a/perception/tensorrt_yolox/README.md b/perception/tensorrt_yolox/README.md index 3c253a0b68489..ca407b1ff6811 100644 --- a/perception/tensorrt_yolox/README.md +++ b/perception/tensorrt_yolox/README.md @@ -71,7 +71,7 @@ those are labeled as `UNKNOWN`, while detected rectangles are drawn in the visua ## Onnx model -A sample model (named `yolox-tiny.onnx`) is downloaded automatically during the build process. +A sample model (named `yolox-tiny.onnx`) is downloaded by ansible script on env preparation stage, if not, please, follow [Manual downloading of artifacts](https://github.com/autowarefoundation/autoware/tree/main/ansible/roles/artifacts). To accelerate Non-maximum-suppression (NMS), which is one of the common post-process after object detection inference, `EfficientNMS_TRT` module is attached after the ordinal YOLOX (tiny) network. The `EfficientNMS_TRT` module contains fixed values for `score_threshold` and `nms_threshold` in it, @@ -146,7 +146,7 @@ Please refer [the official document](https://github.com/Megvii-BaseDetection/YOL ## Label file -A sample label file (named `label.txt`)is also downloaded automatically during the build process +A sample label file (named `label.txt`)is also downloaded automatically during env preparation process (**NOTE:** This file is incompatible with models that output labels for the COCO dataset (e.g., models from the official YOLOX repository)). This file represents the correspondence between class index (integer outputted from YOLOX network) and diff --git a/perception/tensorrt_yolox/launch/yolox_s_plus_opt.launch.xml b/perception/tensorrt_yolox/launch/yolox_s_plus_opt.launch.xml index 43f59e944f889..3f8d7897ab5d3 100644 --- a/perception/tensorrt_yolox/launch/yolox_s_plus_opt.launch.xml +++ b/perception/tensorrt_yolox/launch/yolox_s_plus_opt.launch.xml @@ -1,9 +1,11 @@ + - - + + + diff --git a/perception/tensorrt_yolox/launch/yolox_tiny.launch.xml b/perception/tensorrt_yolox/launch/yolox_tiny.launch.xml index d8c67e39e0b8a..2f08031ea159f 100644 --- a/perception/tensorrt_yolox/launch/yolox_tiny.launch.xml +++ b/perception/tensorrt_yolox/launch/yolox_tiny.launch.xml @@ -3,7 +3,8 @@ - + + diff --git a/perception/tracking_object_merger/CMakeLists.txt b/perception/tracking_object_merger/CMakeLists.txt new file mode 100644 index 0000000000000..108749c5f91a7 --- /dev/null +++ b/perception/tracking_object_merger/CMakeLists.txt @@ -0,0 +1,50 @@ +cmake_minimum_required(VERSION 3.8) +project(tracking_object_merger VERSION 1.0.0) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic -Werror -Wconversion) +endif() + +find_package(autoware_cmake REQUIRED) +autoware_package() +find_package(nlohmann_json REQUIRED) # for debug + + +# find dependencies +find_package(eigen3_cmake_module REQUIRED) +find_package(Eigen3 REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +include_directories( + SYSTEM + ${EIGEN3_INCLUDE_DIR} +) + +ament_auto_add_library(mu_successive_shortest_path SHARED + src/data_association/mu_successive_shortest_path/mu_successive_shortest_path_wrapper.cpp +) + +ament_auto_add_library(decorative_tracker_merger_node SHARED + src/data_association/data_association.cpp + src/decorative_tracker_merger.cpp + src/utils/utils.cpp + src/utils/tracker_state.cpp +) + +target_link_libraries(decorative_tracker_merger_node + mu_successive_shortest_path + Eigen3::Eigen + nlohmann_json::nlohmann_json # for debug +) + +rclcpp_components_register_node(decorative_tracker_merger_node + PLUGIN "tracking_object_merger::DecorativeTrackerMergerNode" + EXECUTABLE decorative_tracker_merger +) + +ament_auto_package(INSTALL_TO_SHARE + launch + config +) diff --git a/perception/tracking_object_merger/README.md b/perception/tracking_object_merger/README.md new file mode 100644 index 0000000000000..9964534372037 --- /dev/null +++ b/perception/tracking_object_merger/README.md @@ -0,0 +1,114 @@ +# Tracking Object Merger + +## Purpose + +This package try to merge two tracking objects from different sensor. + +## Inner-workings / Algorithms + +Merging tracking objects from different sensor is a combination of data association and state fusion algorithms. + +Detailed process depends on the merger policy. + +### decorative_tracker_merger + +In decorative_tracker_merger, we assume there are dominant tracking objects and sub tracking objects. +The name `decorative` means that sub tracking objects are used to complement the main objects. + +Usually the dominant tracking objects are from LiDAR and sub tracking objects are from Radar or Camera. + +Here show the processing pipeline. + +![decorative_tracker_merger](./image/decorative_tracker_merger.drawio.svg) + +#### time sync + +Sub object(Radar or Camera) often has higher frequency than dominant object(LiDAR). So we need to sync the time of sub object to dominant object. + +![time sync](image/time_sync.drawio.svg) + +#### data association + +In the data association, we use the following rules to determine whether two tracking objects are the same object. + +- gating + - `distance gate`: distance between two tracking objects + - `angle gate`: angle between two tracking objects + - `mahalanobis_distance_gate`: Mahalanobis distance between two tracking objects + - `min_iou_gate`: minimum IoU between two tracking objects + - `max_velocity_gate`: maximum velocity difference between two tracking objects +- score + - score used in matching is equivalent to the distance between two tracking objects + +#### tracklet update + +Sub tracking objects are merged into dominant tracking objects. + +Depends on the tracklet input sensor state, we update the tracklet state with different rules. + +| state\priority | 1st | 2nd | 3rd | +| -------------------------- | ------ | ----- | ------ | +| Kinematics except velocity | LiDAR | Radar | Camera | +| Forward velocity | Radar | LiDAR | Camera | +| Object classification | Camera | LiDAR | Radar | + +#### tracklet management + +We use the `existence_probability` to manage tracklet. + +- When we create a new tracklet, we set the `existence_probability` to $p_{sensor}$ value. +- In each update with specific sensor, we set the `existence_probability` to $p_{sensor}$ value. +- When tracklet does not have update with specific sensor, we reduce the `existence_probability` by `decay_rate` +- Object can be published if `existence_probability` is larger than `publish_probability_threshold` +- Object will be removed if `existence_probability` is smaller than `remove_probability_threshold` + +![tracklet_management](./image/tracklet_management.drawio.svg) + +These parameter can be set in `config/decorative_tracker_merger.param.yaml`. + +```yaml +tracker_state_parameter: + remove_probability_threshold: 0.3 + publish_probability_threshold: 0.6 + default_lidar_existence_probability: 0.7 + default_radar_existence_probability: 0.6 + default_camera_existence_probability: 0.6 + decay_rate: 0.1 + max_dt: 1.0 +``` + +#### input/parameters + +| topic name | message type | description | +| ------------------------------- | ----------------------------------------------- | ------------------------------------------------------------------------------------- | +| `~input/main_object` | `autoware_auto_perception_msgs::TrackedObjects` | Dominant tracking objects. Output will be published with this dominant object stamps. | +| `~input/sub_object` | `autoware_auto_perception_msgs::TrackedObjects` | Sub tracking objects. | +| `output/object` | `autoware_auto_perception_msgs::TrackedObjects` | Merged tracking objects. | +| `debug/interpolated_sub_object` | `autoware_auto_perception_msgs::TrackedObjects` | Interpolated sub tracking objects. | + +Default parameters are set in [config/decorative_tracker_merger.param.yaml](./config/decorative_tracker_merger.param.yaml). + +| parameter name | description | default value | +| ------------------------- | ---------------------------------------------------------------------------------------------------------------------------------------------------------------- | ------------- | +| `base_link_frame_id` | base link frame id. This is used to transform the tracking object. | "base_link" | +| `time_sync_threshold` | time sync threshold. If the time difference between two tracking objects is smaller than this value, we consider these two tracking objects are the same object. | 0.05 | +| `sub_object_timeout_sec` | sub object timeout. If the sub object is not updated for this time, we consider this object is not exist. | 0.5 | +| `main_sensor_type` | main sensor type. This is used to determine the dominant tracking object. | "lidar" | +| `sub_sensor_type` | sub sensor type. This is used to determine the sub tracking object. | "radar" | +| `tracker_state_parameter` | tracker state parameter. This is used to manage the tracklet. | | + +- the detail of `tracker_state_parameter` is described in [tracklet management](#tracklet-management) + +#### tuning + +As explained in [tracklet management](#tracklet-management), this tracker merger tend to maintain the both input tracking objects. + +If there are many false positive tracking objects, + +- decrease `default__existence_probability` of that sensor +- increase `decay_rate` +- increase `publish_probability_threshold` to publish only reliable tracking objects + +### equivalent_tracker_merger + +This is future work. diff --git a/perception/tracking_object_merger/config/data_association_matrix.param.yaml b/perception/tracking_object_merger/config/data_association_matrix.param.yaml new file mode 100644 index 0000000000000..702809b3cede1 --- /dev/null +++ b/perception/tracking_object_merger/config/data_association_matrix.param.yaml @@ -0,0 +1,168 @@ +/**: + ros__parameters: + lidar-lidar: + can_assign_matrix: + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE,PEDESTRIAN + [0, 0, 0, 0, 0, 0, 0, 0, #UNKNOWN + 0, 1, 1, 1, 1, 0, 0, 0, #CAR + 0, 1, 1, 1, 1, 0, 0, 0, #TRUCK + 0, 1, 1, 1, 1, 0, 0, 0, #BUS + 0, 1, 1, 1, 1, 0, 0, 0, #TRAILER + 0, 0, 0, 0, 0, 1, 1, 1, #MOTORBIKE + 0, 0, 0, 0, 0, 1, 1, 1, #BICYCLE + 0, 0, 0, 0, 0, 1, 1, 1] #PEDESTRIAN + + max_dist_matrix: + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN + [4.0, 4.0, 5.0, 5.0, 5.0, 2.0, 2.0, 2.0, #UNKNOWN + 4.0, 2.0, 5.0, 5.0, 5.0, 1.0, 1.0, 1.0, #CAR + 5.0, 5.0, 5.0, 5.0, 5.0, 1.0, 1.0, 1.0, #TRUCK + 5.0, 5.0, 5.0, 5.0, 5.0, 1.0, 1.0, 1.0, #BUS + 5.0, 5.0, 5.0, 5.0, 5.0, 1.0, 1.0, 1.0, #TRAILER + 2.0, 1.0, 1.0, 1.0, 1.0, 3.0, 3.0, 3.0, #MOTORBIKE + 2.0, 1.0, 1.0, 1.0, 1.0, 3.0, 3.0, 3.0, #BICYCLE + 2.0, 1.0, 1.0, 1.0, 1.0, 3.0, 3.0, 2.0] #PEDESTRIAN + + max_rad_matrix: # If value is greater than pi, it will be ignored. + #UNKNOWN, CAR, TRUCK, BUS, TRAILER MOTORBIKE, BICYCLE, PEDESTRIAN + [3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, #UNKNOWN + 3.150, 1.047, 1.047, 1.047, 1.047, 3.150, 3.150, 3.150, #CAR + 3.150, 1.047, 1.047, 1.047, 1.047, 3.150, 3.150, 3.150, #TRUCK + 3.150, 1.047, 1.047, 1.047, 1.047, 3.150, 3.150, 3.150, #BUS + 3.150, 1.047, 1.047, 1.047, 1.047, 3.150, 3.150, 3.150, #TRAILER + 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, #MOTORBIKE + 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, #BICYCLE + 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150] #PEDESTRIAN + + max_velocity_diff_matrix: # Ignored when value is larger than 100.0 + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN + [100.0, 100.0,100.0,100.0,100.0, 100.0, 100.0, 100.0, #UNKNOWN + 100.0, 8.0, 8.0, 8.0, 8.0, 100.0, 100.0, 100.0, #CAR + 100.0, 8.0, 8.0, 8.0, 8.0, 100.0, 100.0, 100.0, #TRUCK + 100.0, 8.0, 8.0, 8.0, 8.0, 100.0, 100.0, 100.0, #BUS + 100.0, 8.0, 8.0, 8.0, 8.0, 100.0, 100.0, 100.0, #TRAILER + 100.0, 100.0,100.0,100.0,100.0, 100.0, 100.0, 100.0, #MOTORBIKE + 100.0, 100.0,100.0,100.0,100.0, 100.0, 100.0, 100.0, #BICYCLE + 100.0, 100.0,100.0,100.0,100.0, 100.0, 100.0, 100.0] #PEDESTRIAN + + min_iou_matrix: # If value is negative, it will be ignored. + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN + [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, #UNKNOWN + 0.1, 0.3, 0.2, 0.2, 0.2, 0.1, 0.1, 0.1, #CAR + 0.1, 0.2, 0.3, 0.3, 0.3, 0.1, 0.1, 0.1, #TRUCK + 0.1, 0.2, 0.3, 0.3, 0.3, 0.1, 0.1, 0.1, #BUS + 0.1, 0.2, 0.3, 0.3, 0.3, 0.1, 0.1, 0.1, #TRAILER + 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, #MOTORBIKE + 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, #BICYCLE + 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1] #PEDESTRIAN + + lidar-radar: + can_assign_matrix: + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE,PEDESTRIAN + [0, 0, 0, 0, 0, 0, 0, 0, #UNKNOWN + 0, 1, 1, 1, 1, 0, 0, 0, #CAR + 0, 1, 1, 1, 1, 0, 0, 0, #TRUCK + 0, 1, 1, 1, 1, 0, 0, 0, #BUS + 0, 1, 1, 1, 1, 0, 0, 0, #TRAILER + 0, 0, 0, 0, 0, 1, 1, 1, #MOTORBIKE + 0, 0, 0, 0, 0, 1, 1, 1, #BICYCLE + 0, 0, 0, 0, 0, 1, 1, 1] #PEDESTRIAN + + max_dist_matrix: + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN + [4.0, 4.0, 5.0, 5.0, 5.0, 3.5, 3.5, 3.5, #UNKNOWN + 4.0, 5.5, 6.0, 6.0, 6.0, 1.0, 1.0, 1.0, #CAR + 5.0, 6.0, 6.5, 6.5, 6.5, 1.0, 1.0, 1.0, #TRUCK + 5.0, 6.0, 6.5, 6.5, 6.5, 1.0, 1.0, 1.0, #BUS + 5.0, 6.0, 6.5, 6.5, 6.5, 1.0, 1.0, 1.0, #TRAILER + 3.5, 1.0, 1.0, 1.0, 1.0, 3.0, 3.0, 3.0, #MOTORBIKE + 3.5, 1.0, 1.0, 1.0, 1.0, 3.0, 3.0, 3.0, #BICYCLE + 3.5, 1.0, 1.0, 1.0, 1.0, 3.0, 3.0, 3.5] #PEDESTRIAN + + max_rad_matrix: # If value is greater than pi, it will be ignored. + #UNKNOWN, CAR, TRUCK, BUS, TRAILER MOTORBIKE, BICYCLE, PEDESTRIAN + [3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, #UNKNOWN + 3.150, 1.047, 1.047, 1.047, 1.047, 3.150, 3.150, 3.150, #CAR + 3.150, 1.047, 1.047, 1.047, 1.047, 3.150, 3.150, 3.150, #TRUCK + 3.150, 1.047, 1.047, 1.047, 1.047, 3.150, 3.150, 3.150, #BUS + 3.150, 1.047, 1.047, 1.047, 1.047, 3.150, 3.150, 3.150, #TRAILER + 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, #MOTORBIKE + 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, #BICYCLE + 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150] #PEDESTRIAN + + max_velocity_diff_matrix: # Ignored when value is larger than 100.0 + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN + [100.0, 100.0,100.0,100.0,100.0, 100.0, 100.0, 100.0, #UNKNOWN + 100.0, 8.0, 8.0, 8.0, 8.0, 100.0, 100.0, 100.0, #CAR + 100.0, 8.0, 8.0, 8.0, 8.0, 100.0, 100.0, 100.0, #TRUCK + 100.0, 8.0, 8.0, 8.0, 8.0, 100.0, 100.0, 100.0, #BUS + 100.0, 8.0, 8.0, 8.0, 8.0, 100.0, 100.0, 100.0, #TRAILER + 100.0, 100.0,100.0,100.0,100.0, 100.0, 100.0, 100.0, #MOTORBIKE + 100.0, 100.0,100.0,100.0,100.0, 100.0, 100.0, 100.0, #BICYCLE + 100.0, 100.0,100.0,100.0,100.0, 100.0, 100.0, 100.0] #PEDESTRIAN + + min_iou_matrix: # set all value to 0.0 to disable this constraint + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #UNKNOWN + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #CAR + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #TRUCK + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #BUS + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #TRAILER + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #MOTORBIKE + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #BICYCLE + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] #PEDESTRIAN + + radar-radar: + can_assign_matrix: + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE,PEDESTRIAN + [0, 0, 0, 0, 0, 0, 0, 0, #UNKNOWN + 0, 1, 1, 1, 1, 0, 0, 0, #CAR + 0, 1, 1, 1, 1, 0, 0, 0, #TRUCK + 0, 1, 1, 1, 1, 0, 0, 0, #BUS + 0, 1, 1, 1, 1, 0, 0, 0, #TRAILER + 0, 0, 0, 0, 0, 1, 1, 1, #MOTORBIKE + 0, 0, 0, 0, 0, 1, 1, 1, #BICYCLE + 0, 0, 0, 0, 0, 1, 1, 1] #PEDESTRIAN + + max_dist_matrix: + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN + [4.0, 4.0, 5.0, 5.0, 5.0, 3.5, 3.5, 3.5, #UNKNOWN + 4.0, 7.0, 7.5, 7.5, 7.5, 1.0, 1.0, 1.0, #CAR + 5.0, 7.5, 7.5, 7.5, 7.5, 1.0, 1.0, 1.0, #TRUCK + 5.0, 7.5, 7.5, 7.5, 7.5, 1.0, 1.0, 1.0, #BUS + 5.0, 7.5, 7.5, 7.5, 7.5, 1.0, 1.0, 1.0, #TRAILER + 3.5, 1.0, 1.0, 1.0, 1.0, 3.0, 3.0, 3.0, #MOTORBIKE + 3.5, 1.0, 1.0, 1.0, 1.0, 3.0, 3.0, 3.0, #BICYCLE + 3.5, 1.0, 1.0, 1.0, 1.0, 3.0, 3.0, 3.5] #PEDESTRIAN + max_rad_matrix: # If value is greater than pi, it will be ignored. + #UNKNOWN, CAR, TRUCK, BUS, TRAILER MOTORBIKE, BICYCLE, PEDESTRIAN + [3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, #UNKNOWN + 3.150, 1.047, 1.047, 1.047, 1.047, 3.150, 3.150, 3.150, #CAR + 3.150, 1.047, 1.047, 1.047, 1.047, 3.150, 3.150, 3.150, #TRUCK + 3.150, 1.047, 1.047, 1.047, 1.047, 3.150, 3.150, 3.150, #BUS + 3.150, 1.047, 1.047, 1.047, 1.047, 3.150, 3.150, 3.150, #TRAILER + 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, #MOTORBIKE + 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, #BICYCLE + 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150] #PEDESTRIAN + + max_velocity_diff_matrix: # Ignored when value is larger than 100.0 + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN + [100.0, 100.0,100.0,100.0,100.0, 100.0, 100.0, 100.0, #UNKNOWN + 100.0, 8.0, 8.0, 8.0, 8.0, 100.0, 100.0, 100.0, #CAR + 100.0, 8.0, 8.0, 8.0, 8.0, 100.0, 100.0, 100.0, #TRUCK + 100.0, 8.0, 8.0, 8.0, 8.0, 100.0, 100.0, 100.0, #BUS + 100.0, 8.0, 8.0, 8.0, 8.0, 100.0, 100.0, 100.0, #TRAILER + 100.0, 100.0,100.0,100.0,100.0, 100.0, 100.0, 100.0, #MOTORBIKE + 100.0, 100.0,100.0,100.0,100.0, 100.0, 100.0, 100.0, #BICYCLE + 100.0, 100.0,100.0,100.0,100.0, 100.0, 100.0, 100.0] #PEDESTRIAN + + min_iou_matrix: # set all value to 0.0 to disable this constraint + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #UNKNOWN + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #CAR + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #TRUCK + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #BUS + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #TRAILER + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #MOTORBIKE + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #BICYCLE + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] #PEDESTRIAN diff --git a/perception/tracking_object_merger/config/decorative_tracker_merger.param.yaml b/perception/tracking_object_merger/config/decorative_tracker_merger.param.yaml new file mode 100644 index 0000000000000..4a108be657a27 --- /dev/null +++ b/perception/tracking_object_merger/config/decorative_tracker_merger.param.yaml @@ -0,0 +1,26 @@ +# Node parameters +/**: + ros__parameters: + base_link_frame_id: "base_link" + time_sync_threshold: 0.999 + sub_object_timeout_sec: 0.8 + publish_interpolated_sub_objects: true #for debug + + # choose the input sensor type for each object type + # "lidar", "radar", "camera" are available + main_sensor_type: "lidar" + sub_sensor_type: "radar" + + # tracker settings + tracker_state_parameter: + remove_probability_threshold: 0.3 + publish_probability_threshold: 0.5 + default_lidar_existence_probability: 0.7 + default_radar_existence_probability: 0.6 + default_camera_existence_probability: 0.5 + decay_rate: 0.1 + max_dt: 1.0 + + # logging settings + enable_logging: false + log_file_path: "/tmp/decorative_tracker_merger.log" diff --git a/perception/tracking_object_merger/config/decorative_tracker_merger_policy.param.yaml b/perception/tracking_object_merger/config/decorative_tracker_merger_policy.param.yaml new file mode 100644 index 0000000000000..0b98e1b202957 --- /dev/null +++ b/perception/tracking_object_merger/config/decorative_tracker_merger_policy.param.yaml @@ -0,0 +1,16 @@ +# Merger policy for decorative tracker merger node +# decorative tracker merger works by merging the sub-object trackers into a main object tracker result +# There are 3 merger policy: +# 1. "skip": skip the sub-object tracker result +# 2. "overwrite": overwrite the main object tracker result with sub-object tracker result +# 3. "fusion": merge the main object tracker result with sub-object tracker result by using covariance based fusion +/**: + ros__parameters: + kinematics_to_be_merged: "velocity" # currently only support "velocity" + + # choose the merger policy for each object type + # : "skip", "overwrite", "fusion" + kinematics_merge_policy: "overwrite" + classification_merge_policy: "skip" + existence_prob_merge_policy: "skip" + shape_merge_policy: "skip" diff --git a/perception/tracking_object_merger/image/decorative_tracker_merger.drawio.svg b/perception/tracking_object_merger/image/decorative_tracker_merger.drawio.svg new file mode 100644 index 0000000000000..3e35fa4d1c338 --- /dev/null +++ b/perception/tracking_object_merger/image/decorative_tracker_merger.drawio.svg @@ -0,0 +1,345 @@ + + + + + + + + + + + + +
+
+
+ main_object +
+
+
+ + main_object + + + + + + +
+
+
+ decorative_tracker_merger_node +
+
+
+ + decorative_tracker_merger_node + + + + + + + + +
+
+
+ sub_object +
+
+
+ + sub_object + + + + + + + + +
+
+
+ time-sync +
+ (delay compensation) +
+
+
+ + time-sync... + + + + + + + + +
+
+
+ buffer +
+
+
+ + buffer + + + + + + +
+
+
+ main obj +
+ timestamp +
+
+
+ + main obj... + + + + + + + + + + +
+
+
+ inner +
+ tracklet +
+
+
+ + inner... + + + + + + + +
+
+
+ not +
+ found +
+
+
+ + not... + + + + + + + +
+
+
+ found +
+
+
+ + found + + + + + + +
+
+
+ association +
+
+
+ + association + + + + + + + + +
+
+
+ update +
+ tracklet +
+
+
+ + update... + + + + + + + + +
+
+
+ add new +
+ tracklet +
+
+
+ + add new... + + + + + + + +
+
+
+ converted +
+ tracked object +
+
+
+ + converted... + + + + + + + + +
+
+
+ tracklet management +
+ (publisher) +
+
+
+ + tracklet management... + + + + + + +
+
+
+ merged object +
+
+
+ + merged object + + + + + + +
+
+
msg: TrackedObjects
+
+
+ + msg: TrackedObjects + + + + + + +
+
+
+ remove when low existence +
+ probability objects +
+
+
+ + remove when low existence... + + + + + + + Text is not SVG - cannot display + + + diff --git a/perception/tracking_object_merger/image/time_sync.drawio.svg b/perception/tracking_object_merger/image/time_sync.drawio.svg new file mode 100644 index 0000000000000..dad2eb1a7fd27 --- /dev/null +++ b/perception/tracking_object_merger/image/time_sync.drawio.svg @@ -0,0 +1,223 @@ + + + + + + + + + + + +
+
+
time
+
+
+ + time + + + + + + + +
+
+
main object topic
+
+
+ + main object topic + + + + + + +
+
+
sub object topic
+
+
+ + sub object topic + + + + + + + + + + + + + + + + + + + + +
+
+
publish timing
+
+
+ + publish timing + + + + + + +
+
+
+ interpolated sub object +
+
+
+ + interpolated sub object + + + + + + + + +
+
+
raw main/sub object
+
+
+ + raw main/sub object + + + + + + + + + +
+
+
+ time sync threshold +
+
+
+ + time sync threshold + + + + + + +
+
+
+ time synchronize prediction pattern +
+
+
+ + time synchronize prediction pattern + + + + + + + + + +
+
+
+ 1. forward +
+
+
+ 2. backward +
+
+
+ 3. interpolation +
+
+
+
+ + 1. forward... + + + + + + + + + + + + + + + + + + +
+
+
legend
+
+
+ + legend + + + + + + +
+
+
+ disabled +
+ now +
+
+
+ + disabled... + + + + + + + Text is not SVG - cannot display + + + diff --git a/perception/tracking_object_merger/image/tracklet_management.drawio.svg b/perception/tracking_object_merger/image/tracklet_management.drawio.svg new file mode 100644 index 0000000000000..4f2a00dfd4dfd --- /dev/null +++ b/perception/tracking_object_merger/image/tracklet_management.drawio.svg @@ -0,0 +1,216 @@ + + + + + + + + +
+
+
0.0
+
+
+ + 0.0 + + + + + + +
+
+
1.0
+
+
+ + 1.0 + + + + + + +
+
+
Existence Probability
+
+
+ + Existence Probability + + + + + + + +
+
+
remove threshold
+
+
+ + remove threshold + + + + + + +
+
+
can publish threshold
+
+
+ + can publish threshold + + + + + + + + + +
+
+
+ decay when +
+ not observed +
+
+
+ + decay when... + + + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + + + + + + + +
+
+
+ init or update probability +
+ from sensor +
+
+
+ + init or update probabilit... + + + + + + + Text is not SVG - cannot display + + + diff --git a/perception/tracking_object_merger/include/tracking_object_merger/data_association/data_association.hpp b/perception/tracking_object_merger/include/tracking_object_merger/data_association/data_association.hpp new file mode 100644 index 0000000000000..05fc9f6caccb5 --- /dev/null +++ b/perception/tracking_object_merger/include/tracking_object_merger/data_association/data_association.hpp @@ -0,0 +1,73 @@ +// Copyright 2023 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. +// +// +// Author: v1.0 Yukihiro Saito +// + +#ifndef TRACKING_OBJECT_MERGER__DATA_ASSOCIATION__DATA_ASSOCIATION_HPP_ +#define TRACKING_OBJECT_MERGER__DATA_ASSOCIATION__DATA_ASSOCIATION_HPP_ + +// #include // for debug json library + +#include +#include +#include +#include + +#define EIGEN_MPL2_ONLY +#include "tracking_object_merger/data_association/solver/gnn_solver.hpp" +#include "tracking_object_merger/utils/tracker_state.hpp" + +#include +#include + +#include +#include + +#include + +class DataAssociation +{ +private: + Eigen::MatrixXi can_assign_matrix_; + Eigen::MatrixXd max_dist_matrix_; + Eigen::MatrixXd max_rad_matrix_; + Eigen::MatrixXd min_iou_matrix_; + Eigen::MatrixXd max_velocity_diff_matrix_; + const double score_threshold_; + std::unique_ptr gnn_solver_ptr_; + +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + DataAssociation( + std::vector can_assign_vector, std::vector max_dist_vector, + std::vector max_rad_vector, std::vector min_iou_vector, + std::vector max_velocity_diff_vector); + void assign( + const Eigen::MatrixXd & src, std::unordered_map & direct_assignment, + std::unordered_map & reverse_assignment); + Eigen::MatrixXd calcScoreMatrix( + const autoware_auto_perception_msgs::msg::TrackedObjects & objects0, + const autoware_auto_perception_msgs::msg::TrackedObjects & objects1); + Eigen::MatrixXd calcScoreMatrix( + const autoware_auto_perception_msgs::msg::TrackedObjects & objects0, + const std::vector & trackers); + double calcScoreBetweenObjects( + const autoware_auto_perception_msgs::msg::TrackedObject & object0, + const autoware_auto_perception_msgs::msg::TrackedObject & object1) const; + virtual ~DataAssociation() {} +}; + +#endif // TRACKING_OBJECT_MERGER__DATA_ASSOCIATION__DATA_ASSOCIATION_HPP_ diff --git a/perception/tracking_object_merger/include/tracking_object_merger/data_association/solver/gnn_solver.hpp b/perception/tracking_object_merger/include/tracking_object_merger/data_association/solver/gnn_solver.hpp new file mode 100644 index 0000000000000..31240848f1977 --- /dev/null +++ b/perception/tracking_object_merger/include/tracking_object_merger/data_association/solver/gnn_solver.hpp @@ -0,0 +1,22 @@ +// Copyright 2023 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 TRACKING_OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_HPP_ +#define TRACKING_OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_HPP_ + +#include "tracking_object_merger/data_association/solver/gnn_solver_interface.hpp" +#include "tracking_object_merger/data_association/solver/mu_successive_shortest_path.hpp" +#include "tracking_object_merger/data_association/solver/successive_shortest_path.hpp" + +#endif // TRACKING_OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_HPP_ diff --git a/perception/tracking_object_merger/include/tracking_object_merger/data_association/solver/gnn_solver_interface.hpp b/perception/tracking_object_merger/include/tracking_object_merger/data_association/solver/gnn_solver_interface.hpp new file mode 100644 index 0000000000000..e915b5d2a9e7b --- /dev/null +++ b/perception/tracking_object_merger/include/tracking_object_merger/data_association/solver/gnn_solver_interface.hpp @@ -0,0 +1,35 @@ +// Copyright 2023 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 TRACKING_OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_ +#define TRACKING_OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_ + +#include +#include + +namespace gnn_solver +{ +class GnnSolverInterface +{ +public: + GnnSolverInterface() = default; + virtual ~GnnSolverInterface() = default; + + virtual void maximizeLinearAssignment( + const std::vector> & cost, std::unordered_map * direct_assignment, + std::unordered_map * reverse_assignment) = 0; +}; +} // namespace gnn_solver + +#endif // TRACKING_OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_ diff --git a/perception/tracking_object_merger/include/tracking_object_merger/data_association/solver/mu_successive_shortest_path.hpp b/perception/tracking_object_merger/include/tracking_object_merger/data_association/solver/mu_successive_shortest_path.hpp new file mode 100644 index 0000000000000..5c0ebc04fdde3 --- /dev/null +++ b/perception/tracking_object_merger/include/tracking_object_merger/data_association/solver/mu_successive_shortest_path.hpp @@ -0,0 +1,37 @@ +// Copyright 2023 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 TRACKING_OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__MU_SUCCESSIVE_SHORTEST_PATH_HPP_ +#define TRACKING_OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__MU_SUCCESSIVE_SHORTEST_PATH_HPP_ + +#include "tracking_object_merger/data_association/solver/gnn_solver_interface.hpp" + +#include +#include + +namespace gnn_solver +{ +class MuSSP : public GnnSolverInterface +{ +public: + MuSSP() = default; + ~MuSSP() = default; + + void maximizeLinearAssignment( + const std::vector> & cost, std::unordered_map * direct_assignment, + std::unordered_map * reverse_assignment) override; +}; +} // namespace gnn_solver + +#endif // TRACKING_OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__MU_SUCCESSIVE_SHORTEST_PATH_HPP_ diff --git a/perception/tracking_object_merger/include/tracking_object_merger/data_association/solver/successive_shortest_path.hpp b/perception/tracking_object_merger/include/tracking_object_merger/data_association/solver/successive_shortest_path.hpp new file mode 100644 index 0000000000000..47a737cf58298 --- /dev/null +++ b/perception/tracking_object_merger/include/tracking_object_merger/data_association/solver/successive_shortest_path.hpp @@ -0,0 +1,37 @@ +// Copyright 2023 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 TRACKING_OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__SUCCESSIVE_SHORTEST_PATH_HPP_ +#define TRACKING_OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__SUCCESSIVE_SHORTEST_PATH_HPP_ + +#include "tracking_object_merger/data_association/solver/gnn_solver_interface.hpp" + +#include +#include + +namespace gnn_solver +{ +class SSP : public GnnSolverInterface +{ +public: + SSP() = default; + ~SSP() = default; + + void maximizeLinearAssignment( + const std::vector> & cost, std::unordered_map * direct_assignment, + std::unordered_map * reverse_assignment) override; +}; +} // namespace gnn_solver + +#endif // TRACKING_OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__SUCCESSIVE_SHORTEST_PATH_HPP_ diff --git a/perception/tracking_object_merger/include/tracking_object_merger/decorative_tracker_merger.hpp b/perception/tracking_object_merger/include/tracking_object_merger/decorative_tracker_merger.hpp new file mode 100644 index 0000000000000..0509fb2a07dc5 --- /dev/null +++ b/perception/tracking_object_merger/include/tracking_object_merger/decorative_tracker_merger.hpp @@ -0,0 +1,134 @@ +// Copyright 2023 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 TRACKING_OBJECT_MERGER__DECORATIVE_TRACKER_MERGER_HPP_ +#define TRACKING_OBJECT_MERGER__DECORATIVE_TRACKER_MERGER_HPP_ + +#include "tracking_object_merger/data_association/data_association.hpp" +#include "tracking_object_merger/utils/tracker_state.hpp" +#include "tracking_object_merger/utils/utils.hpp" + +#include + +#include +#include + +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif + +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace tracking_object_merger +{ + +class DecorativeTrackerMergerNode : public rclcpp::Node +{ +public: + explicit DecorativeTrackerMergerNode(const rclcpp::NodeOptions & node_options); + enum class PriorityMode : int { Object0 = 0, Object1 = 1, Confidence = 2 }; + +private: + void set3dDataAssociation( + const std::string & prefix, + std::unordered_map> & data_association_map); + + void mainObjectsCallback( + const autoware_auto_perception_msgs::msg::TrackedObjects::ConstSharedPtr & input_object_msg); + void subObjectsCallback( + const autoware_auto_perception_msgs::msg::TrackedObjects::ConstSharedPtr & input_object_msg); + + bool decorativeMerger( + const MEASUREMENT_STATE input_index, + const autoware_auto_perception_msgs::msg::TrackedObjects::ConstSharedPtr & input_object_msg); + autoware_auto_perception_msgs::msg::TrackedObjects predictFutureState( + const autoware_auto_perception_msgs::msg::TrackedObjects::ConstSharedPtr & input_object_msg, + const std_msgs::msg::Header & header); + std::optional interpolateObjectState( + const autoware_auto_perception_msgs::msg::TrackedObjects::ConstSharedPtr & input_object_msg1, + const autoware_auto_perception_msgs::msg::TrackedObjects::ConstSharedPtr & input_object_msg2, + const std_msgs::msg::Header & header); + TrackedObjects getTrackedObjects(const std_msgs::msg::Header & header); + TrackerState createNewTracker( + const MEASUREMENT_STATE input_index, rclcpp::Time current_time, + const autoware_auto_perception_msgs::msg::TrackedObject & input_object); + +private: + tf2_ros::Buffer tf_buffer_; + tf2_ros::TransformListener tf_listener_; + rclcpp::Publisher::SharedPtr + merged_object_pub_; + rclcpp::Subscription::SharedPtr + sub_main_objects_; + rclcpp::Subscription::SharedPtr + sub_sub_objects_; + // debug object publisher + rclcpp::Publisher::SharedPtr + debug_object_pub_; + bool publish_interpolated_sub_objects_; + + /* handle objects */ + std::unordered_map> + input_merger_map_; + MEASUREMENT_STATE main_sensor_type_; + MEASUREMENT_STATE sub_sensor_type_; + std::vector inner_tracker_objects_; + std::unordered_map> data_association_map_; + std::string target_frame_; + std::string base_link_frame_id_; + // buffer to save the sub objects + std::vector + sub_objects_buffer_; + double sub_object_timeout_sec_; + double time_sync_threshold_; + + // tracker default settings + TrackerStateParameter tracker_state_parameter_; + + // merge policy (currently not used) + struct + { + std::string kinematics_to_be_merged; + merger_utils::MergePolicy kinematics_merge_policy; + merger_utils::MergePolicy classification_merge_policy; + merger_utils::MergePolicy existence_prob_merge_policy; + merger_utils::MergePolicy shape_merge_policy; + } merger_policy_params_; + + std::map merger_policy_map_ = { + {"skip", merger_utils::MergePolicy::SKIP}, + {"overwrite", merger_utils::MergePolicy::OVERWRITE}, + {"fusion", merger_utils::MergePolicy::FUSION}}; + + // debug parameters + struct logging + { + bool enable = false; + std::string path; + } logging_; +}; + +} // namespace tracking_object_merger + +#endif // TRACKING_OBJECT_MERGER__DECORATIVE_TRACKER_MERGER_HPP_ diff --git a/perception/tracking_object_merger/include/tracking_object_merger/utils/tracker_state.hpp b/perception/tracking_object_merger/include/tracking_object_merger/utils/tracker_state.hpp new file mode 100644 index 0000000000000..6bedf7db8d727 --- /dev/null +++ b/perception/tracking_object_merger/include/tracking_object_merger/utils/tracker_state.hpp @@ -0,0 +1,148 @@ +// Copyright 2023 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 TRACKING_OBJECT_MERGER__UTILS__TRACKER_STATE_HPP_ +#define TRACKING_OBJECT_MERGER__UTILS__TRACKER_STATE_HPP_ + +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +using autoware_auto_perception_msgs::msg::TrackedObject; +using autoware_auto_perception_msgs::msg::TrackedObjects; + +// Enum classes to distinguish input sources +enum class MEASUREMENT_STATE : int { + NONE = 0, // 0000 + LIDAR = 1, // 0001 + RADAR = 2, // 0010 + CAMERA = 4, // 0100 + LIDAR_RADAR = 3, // 0011 + LIDAR_CAMERA = 5, // 0101 + RADAR_CAMERA = 6, // 0110 + LIDAR_RADAR_CAMERA = 7, // 0111 +}; + +// Operator overloading for MEASUREMENT_STATE +// 1. operator| for MEASUREMENT_STATE +// e.g. MEASUREMENT_STATE::LIDAR | MEASUREMENT_STATE::RADAR == MEASUREMENT_STATE::LIDAR_RADAR +// 2. operator& for MEASUREMENT_STATE +// e.g. MEASUREMENT_STATE::LIDAR_RADAR & MEASUREMENT_STATE::LIDAR == true +// e.g. MEASUREMENT_STATE::LIDAR_RADAR & MEASUREMENT_STATE::CAMERA == false +inline MEASUREMENT_STATE operator|(MEASUREMENT_STATE lhs, MEASUREMENT_STATE rhs) +{ + return static_cast(static_cast(lhs) | static_cast(rhs)); +} +inline bool operator&(MEASUREMENT_STATE combined, MEASUREMENT_STATE target) +{ + return (static_cast(combined) & static_cast(target)) != 0; +} + +// Struct to handle tracker state public parameter +struct TrackerStateParameter +{ + double publish_probability_threshold = 0.5; + double remove_probability_threshold = 0.3; + double default_lidar_existence_probability = 0.8; + double default_radar_existence_probability = 0.7; + double default_camera_existence_probability = 0.6; + double decay_rate = 0.1; + double max_dt = 2.0; +}; + +// Class to handle tracker state +class TrackerState +{ +private: + /* data */ + TrackedObject tracked_object_; + rclcpp::Time last_update_time_; + // Eigen::MatrixXf state_matrix_; + // Eigen::MatrixXf covariance_matrix_; + + // timer handle + std::unordered_map last_updated_time_map_; + double max_dt_ = 2.0; + +public: + TrackerState( + const MEASUREMENT_STATE input, const rclcpp::Time & last_update_time, + const TrackedObject & tracked_object); + TrackerState( + const MEASUREMENT_STATE input_source, const rclcpp::Time & last_update_time, + const autoware_auto_perception_msgs::msg::TrackedObject & tracked_object, + const unique_identifier_msgs::msg::UUID & uuid); + + ~TrackerState(); + +public: + void setParameter(const TrackerStateParameter & parameter); + bool predict(const rclcpp::Time & time); + bool predict(const double dt, std::function func); + MEASUREMENT_STATE getCurrentMeasurementState(const rclcpp::Time & current_time) const; + bool updateState( + const MEASUREMENT_STATE input, const rclcpp::Time & current_time, + const TrackedObject & tracked_object); + void updateWithLIDAR(const rclcpp::Time & current_time, const TrackedObject & tracked_object); + void updateWithRADAR(const rclcpp::Time & current_time, const TrackedObject & tracked_object); + void updateWithCAMERA(const rclcpp::Time & current_time, const TrackedObject & tracked_object); + void updateWithoutSensor(const rclcpp::Time & current_time); + bool update(const MEASUREMENT_STATE input, const TrackedObject & tracked_object); + bool updateWithFunction( + const MEASUREMENT_STATE input, const rclcpp::Time & current_time, + const TrackedObject & tracked_object, + std::function update_func); + // const functions + bool hasUUID(const MEASUREMENT_STATE input, const unique_identifier_msgs::msg::UUID & uuid) const; + bool isValid() const; + bool canPublish() const; + TrackedObject getObject() const; + +public: + // handle uuid + unique_identifier_msgs::msg::UUID const_uuid_; + // each sensor input to uuid map + std::unordered_map> + input_uuid_map_; + MEASUREMENT_STATE measurement_state_; + + // following lifecycle control parameter is overwritten by TrackerStateParameter + std::unordered_map default_existence_probability_map_ = { + {MEASUREMENT_STATE::LIDAR, 0.8}, + {MEASUREMENT_STATE::RADAR, 0.7}, + {MEASUREMENT_STATE::CAMERA, 0.6}, + }; + double existence_probability_ = 0.0; + double publish_probability_threshold_ = 0.5; + double remove_probability_threshold_ = 0.3; + double decay_rate_ = 0.1; +}; + +TrackedObjects getTrackedObjectsFromTrackerStates( + std::vector & tracker_states, const rclcpp::Time & time); + +#endif // TRACKING_OBJECT_MERGER__UTILS__TRACKER_STATE_HPP_ diff --git a/perception/tracking_object_merger/include/tracking_object_merger/utils/utils.hpp b/perception/tracking_object_merger/include/tracking_object_merger/utils/utils.hpp new file mode 100644 index 0000000000000..bc6dfef9b80bf --- /dev/null +++ b/perception/tracking_object_merger/include/tracking_object_merger/utils/utils.hpp @@ -0,0 +1,133 @@ +// Copyright 2023 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 TRACKING_OBJECT_MERGER__UTILS__UTILS_HPP_ +#define TRACKING_OBJECT_MERGER__UTILS__UTILS_HPP_ + +// #include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +using autoware_auto_perception_msgs::msg::TrackedObject; +using autoware_auto_perception_msgs::msg::TrackedObjects; +namespace utils +{ +enum MSG_COV_IDX { + X_X = 0, + X_Y = 1, + X_Z = 2, + X_ROLL = 3, + X_PITCH = 4, + X_YAW = 5, + Y_X = 6, + Y_Y = 7, + Y_Z = 8, + Y_ROLL = 9, + Y_PITCH = 10, + Y_YAW = 11, + Z_X = 12, + Z_Y = 13, + Z_Z = 14, + Z_ROLL = 15, + Z_PITCH = 16, + Z_YAW = 17, + ROLL_X = 18, + ROLL_Y = 19, + ROLL_Z = 20, + ROLL_ROLL = 21, + ROLL_PITCH = 22, + ROLL_YAW = 23, + PITCH_X = 24, + PITCH_Y = 25, + PITCH_Z = 26, + PITCH_ROLL = 27, + PITCH_PITCH = 28, + PITCH_YAW = 29, + YAW_X = 30, + YAW_Y = 31, + YAW_Z = 32, + YAW_ROLL = 33, + YAW_PITCH = 34, + YAW_YAW = 35 +}; + +// linear interpolation for tracked objects +TrackedObject linearInterpolationForTrackedObject( + const TrackedObject & obj1, const TrackedObject & obj2); + +// predict tracked objects +TrackedObject predictPastOrFutureTrackedObject(const TrackedObject & obj, const double dt); + +TrackedObjects predictPastOrFutureTrackedObjects( + const TrackedObjects & obj, const std_msgs::msg::Header & header); + +// predict tracked objects +TrackedObjects interpolateTrackedObjects( + const TrackedObjects & objects1, const TrackedObjects & objects2, std_msgs::msg::Header header); + +} // namespace utils + +namespace merger_utils +{ +// merge policy +enum MergePolicy : int { SKIP = 0, OVERWRITE = 1, FUSION = 2 }; + +// object kinematics velocity merger +autoware_auto_perception_msgs::msg::TrackedObjectKinematics objectKinematicsVXMerger( + const TrackedObject & main_obj, const TrackedObject & sub_obj, const MergePolicy policy); + +// object classification merger +TrackedObject objectClassificationMerger( + const TrackedObject & main_obj, const TrackedObject & sub_obj, const MergePolicy policy); + +// probability merger +float probabilityMerger(const float main_prob, const float sub_prob, const MergePolicy policy); + +// shape merger +autoware_auto_perception_msgs::msg::Shape shapeMerger( + const TrackedObject & main_obj, const TrackedObject & sub_obj, const MergePolicy policy); + +// update tracked object +void updateExceptVelocity(TrackedObject & main_obj, const TrackedObject & sub_obj); + +void updateOnlyObjectVelocity(TrackedObject & main_obj, const TrackedObject & sub_obj); + +void updateOnlyClassification(TrackedObject & main_obj, const TrackedObject & sub_obj); + +void updateWholeTrackedObject(TrackedObject & main_obj, const TrackedObject & sub_obj); + +} // namespace merger_utils + +#endif // TRACKING_OBJECT_MERGER__UTILS__UTILS_HPP_ diff --git a/perception/tracking_object_merger/launch/decorative_tracker_merger.launch.xml b/perception/tracking_object_merger/launch/decorative_tracker_merger.launch.xml new file mode 100644 index 0000000000000..5affbe474e8ae --- /dev/null +++ b/perception/tracking_object_merger/launch/decorative_tracker_merger.launch.xml @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/perception/tracking_object_merger/package.xml b/perception/tracking_object_merger/package.xml new file mode 100644 index 0000000000000..d74a8449b20e6 --- /dev/null +++ b/perception/tracking_object_merger/package.xml @@ -0,0 +1,32 @@ + + + + tracking_object_merger + 0.0.0 + merge tracking object + Yukihiro Saito + Yoshi Ri + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + eigen3_cmake_module + + autoware_auto_perception_msgs + eigen + mussp + object_recognition_utils + rclcpp + rclcpp_components + tf2 + tf2_ros + tier4_autoware_utils + tier4_perception_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/perception/tracking_object_merger/src/data_association/data_association.cpp b/perception/tracking_object_merger/src/data_association/data_association.cpp new file mode 100644 index 0000000000000..e7ab6077250f8 --- /dev/null +++ b/perception/tracking_object_merger/src/data_association/data_association.cpp @@ -0,0 +1,238 @@ +// Copyright 2023 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 "tracking_object_merger/data_association/data_association.hpp" + +#include "object_recognition_utils/object_recognition_utils.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" +#include "tracking_object_merger/data_association/solver/gnn_solver.hpp" +#include "tracking_object_merger/utils/utils.hpp" + +#include +#include +#include +#include +#include +#include +namespace +{ +double getFormedYawAngle( + const geometry_msgs::msg::Quaternion & quat0, const geometry_msgs::msg::Quaternion & quat1, + const bool distinguish_front_or_back = true) +{ + const double yaw0 = tier4_autoware_utils::normalizeRadian(tf2::getYaw(quat0)); + const double yaw1 = tier4_autoware_utils::normalizeRadian(tf2::getYaw(quat1)); + const double angle_range = distinguish_front_or_back ? M_PI : M_PI_2; + const double angle_step = distinguish_front_or_back ? 2.0 * M_PI : M_PI; + // Fixed yaw0 to be in the range of +-90 or 180 degrees of yaw1 + double fixed_yaw0 = yaw0; + while (angle_range <= yaw1 - fixed_yaw0) { + fixed_yaw0 = fixed_yaw0 + angle_step; + } + while (angle_range <= fixed_yaw0 - yaw1) { + fixed_yaw0 = fixed_yaw0 - angle_step; + } + return std::fabs(fixed_yaw0 - yaw1); +} +} // namespace + +DataAssociation::DataAssociation( + std::vector can_assign_vector, std::vector max_dist_vector, + std::vector max_rad_vector, std::vector min_iou_vector, + std::vector max_velocity_diff_vector) +: score_threshold_(0.01) +{ + { + const int assign_label_num = static_cast(std::sqrt(can_assign_vector.size())); + Eigen::Map can_assign_matrix_tmp( + can_assign_vector.data(), assign_label_num, assign_label_num); + can_assign_matrix_ = can_assign_matrix_tmp.transpose(); + } + { + const int max_dist_label_num = static_cast(std::sqrt(max_dist_vector.size())); + Eigen::Map max_dist_matrix_tmp( + max_dist_vector.data(), max_dist_label_num, max_dist_label_num); + max_dist_matrix_ = max_dist_matrix_tmp.transpose(); + } + { + const int max_rad_label_num = static_cast(std::sqrt(max_rad_vector.size())); + Eigen::Map max_rad_matrix_tmp( + max_rad_vector.data(), max_rad_label_num, max_rad_label_num); + max_rad_matrix_ = max_rad_matrix_tmp.transpose(); + } + { + const int min_iou_label_num = static_cast(std::sqrt(min_iou_vector.size())); + Eigen::Map min_iou_matrix_tmp( + min_iou_vector.data(), min_iou_label_num, min_iou_label_num); + min_iou_matrix_ = min_iou_matrix_tmp.transpose(); + } + { + const int max_velocity_diff_label_num = + static_cast(std::sqrt(max_velocity_diff_vector.size())); + Eigen::Map max_velocity_diff_matrix_tmp( + max_velocity_diff_vector.data(), max_velocity_diff_label_num, max_velocity_diff_label_num); + max_velocity_diff_matrix_ = max_velocity_diff_matrix_tmp.transpose(); + } + + gnn_solver_ptr_ = std::make_unique(); +} + +void DataAssociation::assign( + const Eigen::MatrixXd & src, std::unordered_map & direct_assignment, + std::unordered_map & reverse_assignment) +{ + std::vector> score(src.rows()); + for (int row = 0; row < src.rows(); ++row) { + score.at(row).resize(src.cols()); + for (int col = 0; col < src.cols(); ++col) { + score.at(row).at(col) = src(row, col); + } + } + // Solve + gnn_solver_ptr_->maximizeLinearAssignment(score, &direct_assignment, &reverse_assignment); + + for (auto itr = direct_assignment.begin(); itr != direct_assignment.end();) { + if (src(itr->first, itr->second) < score_threshold_) { + itr = direct_assignment.erase(itr); + continue; + } else { + ++itr; + } + } + for (auto itr = reverse_assignment.begin(); itr != reverse_assignment.end();) { + if (src(itr->second, itr->first) < score_threshold_) { + itr = reverse_assignment.erase(itr); + continue; + } else { + ++itr; + } + } +} + +/** + * @brief calc score matrix between two tracked objects + * This is used to associate input measurements + * + * @param objects0 : measurements + * @param objects1 : base objects(tracker objects) + * @return Eigen::MatrixXd + */ +Eigen::MatrixXd DataAssociation::calcScoreMatrix( + const autoware_auto_perception_msgs::msg::TrackedObjects & objects0, + const autoware_auto_perception_msgs::msg::TrackedObjects & objects1) +{ + Eigen::MatrixXd score_matrix = + Eigen::MatrixXd::Zero(objects1.objects.size(), objects0.objects.size()); + for (size_t objects1_idx = 0; objects1_idx < objects1.objects.size(); ++objects1_idx) { + const auto & object1 = objects1.objects.at(objects1_idx); + for (size_t objects0_idx = 0; objects0_idx < objects0.objects.size(); ++objects0_idx) { + const auto & object0 = objects0.objects.at(objects0_idx); + const double score = calcScoreBetweenObjects(object0, object1); + + score_matrix(objects1_idx, objects0_idx) = score; + } + } + return score_matrix; +} + +/** + * @brief calc score matrix between two tracked object and Tracker State + * + * @param objects0 : measurements + * @param objects1 : tracker inner objects + * @return Eigen::MatrixXd + */ +Eigen::MatrixXd DataAssociation::calcScoreMatrix( + const autoware_auto_perception_msgs::msg::TrackedObjects & objects0, + const std::vector & trackers) +{ + Eigen::MatrixXd score_matrix = Eigen::MatrixXd::Zero(trackers.size(), objects0.objects.size()); + for (size_t trackers_idx = 0; trackers_idx < trackers.size(); ++trackers_idx) { + const auto & object1 = trackers.at(trackers_idx).getObject(); + + for (size_t objects0_idx = 0; objects0_idx < objects0.objects.size(); ++objects0_idx) { + const auto & object0 = objects0.objects.at(objects0_idx); + const double score = calcScoreBetweenObjects(object0, object1); + + score_matrix(trackers_idx, objects0_idx) = score; + } + } + return score_matrix; +} + +double DataAssociation::calcScoreBetweenObjects( + const autoware_auto_perception_msgs::msg::TrackedObject & object0, + const autoware_auto_perception_msgs::msg::TrackedObject & object1) const +{ + const std::uint8_t object1_label = + object_recognition_utils::getHighestProbLabel(object1.classification); + const std::uint8_t object0_label = + object_recognition_utils::getHighestProbLabel(object0.classification); + + std::vector tracker_pose = { + object1.kinematics.pose_with_covariance.pose.position.x, + object1.kinematics.pose_with_covariance.pose.position.y}; + std::vector measurement_pose = { + object0.kinematics.pose_with_covariance.pose.position.x, + object0.kinematics.pose_with_covariance.pose.position.y}; + + double score = 0.0; + if (can_assign_matrix_(object1_label, object0_label)) { + const double max_dist = max_dist_matrix_(object1_label, object0_label); + const double dist = tier4_autoware_utils::calcDistance2d( + object0.kinematics.pose_with_covariance.pose.position, + object1.kinematics.pose_with_covariance.pose.position); + + bool passed_gate = true; + // dist gate + if (passed_gate) { + if (max_dist < dist) passed_gate = false; + } + // angle gate + if (passed_gate) { + const double max_rad = max_rad_matrix_(object1_label, object0_label); + const double angle = getFormedYawAngle( + object0.kinematics.pose_with_covariance.pose.orientation, + object1.kinematics.pose_with_covariance.pose.orientation, false); + if (std::fabs(max_rad) < M_PI && std::fabs(max_rad) < std::fabs(angle)) passed_gate = false; + } + // 2d iou gate + if (passed_gate) { + const double min_iou = min_iou_matrix_(object1_label, object0_label); + const double min_union_iou_area = 1e-2; + const double iou = object_recognition_utils::get2dIoU(object0, object1, min_union_iou_area); + if (iou < min_iou) passed_gate = false; + } + // max velocity diff gate + if (passed_gate) { + const double max_velocity_diff = max_velocity_diff_matrix_(object1_label, object0_label); + // calc absolute velocity diff (only thinking about speed) + const auto speed0 = std::hypot( + object0.kinematics.twist_with_covariance.twist.linear.x, + object0.kinematics.twist_with_covariance.twist.linear.y); + const auto speed1 = std::hypot( + object1.kinematics.twist_with_covariance.twist.linear.x, + object1.kinematics.twist_with_covariance.twist.linear.y); + const double velocity_diff = std::fabs(speed0 - speed1); + if (max_velocity_diff < velocity_diff) passed_gate = false; + } + + // all gate is passed + if (passed_gate) { + score = (max_dist - std::min(dist, max_dist)) / max_dist; + if (score < score_threshold_) score = 0.0; + } + } + return score; +} diff --git a/perception/tracking_object_merger/src/data_association/mu_successive_shortest_path/mu_successive_shortest_path_wrapper.cpp b/perception/tracking_object_merger/src/data_association/mu_successive_shortest_path/mu_successive_shortest_path_wrapper.cpp new file mode 100644 index 0000000000000..fcc79c0a3cbd7 --- /dev/null +++ b/perception/tracking_object_merger/src/data_association/mu_successive_shortest_path/mu_successive_shortest_path_wrapper.cpp @@ -0,0 +1,41 @@ +// Copyright 2023 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 "tracking_object_merger/data_association/solver/mu_successive_shortest_path.hpp" + +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace gnn_solver +{ +void MuSSP::maximizeLinearAssignment( + const std::vector> & cost, std::unordered_map * direct_assignment, + std::unordered_map * reverse_assignment) +{ + // Terminate if the graph is empty + if (cost.size() == 0 || cost.at(0).size() == 0) { + return; + } + + // Solve DA by muSSP + solve_muSSP(cost, direct_assignment, reverse_assignment); +} +} // namespace gnn_solver diff --git a/perception/tracking_object_merger/src/data_association/successive_shortest_path/successive_shortest_path.cpp b/perception/tracking_object_merger/src/data_association/successive_shortest_path/successive_shortest_path.cpp new file mode 100644 index 0000000000000..133f0d377373f --- /dev/null +++ b/perception/tracking_object_merger/src/data_association/successive_shortest_path/successive_shortest_path.cpp @@ -0,0 +1,370 @@ +// Copyright 2023 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 "tracking_object_merger/data_association/solver/successive_shortest_path.hpp" + +#include +#include +#include +#include +#include +#include +#include + +namespace gnn_solver +{ +struct ResidualEdge +{ + // Destination node + const int dst; + int capacity; + const double cost; + int flow; + // Access to the reverse edge by adjacency_list.at(dst).at(reverse) + const int reverse; + + // ResidualEdge() + // : dst(0), capacity(0), cost(0), flow(0), reverse(0) {} + + ResidualEdge(int dst, int capacity, double cost, int flow, int reverse) + : dst(dst), capacity(capacity), cost(cost), flow(flow), reverse(reverse) + { + } +}; + +void SSP::maximizeLinearAssignment( + const std::vector> & cost, std::unordered_map * direct_assignment, + std::unordered_map * reverse_assignment) +{ + // NOTE: Need to set as default arguments + bool sparse_cost = true; + // bool sparse_cost = false; + + // Hyperparameters + // double MAX_COST = 6; + const double MAX_COST = 10; + const double INF_DIST = 10000000; + const double EPS = 1e-5; + + // When there is no agents or no tasks, terminate + if (cost.size() == 0 || cost.at(0).size() == 0) { + return; + } + + // Construct a bipartite graph from the cost matrix + int n_agents = cost.size(); + int n_tasks = cost.at(0).size(); + + int n_dummies; + if (sparse_cost) { + n_dummies = n_agents; + } else { + n_dummies = 0; + } + + int source = 0; + int sink = n_agents + n_tasks + 1; + int n_nodes = n_agents + n_tasks + n_dummies + 2; + + // // Print cost matrix + // std::cout << std::endl; + // for (int agent = 0; agent < n_agents; agent++) + // { + // for (int task = 0; task < n_tasks; task++) + // { + // std::cout << cost.at(agent).at(task) << " "; + // } + // std::cout << std::endl; + // } + + // std::chrono::system_clock::time_point start_time, end_time; + // start_time = std::chrono::system_clock::now(); + + // Adjacency list of residual graph (index: nodes) + // - 0: source node + // - {1, ..., n_agents}: agent nodes + // - {n_agents+1, ..., n_agents+n_tasks}: task nodes + // - n_agents+n_tasks+1: sink node + // - {n_agents+n_tasks+2, ..., n_agents+n_tasks+1+n_agents}: + // dummy node (when sparse_cost is true) + std::vector> adjacency_list(n_nodes); + + // Reserve memory + for (int v = 0; v < n_nodes; ++v) { + if (v == source) { + // Source + adjacency_list.at(v).reserve(n_agents); + } else if (v <= n_agents) { + // Agents + adjacency_list.at(v).reserve(n_tasks + 1 + 1); + } else if (v <= n_agents + n_tasks) { + // Tasks + adjacency_list.at(v).reserve(n_agents + 1); + } else if (v == sink) { + // Sink + adjacency_list.at(v).reserve(n_tasks + n_dummies); + } else { + // Dummies + adjacency_list.at(v).reserve(2); + } + } + + // Add edges form source + for (int agent = 0; agent < n_agents; ++agent) { + // From source to agent + adjacency_list.at(source).emplace_back(agent + 1, 1, 0, 0, adjacency_list.at(agent + 1).size()); + // From agent to source + adjacency_list.at(agent + 1).emplace_back( + source, 0, 0, 0, adjacency_list.at(source).size() - 1); + } + + // Add edges from agents + for (int agent = 0; agent < n_agents; ++agent) { + for (int task = 0; task < n_tasks; ++task) { + if (!sparse_cost || cost.at(agent).at(task) > EPS) { + // From agent to task + adjacency_list.at(agent + 1).emplace_back( + task + n_agents + 1, 1, MAX_COST - cost.at(agent).at(task), 0, + adjacency_list.at(task + n_agents + 1).size()); + + // From task to agent + adjacency_list.at(task + n_agents + 1) + .emplace_back( + agent + 1, 0, cost.at(agent).at(task) - MAX_COST, 0, + adjacency_list.at(agent + 1).size() - 1); + } + } + } + + // Add edges form tasks + for (int task = 0; task < n_tasks; ++task) { + // From task to sink + adjacency_list.at(task + n_agents + 1) + .emplace_back(sink, 1, 0, 0, adjacency_list.at(sink).size()); + + // From sink to task + adjacency_list.at(sink).emplace_back( + task + n_agents + 1, 0, 0, 0, adjacency_list.at(task + n_agents + 1).size() - 1); + } + + // Add edges from dummy + if (sparse_cost) { + for (int agent = 0; agent < n_agents; ++agent) { + // From agent to dummy + adjacency_list.at(agent + 1).emplace_back( + agent + n_agents + n_tasks + 2, 1, MAX_COST, 0, + adjacency_list.at(agent + n_agents + n_tasks + 2).size()); + + // From dummy to agent + adjacency_list.at(agent + n_agents + n_tasks + 2) + .emplace_back(agent + 1, 0, -MAX_COST, 0, adjacency_list.at(agent + 1).size() - 1); + + // From dummy to sink + adjacency_list.at(agent + n_agents + n_tasks + 2) + .emplace_back(sink, 1, 0, 0, adjacency_list.at(sink).size()); + + // From sink to dummy + adjacency_list.at(sink).emplace_back( + agent + n_agents + n_tasks + 2, 0, 0, 0, + adjacency_list.at(agent + n_agents + n_tasks + 2).size() - 1); + } + } + + // Maximum flow value + const int max_flow = std::min(n_agents, n_tasks); + + // Feasible potentials + std::vector potentials(n_nodes, 0); + + // Shortest path lengths + std::vector distances(n_nodes, INF_DIST); + + // Whether previously visited the node or not + std::vector is_visited(n_nodes, false); + + // Parent node () + std::vector> prev_values(n_nodes); + + for (int i = 0; i < max_flow; ++i) { + // Initialize priority queue () + std::priority_queue< + std::pair, std::vector>, + std::greater>> + p_queue; + + // Reset all trajectory states + if (i > 0) { + std::fill(distances.begin(), distances.end(), INF_DIST); + std::fill(is_visited.begin(), is_visited.end(), false); + } + + // Start trajectory from the source node + p_queue.push(std::make_pair(0, source)); + distances.at(source) = 0; + + while (!p_queue.empty()) { + // Get the next element + std::pair cur_elem = p_queue.top(); + // std::cout << "[pop]: (" << cur_elem.first << ", " << cur_elem.second << ")" << std::endl; + p_queue.pop(); + + double cur_node_dist = cur_elem.first; + int cur_node = cur_elem.second; + + // If already visited node, skip and continue + if (is_visited.at(cur_node)) { + continue; + } + assert(cur_node_dist == distances.at(cur_node)); + + // Mark as visited + is_visited.at(cur_node) = true; + // Update potential + potentials.at(cur_node) += cur_node_dist; + + // When reached to the sink node, terminate. + if (cur_node == sink) { + break; + } + + // Loop over the incident nodes(/edges) + for (auto it_incident_edge = adjacency_list.at(cur_node).cbegin(); + it_incident_edge != adjacency_list.at(cur_node).cend(); it_incident_edge++) { + // If the node is not visited and have capacity to increase flow, visit. + if (!is_visited.at(it_incident_edge->dst) && it_incident_edge->capacity > 0) { + // Calculate reduced cost + double reduced_cost = + it_incident_edge->cost + potentials.at(cur_node) - potentials.at(it_incident_edge->dst); + assert(reduced_cost >= 0); + if (distances.at(it_incident_edge->dst) > reduced_cost) { + distances.at(it_incident_edge->dst) = reduced_cost; + prev_values.at(it_incident_edge->dst) = + std::make_pair(cur_node, it_incident_edge - adjacency_list.at(cur_node).cbegin()); + // std::cout << "[push]: (" << reduced_cost << ", " << next_v << ")" << std::endl; + p_queue.push(std::make_pair(reduced_cost, it_incident_edge->dst)); + } + } + } + } + + // Shortest path length to sink is greater than MAX_COST, + // which means no non-dummy routes left, terminate + if (potentials.at(sink) >= MAX_COST) { + break; + } + + // Update potentials of unvisited nodes + for (int v = 0; v < n_nodes; ++v) { + if (!is_visited.at(v)) { + potentials.at(v) += distances.at(sink); + } + } + // //Print potentials + // for (int v = 0; v < n_nodes; ++v) + // { + // std::cout << potentials.at(v) << ", "; + // } + // std::cout << std::endl; + + // Increase/decrease flow and capacity along the shortest path from the source to the sink + int v = sink; + int prev_v; + while (v != source) { + ResidualEdge & e_forward = + adjacency_list.at(prev_values.at(v).first).at(prev_values.at(v).second); + assert(e_forward.dst == v); + ResidualEdge & e_backward = adjacency_list.at(v).at(e_forward.reverse); + prev_v = e_backward.dst; + + if (e_backward.flow == 0) { + // Increase flow + // State A + assert(e_forward.capacity == 1); + assert(e_forward.flow == 0); + assert(e_backward.capacity == 0); + assert(e_backward.flow == 0); + + e_forward.capacity -= 1; + e_forward.flow += 1; + e_backward.capacity += 1; + + // State B + assert(e_forward.capacity == 0); + assert(e_forward.flow == 1); + assert(e_backward.capacity == 1); + assert(e_backward.flow == 0); + } else { + // Decrease flow + // State B + assert(e_forward.capacity == 1); + assert(e_forward.flow == 0); + assert(e_backward.capacity == 0); + assert(e_backward.flow == 1); + + e_forward.capacity -= 1; + e_backward.capacity += 1; + e_backward.flow -= 1; + + // State A + assert(e_forward.capacity == 0); + assert(e_forward.flow == 0); + assert(e_backward.capacity == 1); + assert(e_backward.flow == 0); + } + + v = prev_v; + } + +#ifndef NDEBUG + // Check if the potentials are feasible potentials + for (int v = 0; v < n_nodes; ++v) { + for (auto it_incident_edge = adjacency_list.at(v).cbegin(); + it_incident_edge != adjacency_list.at(v).cend(); ++it_incident_edge) { + if (it_incident_edge->capacity > 0) { + double reduced_cost = + it_incident_edge->cost + potentials.at(v) - potentials.at(it_incident_edge->dst); + assert(reduced_cost >= 0); + } + } + } +#endif + } + + // Output + for (int agent = 0; agent < n_agents; ++agent) { + for (auto it_incident_edge = adjacency_list.at(agent + 1).cbegin(); + it_incident_edge != adjacency_list.at(agent + 1).cend(); ++it_incident_edge) { + int task = it_incident_edge->dst - (n_agents + 1); + + // If the flow value is 1 and task is not dummy, assign the task to the agent. + if (it_incident_edge->flow == 1 && 0 <= task && task < n_tasks) { + (*direct_assignment)[agent] = task; + (*reverse_assignment)[task] = agent; + break; + } + } + } + +#ifndef NDEBUG + // Check if the result is valid assignment + for (int agent = 0; agent < n_agents; ++agent) { + if (direct_assignment->find(agent) != direct_assignment->cend()) { + int task = (*direct_assignment).at(agent); + assert(direct_assignment->at(agent) == task); + assert(reverse_assignment->at(task) == agent); + } + } +#endif +} +} // namespace gnn_solver diff --git a/perception/tracking_object_merger/src/decorative_tracker_merger.cpp b/perception/tracking_object_merger/src/decorative_tracker_merger.cpp new file mode 100644 index 0000000000000..47a333691eabf --- /dev/null +++ b/perception/tracking_object_merger/src/decorative_tracker_merger.cpp @@ -0,0 +1,403 @@ +// Copyright 2023 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 "tracking_object_merger/decorative_tracker_merger.hpp" + +#include "object_recognition_utils/object_recognition_utils.hpp" +#include "tracking_object_merger/data_association/solver/successive_shortest_path.hpp" +#include "tracking_object_merger/utils/utils.hpp" + +#include + +#include +#include + +#define EIGEN_MPL2_ONLY +#include +#include + +using Label = autoware_auto_perception_msgs::msg::ObjectClassification; + +namespace tracking_object_merger +{ + +using autoware_auto_perception_msgs::msg::TrackedObject; +using autoware_auto_perception_msgs::msg::TrackedObjects; + +// get unix time from header +double getUnixTime(const std_msgs::msg::Header & header) +{ + return header.stamp.sec + header.stamp.nanosec * 1e-9; +} + +// calc association score matrix +Eigen::MatrixXd calcScoreMatrixForAssociation( + const MEASUREMENT_STATE measurement_state, + const autoware_auto_perception_msgs::msg::TrackedObjects & objects0, + const std::vector & trackers, + const std::unordered_map> & data_association_map + // const bool debug_log, const std::string & file_name // do not logging for now +) +{ + // get current time + const rclcpp::Time current_time = rclcpp::Time(objects0.header.stamp); + + // calc score matrix + Eigen::MatrixXd score_matrix = Eigen::MatrixXd::Zero(trackers.size(), objects0.objects.size()); + for (size_t trackers_idx = 0; trackers_idx < trackers.size(); ++trackers_idx) { + const auto & tracker_obj = trackers.at(trackers_idx); + const auto & object1 = tracker_obj.getObject(); + const auto & tracker_state = tracker_obj.getCurrentMeasurementState(current_time); + + for (size_t objects0_idx = 0; objects0_idx < objects0.objects.size(); ++objects0_idx) { + const auto & object0 = objects0.objects.at(objects0_idx); + // switch calc score function by input and trackers measurement state + // we assume that lidar and radar are exclusive + double score; + const auto input_has_lidar = measurement_state & MEASUREMENT_STATE::LIDAR; + const auto tracker_has_lidar = tracker_state & MEASUREMENT_STATE::LIDAR; + if (input_has_lidar && tracker_has_lidar) { + score = data_association_map.at("lidar-lidar")->calcScoreBetweenObjects(object0, object1); + } else if (!input_has_lidar && !tracker_has_lidar) { + score = data_association_map.at("radar-radar")->calcScoreBetweenObjects(object0, object1); + } else { + score = data_association_map.at("lidar-radar")->calcScoreBetweenObjects(object0, object1); + } + score_matrix(trackers_idx, objects0_idx) = score; + } + } + return score_matrix; +} + +DecorativeTrackerMergerNode::DecorativeTrackerMergerNode(const rclcpp::NodeOptions & node_options) +: rclcpp::Node("decorative_object_merger_node", node_options), + tf_buffer_(get_clock()), + tf_listener_(tf_buffer_) +{ + // Subscriber + sub_main_objects_ = create_subscription( + "input/main_object", rclcpp::QoS{1}, + std::bind(&DecorativeTrackerMergerNode::mainObjectsCallback, this, std::placeholders::_1)); + sub_sub_objects_ = create_subscription( + "input/sub_object", rclcpp::QoS{1}, + std::bind(&DecorativeTrackerMergerNode::subObjectsCallback, this, std::placeholders::_1)); + + // merged object publisher + merged_object_pub_ = create_publisher("output/object", rclcpp::QoS{1}); + // debug object publisher + debug_object_pub_ = + create_publisher("debug/interpolated_sub_object", rclcpp::QoS{1}); + + // logging + logging_.enable = declare_parameter("enable_logging"); + logging_.path = declare_parameter("logging_file_path"); + + // Parameters + publish_interpolated_sub_objects_ = declare_parameter("publish_interpolated_sub_objects"); + base_link_frame_id_ = declare_parameter("base_link_frame_id"); + time_sync_threshold_ = declare_parameter("time_sync_threshold"); + sub_object_timeout_sec_ = declare_parameter("sub_object_timeout_sec"); + // default setting parameter for tracker + tracker_state_parameter_.remove_probability_threshold = + declare_parameter("tracker_state_parameter.remove_probability_threshold"); + tracker_state_parameter_.publish_probability_threshold = + declare_parameter("tracker_state_parameter.publish_probability_threshold"); + tracker_state_parameter_.default_lidar_existence_probability = + declare_parameter("tracker_state_parameter.default_lidar_existence_probability"); + tracker_state_parameter_.default_radar_existence_probability = + declare_parameter("tracker_state_parameter.default_radar_existence_probability"); + tracker_state_parameter_.default_camera_existence_probability = + declare_parameter("tracker_state_parameter.default_camera_existence_probability"); + tracker_state_parameter_.decay_rate = + declare_parameter("tracker_state_parameter.decay_rate"); + tracker_state_parameter_.max_dt = declare_parameter("tracker_state_parameter.max_dt"); + + const std::string main_sensor_type = declare_parameter("main_sensor_type"); + const std::string sub_sensor_type = declare_parameter("sub_sensor_type"); + // str to MEASUREMENT_STATE + auto str2measurement_state = [](const std::string & str) { + if (str == "lidar") { + return MEASUREMENT_STATE::LIDAR; + } else if (str == "radar") { + return MEASUREMENT_STATE::RADAR; + } else { + throw std::runtime_error("invalid sensor type"); + } + }; + main_sensor_type_ = str2measurement_state(main_sensor_type); + sub_sensor_type_ = str2measurement_state(sub_sensor_type); + + /* init association **/ + // lidar-lidar association matrix + set3dDataAssociation("lidar-lidar", data_association_map_); + // lidar-radar association matrix + set3dDataAssociation("lidar-radar", data_association_map_); + // radar-radar association matrix + set3dDataAssociation("radar-radar", data_association_map_); +} + +void DecorativeTrackerMergerNode::set3dDataAssociation( + const std::string & prefix, + std::unordered_map> & data_association_map) +{ + const auto tmp = this->declare_parameter>(prefix + ".can_assign_matrix"); + const std::vector can_assign_matrix(tmp.begin(), tmp.end()); + const auto max_dist_matrix = + this->declare_parameter>(prefix + ".max_dist_matrix"); + const auto max_rad_matrix = + this->declare_parameter>(prefix + ".max_rad_matrix"); + const auto min_iou_matrix = + this->declare_parameter>(prefix + ".min_iou_matrix"); + const auto max_velocity_diff_matrix = + this->declare_parameter>(prefix + ".max_velocity_diff_matrix"); + + data_association_map[prefix] = std::make_unique( + can_assign_matrix, max_dist_matrix, max_rad_matrix, min_iou_matrix, max_velocity_diff_matrix); +} + +/** + * @brief callback function for main objects + * + * @param main_objects + * @note if there are no sub objects, publish main objects as it is + * else, merge main objects and sub objects + */ +void DecorativeTrackerMergerNode::mainObjectsCallback( + const TrackedObjects::ConstSharedPtr & main_objects) +{ + // try to merge sub object + if (!sub_objects_buffer_.empty()) { + // get interpolated sub objects + // get newest sub objects which timestamp is earlier to main objects + TrackedObjects::ConstSharedPtr closest_time_sub_objects; + TrackedObjects::ConstSharedPtr closest_time_sub_objects_later; + for (const auto & sub_object : sub_objects_buffer_) { + if (getUnixTime(sub_object->header) < getUnixTime(main_objects->header)) { + closest_time_sub_objects = sub_object; + } else { + closest_time_sub_objects_later = sub_object; + break; + } + } + // get delay compensated sub objects + const auto interpolated_sub_objects = interpolateObjectState( + closest_time_sub_objects, closest_time_sub_objects_later, main_objects->header); + if (interpolated_sub_objects.has_value()) { + // Merge sub objects + const auto interp_sub_objs = interpolated_sub_objects.value(); + debug_object_pub_->publish(interp_sub_objs); + this->decorativeMerger( + sub_sensor_type_, std::make_shared(interpolated_sub_objects.value())); + } else { + RCLCPP_DEBUG(this->get_logger(), "interpolated_sub_objects is null"); + } + } + + // try to merge main object + this->decorativeMerger(main_sensor_type_, main_objects); + + merged_object_pub_->publish(getTrackedObjects(main_objects->header)); +} + +/** + * @brief callback function for sub objects + * + * @param msg + * @note push back sub objects to buffer and remove old sub objects + */ +void DecorativeTrackerMergerNode::subObjectsCallback(const TrackedObjects::ConstSharedPtr & msg) +{ + sub_objects_buffer_.push_back(msg); + // remove old sub objects + // const auto now = get_clock()->now(); + const auto now = rclcpp::Time(msg->header.stamp); + const auto remove_itr = std::remove_if( + sub_objects_buffer_.begin(), sub_objects_buffer_.end(), [now, this](const auto & sub_object) { + return (now - rclcpp::Time(sub_object->header.stamp)).seconds() > sub_object_timeout_sec_; + }); + sub_objects_buffer_.erase(remove_itr, sub_objects_buffer_.end()); +} + +/** + * @brief merge objects into inner_tracker_objects_ + * + * @param main_objects + * @return TrackedObjects + * + * @note 1. interpolate sub objects to sync main objects + * 2. do association + * 3. merge objects + */ +bool DecorativeTrackerMergerNode::decorativeMerger( + const MEASUREMENT_STATE input_sensor, const TrackedObjects::ConstSharedPtr & input_objects_msg) +{ + // get current time + const auto current_time = rclcpp::Time(input_objects_msg->header.stamp); + if (input_objects_msg->objects.empty()) { + return false; + } + if (inner_tracker_objects_.empty()) { + for (const auto & object : input_objects_msg->objects) { + inner_tracker_objects_.push_back(createNewTracker(input_sensor, current_time, object)); + } + } + + // do prediction for inner objects + for (auto & object : inner_tracker_objects_) { + object.predict(current_time); + } + + // TODO(yoshiri): pre-association + + // associate inner objects and input objects + /* global nearest neighbor */ + std::unordered_map direct_assignment, reverse_assignment; + const auto & objects1 = input_objects_msg->objects; + Eigen::MatrixXd score_matrix = calcScoreMatrixForAssociation( + input_sensor, *input_objects_msg, inner_tracker_objects_, data_association_map_); + data_association_map_.at("lidar-lidar") + ->assign(score_matrix, direct_assignment, reverse_assignment); + + // look for tracker + for (int tracker_idx = 0; tracker_idx < static_cast(inner_tracker_objects_.size()); + ++tracker_idx) { + auto & object0_state = inner_tracker_objects_.at(tracker_idx); + if (direct_assignment.find(tracker_idx) != direct_assignment.end()) { // found and merge + const auto & object1 = objects1.at(direct_assignment.at(tracker_idx)); + // merge object1 into object0_state + object0_state.updateState(input_sensor, current_time, object1); + } else { // not found + // decrease existence probability + object0_state.updateWithoutSensor(current_time); + } + } + // look for new object + for (int object1_idx = 0; object1_idx < static_cast(objects1.size()); ++object1_idx) { + const auto & object1 = objects1.at(object1_idx); + if (reverse_assignment.find(object1_idx) != reverse_assignment.end()) { // found + } else { // not found + inner_tracker_objects_.push_back(createNewTracker(input_sensor, current_time, object1)); + } + } + return true; +} + +/** + * @brief interpolate sub objects to sync main objects + * + * @param former_msg + * @param latter_msg + * @param output_header + * @return std::optional + * + * @note 1. if both msg is nullptr, return null optional + * 2. if former_msg is nullptr, return latter_msg + * 3. if latter_msg is nullptr, return former_msg + * 4. if both msg is not nullptr, do the interpolation + */ +std::optional DecorativeTrackerMergerNode::interpolateObjectState( + const TrackedObjects::ConstSharedPtr & former_msg, + const TrackedObjects::ConstSharedPtr & latter_msg, const std_msgs::msg::Header & output_header) +{ + // Assumption: output_header must be newer than former_msg and older than latter_msg + // There three possible patterns + // 1. both msg is nullptr + // 2. former_msg is nullptr + // 3. latter_msg is nullptr + // 4. both msg is not nullptr + + // 1. both msg is nullptr + if (former_msg == nullptr && latter_msg == nullptr) { + // return null optional + RCLCPP_DEBUG(this->get_logger(), "interpolation err: both msg is nullptr"); + return std::nullopt; + } + + // 2. former_msg is nullptr + if (former_msg == nullptr) { + // std::cout << "interpolation: 2. former_msg is nullptr" << std::endl; + // depends on header stamp difference + if ( + (rclcpp::Time(latter_msg->header.stamp) - rclcpp::Time(output_header.stamp)).seconds() > + time_sync_threshold_) { + // do nothing + RCLCPP_DEBUG( + this->get_logger(), "interpolation err: latter msg is too different from output msg"); + return std::nullopt; + } else { // else, return latter_msg + return *latter_msg; + } + + // 3. latter_msg is nullptr + } else if (latter_msg == nullptr) { + // std::cout << "interpolation: 3. latter_msg is nullptr" << std::endl; + // depends on header stamp difference + const auto dt = + (rclcpp::Time(output_header.stamp) - rclcpp::Time(former_msg->header.stamp)).seconds(); + if (dt > time_sync_threshold_) { + // do nothing + RCLCPP_DEBUG(this->get_logger(), "interpolation err: former msg is too old"); + return std::nullopt; + } else { + // else, return former_msg + return utils::predictPastOrFutureTrackedObjects(*former_msg, output_header); + } + + // 4. both msg is not nullptr + } else { + // do the interpolation + // std::cout << "interpolation: 4. both msg is not nullptr" << std::endl; + TrackedObjects interpolated_msg = + utils::interpolateTrackedObjects(*former_msg, *latter_msg, output_header); + return interpolated_msg; + } +} + +// get merged objects +TrackedObjects DecorativeTrackerMergerNode::getTrackedObjects(const std_msgs::msg::Header & header) +{ + // get main objects + rclcpp::Time current_time = rclcpp::Time(header.stamp); + return getTrackedObjectsFromTrackerStates(inner_tracker_objects_, current_time); +} + +// create new tracker +TrackerState DecorativeTrackerMergerNode::createNewTracker( + const MEASUREMENT_STATE input_index, rclcpp::Time current_time, + const autoware_auto_perception_msgs::msg::TrackedObject & input_object) +{ + // check if object id is not included in innner_tracker_objects_ + for (const auto & object : inner_tracker_objects_) { + if (object.const_uuid_ == input_object.object_id) { + // create new uuid + unique_identifier_msgs::msg::UUID uuid; + // Generate random number + std::mt19937 gen(std::random_device{}()); + std::independent_bits_engine bit_eng(gen); + std::generate(uuid.uuid.begin(), uuid.uuid.end(), bit_eng); + auto new_tracker = TrackerState(input_index, current_time, input_object, uuid); + new_tracker.setParameter(tracker_state_parameter_); + return new_tracker; + } + } + // if not found, just create new tracker + auto new_tracker = TrackerState(input_index, current_time, input_object); + new_tracker.setParameter(tracker_state_parameter_); + return new_tracker; +} + +} // namespace tracking_object_merger + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(tracking_object_merger::DecorativeTrackerMergerNode) diff --git a/perception/tracking_object_merger/src/utils/tracker_state.cpp b/perception/tracking_object_merger/src/utils/tracker_state.cpp new file mode 100644 index 0000000000000..f0dc657145a24 --- /dev/null +++ b/perception/tracking_object_merger/src/utils/tracker_state.cpp @@ -0,0 +1,321 @@ +// Copyright 2023 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 "tracking_object_merger/utils/tracker_state.hpp" + +#include "tracking_object_merger/utils/utils.hpp" + +using autoware_auto_perception_msgs::msg::TrackedObject; +using autoware_auto_perception_msgs::msg::TrackedObjects; + +/** + * @brief Construct a new Tracker State:: Tracker State object + * + * @param input_source : input source distinguished + * @param tracked_object : input source tracked object + * @param last_update_time : last update time + */ +TrackerState::TrackerState( + const MEASUREMENT_STATE input_source, const rclcpp::Time & last_update_time, + const autoware_auto_perception_msgs::msg::TrackedObject & tracked_object) +: tracked_object_(tracked_object), + last_update_time_(last_update_time), + const_uuid_(tracked_object.object_id), + measurement_state_(input_source) +{ + input_uuid_map_[input_source] = tracked_object_.object_id; + last_updated_time_map_[input_source] = last_update_time; + existence_probability_ = default_existence_probability_map_[input_source]; +} + +TrackerState::TrackerState( + const MEASUREMENT_STATE input_source, const rclcpp::Time & last_update_time, + const autoware_auto_perception_msgs::msg::TrackedObject & tracked_object, + const unique_identifier_msgs::msg::UUID & uuid) +: tracked_object_(tracked_object), + last_update_time_(last_update_time), + const_uuid_(uuid), + measurement_state_(input_source) +{ + input_uuid_map_[input_source] = tracked_object_.object_id; + last_updated_time_map_[input_source] = last_update_time; + existence_probability_ = default_existence_probability_map_[input_source]; +} + +void TrackerState::setParameter(const TrackerStateParameter & parameter) +{ + max_dt_ = parameter.max_dt; + publish_probability_threshold_ = parameter.publish_probability_threshold; + remove_probability_threshold_ = parameter.remove_probability_threshold; + default_existence_probability_map_.at(MEASUREMENT_STATE::LIDAR) = + parameter.default_lidar_existence_probability; + default_existence_probability_map_.at(MEASUREMENT_STATE::RADAR) = + parameter.default_radar_existence_probability; + default_existence_probability_map_.at(MEASUREMENT_STATE::CAMERA) = + parameter.default_camera_existence_probability; + decay_rate_ = parameter.decay_rate; +} + +/** + * @brief Predict state to current time + * + * @param current_time + * @return true + * @return false + */ +bool TrackerState::predict(const rclcpp::Time & current_time) +{ + // get dt and give warning if dt is too large + double dt = (current_time - last_update_time_).seconds(); + if (std::abs(dt) > max_dt_) { + std::cerr << "[tracking_object_merger] dt is too large: " << dt << std::endl; + return false; + } + + // do prediction + last_update_time_ = current_time; + return this->predict(dt, utils::predictPastOrFutureTrackedObject); +} + +/** + * @brief Predict state to current time with given update function + * + * @param dt : time to predict + * @param func : update function (e.g. PredictPastOrFutureTrackedObject) + * @return true: prediction succeeded + * @return false: prediction failed + */ +bool TrackerState::predict( + const double dt, std::function func) +{ + const auto predicted_object = func(tracked_object_, dt); + tracked_object_ = predicted_object; + return true; +} + +// get current measurement state +MEASUREMENT_STATE TrackerState::getCurrentMeasurementState(const rclcpp::Time & current_time) const +{ + MEASUREMENT_STATE measurement_state = MEASUREMENT_STATE::NONE; + // check LIDAR + if (last_updated_time_map_.find(MEASUREMENT_STATE::LIDAR) != last_updated_time_map_.end()) { + if ((current_time - last_updated_time_map_.at(MEASUREMENT_STATE::LIDAR)).seconds() < max_dt_) { + measurement_state = measurement_state | MEASUREMENT_STATE::LIDAR; + } + } + // check RADAR + if (last_updated_time_map_.find(MEASUREMENT_STATE::RADAR) != last_updated_time_map_.end()) { + if ((current_time - last_updated_time_map_.at(MEASUREMENT_STATE::RADAR)).seconds() < max_dt_) { + measurement_state = measurement_state | MEASUREMENT_STATE::RADAR; + } + } + // check CAMERA + if (last_updated_time_map_.find(MEASUREMENT_STATE::CAMERA) != last_updated_time_map_.end()) { + if ((current_time - last_updated_time_map_.at(MEASUREMENT_STATE::CAMERA)).seconds() < max_dt_) { + measurement_state = measurement_state | MEASUREMENT_STATE::CAMERA; + } + } + return measurement_state; +} + +bool TrackerState::updateState( + const MEASUREMENT_STATE input, const rclcpp::Time & current_time, + const TrackedObject & tracked_object) +{ + // calc dt + double dt = (current_time - last_update_time_).seconds(); + if (dt < 0.0) { + std::cerr << "[tracking_object_merger] dt is negative: " << dt << std::endl; + return false; + } + + // predict + if (dt > 0.0) { + this->predict(dt, utils::predictPastOrFutureTrackedObject); + } + + // get current measurement state + measurement_state_ = getCurrentMeasurementState(current_time); + + // update with input + if (input & MEASUREMENT_STATE::LIDAR) { + updateWithLIDAR(current_time, tracked_object); + } + if (input & MEASUREMENT_STATE::RADAR) { + updateWithRADAR(current_time, tracked_object); + } + if (input & MEASUREMENT_STATE::CAMERA) { + updateWithCAMERA(current_time, tracked_object); + } + return true; +} + +void TrackerState::updateWithCAMERA( + const rclcpp::Time & current_time, const TrackedObject & tracked_object) +{ + // update tracked object + updateWithFunction( + MEASUREMENT_STATE::CAMERA, current_time, tracked_object, + merger_utils::updateOnlyClassification); +} + +void TrackerState::updateWithLIDAR( + const rclcpp::Time & current_time, const TrackedObject & tracked_object) +{ + // if radar is available, do not update velocity + if (measurement_state_ & MEASUREMENT_STATE::RADAR) { + // update tracked object + updateWithFunction( + MEASUREMENT_STATE::LIDAR, current_time, tracked_object, merger_utils::updateExceptVelocity); + } else { + // else just update tracked object + updateWithFunction( + MEASUREMENT_STATE::LIDAR, current_time, tracked_object, + merger_utils::updateWholeTrackedObject); + } +} + +void TrackerState::updateWithRADAR( + const rclcpp::Time & current_time, const TrackedObject & tracked_object) +{ + // if lidar is available, update only velocity + if (measurement_state_ & MEASUREMENT_STATE::LIDAR) { + // update tracked object + updateWithFunction( + MEASUREMENT_STATE::RADAR, current_time, tracked_object, + merger_utils::updateOnlyObjectVelocity); + } else { + // else just update tracked object + updateWithFunction( + MEASUREMENT_STATE::RADAR, current_time, tracked_object, + merger_utils::updateWholeTrackedObject); + } +} + +bool TrackerState::updateWithFunction( + const MEASUREMENT_STATE input, const rclcpp::Time & current_time, + const TrackedObject & input_tracked_object, + std::function update_func) +{ + // put input uuid and last update time + if (current_time > last_update_time_) { + const auto predict_successful = predict(current_time); + if (!predict_successful) { + return false; + } + } + + // update with measurement state + last_updated_time_map_[input] = current_time; + input_uuid_map_[input] = input_tracked_object.object_id; + measurement_state_ = measurement_state_ | input; + existence_probability_ = + std::max(existence_probability_, default_existence_probability_map_[input]); + + // update tracked object + update_func(tracked_object_, input_tracked_object); + tracked_object_.object_id = const_uuid_; // overwrite uuid to stay same + return true; +} + +void TrackerState::updateWithoutSensor(const rclcpp::Time & current_time) +{ + // calc dt + double dt = (current_time - last_update_time_).seconds(); + if (dt < 0) { + std::cerr << "[tracking_object_merger] dt is negative: " << dt << std::endl; + return; + } + + // predict + if (dt > 0.0) { + const auto predict_successful = this->predict(dt, utils::predictPastOrFutureTrackedObject); + if (!predict_successful) { + existence_probability_ = 0.0; // remove object + } + } + + // reduce probability + existence_probability_ -= decay_rate_; + if (existence_probability_ < 0.0) { + existence_probability_ = 0.0; + } +} + +TrackedObject TrackerState::getObject() const +{ + TrackedObject tracked_object = tracked_object_; + tracked_object.object_id = const_uuid_; + tracked_object.existence_probability = + static_cast(existence_probability_); // double -> float + return tracked_object; +} + +bool TrackerState::hasUUID( + const MEASUREMENT_STATE input, const unique_identifier_msgs::msg::UUID & uuid) const +{ + if (input_uuid_map_.find(input) == input_uuid_map_.end()) { + return false; + } + return input_uuid_map_.at(input) == uuid; +} + +bool TrackerState::isValid() const +{ + // check if tracker state is valid + if (existence_probability_ < remove_probability_threshold_) { + return false; + } + return true; +} + +bool TrackerState::canPublish() const +{ + // check if tracker state is valid + if (existence_probability_ < publish_probability_threshold_) { + return false; + } + return true; +} + +TrackerState::~TrackerState() +{ + // destructor +} + +TrackedObjects getTrackedObjectsFromTrackerStates( + std::vector & tracker_states, const rclcpp::Time & current_time) +{ + TrackedObjects tracked_objects; + + // sanitize and get tracked objects + for (auto it = tracker_states.begin(); it != tracker_states.end();) { + // check if tracker state is valid + if (it->isValid()) { + if (it->canPublish()) { + // if valid, get tracked object + tracked_objects.objects.push_back(it->getObject()); + } + ++it; + } else { + // if not valid, delete tracker state + it = tracker_states.erase(it); + } + } + + // update header + tracked_objects.header.stamp = current_time; + tracked_objects.header.frame_id = "map"; // TODO(yoshiri): get frame_id from input + return tracked_objects; +} diff --git a/perception/tracking_object_merger/src/utils/utils.cpp b/perception/tracking_object_merger/src/utils/utils.cpp new file mode 100644 index 0000000000000..5987bdc2d1bef --- /dev/null +++ b/perception/tracking_object_merger/src/utils/utils.cpp @@ -0,0 +1,403 @@ +// Copyright 2023 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 "tracking_object_merger/utils/utils.hpp" + +#include +#include +#include + +#include +#include +#include +#include + +using autoware_auto_perception_msgs::msg::TrackedObject; +using autoware_auto_perception_msgs::msg::TrackedObjects; +namespace utils +{ + +/** + * @brief linear interpolation for tracked object + * + * @param obj1 : obj1 and obj2 must have same shape type + * @param obj2 : obj1 and obj2 must have same shape type + * @param weight : 0 <= weight <= 1, weight = 0: obj1, weight = 1: obj2 + * @return TrackedObject : interpolated object + */ +TrackedObject linearInterpolationForTrackedObject( + const TrackedObject & obj1, const TrackedObject & obj2, const double weight) +{ + // interpolate obj1 and obj2 with weight: obj1 * (1 - weight) + obj2 * weight + // weight = 0: obj1, weight = 1: obj2 + + // weight check (0 <= weight <= 1) + if (weight < 0 || weight > 1) { + std::cerr << "weight must be 0 <= weight <= 1 in linearInterpolationForTrackedObject function." + << std::endl; + return obj1; + } + + // output object + TrackedObject output; + // copy input object at first + output = obj1; + + // get current twist and pose + const auto twist1 = obj1.kinematics.twist_with_covariance.twist; + const auto pose1 = obj1.kinematics.pose_with_covariance.pose; + const auto twist2 = obj2.kinematics.twist_with_covariance.twist; + const auto pose2 = obj2.kinematics.pose_with_covariance.pose; + + // interpolate twist + auto & output_twist = output.kinematics.twist_with_covariance.twist; + output_twist.linear.x = twist1.linear.x * (1 - weight) + twist2.linear.x * weight; + output_twist.linear.y = twist1.linear.y * (1 - weight) + twist2.linear.y * weight; + output_twist.linear.z = twist1.linear.z * (1 - weight) + twist2.linear.z * weight; + output_twist.angular.x = twist1.angular.x * (1 - weight) + twist2.angular.x * weight; + output_twist.angular.y = twist1.angular.y * (1 - weight) + twist2.angular.y * weight; + output_twist.angular.z = twist1.angular.z * (1 - weight) + twist2.angular.z * weight; + + // interpolate pose + auto & output_pose = output.kinematics.pose_with_covariance.pose; + output_pose.position.x = pose1.position.x * (1 - weight) + pose2.position.x * weight; + output_pose.position.y = pose1.position.y * (1 - weight) + pose2.position.y * weight; + output_pose.position.z = pose1.position.z * (1 - weight) + pose2.position.z * weight; + // interpolate orientation with slerp + // https://en.wikipedia.org/wiki/Slerp + const auto q1 = tf2::Quaternion( + pose1.orientation.x, pose1.orientation.y, pose1.orientation.z, pose1.orientation.w); + const auto q2 = tf2::Quaternion( + pose2.orientation.x, pose2.orientation.y, pose2.orientation.z, pose2.orientation.w); + const auto q = q1.slerp(q2, weight); + output_pose.orientation.x = q.x(); + output_pose.orientation.y = q.y(); + output_pose.orientation.z = q.z(); + output_pose.orientation.w = q.w(); + + // interpolate shape(mostly for bounding box) + const auto shape1 = obj1.shape; + const auto shape2 = obj2.shape; + if (shape1.type != shape2.type) { + // if shape type is different, return obj1 + } else { + if (shape1.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + auto & output_shape = output.shape; + output_shape.dimensions.x = shape1.dimensions.x * (1 - weight) + shape2.dimensions.x * weight; + output_shape.dimensions.y = shape1.dimensions.y * (1 - weight) + shape2.dimensions.y * weight; + output_shape.dimensions.z = shape1.dimensions.z * (1 - weight) + shape2.dimensions.z * weight; + } else if (shape1.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { + // (TODO) implement + } else if (shape1.type == autoware_auto_perception_msgs::msg::Shape::POLYGON) { + // (TODO) implement + } else { + // when type is unknown, print warning and do nothing + std::cerr << "unknown shape type in linearInterpolationForTrackedObject function." + << std::endl; + return output; + } + } + + return output; +} + +/** + * @brief predict past or future tracked object + * + * @param obj + * @param dt + * @return TrackedObject + */ +TrackedObject predictPastOrFutureTrackedObject(const TrackedObject & obj, const double dt) +{ + // output object + TrackedObject output; + // copy input object at first + output = obj; + if (dt == 0) { + return output; + } + + // get current twist and pose + const auto twist = obj.kinematics.twist_with_covariance.twist; + const auto pose = obj.kinematics.pose_with_covariance.pose; + + // get yaw + const auto yaw = tf2::getYaw(pose.orientation); + const auto vx = twist.linear.x; + const auto vy = twist.linear.y; + + // linear prediction equation: + // x = x0 + vx * cos(yaw) * dt - vy * sin(yaw) * dt + // y = y0 + vx * sin(yaw) * dt + vy * cos(yaw) * dt + auto & output_pose = output.kinematics.pose_with_covariance.pose; + output_pose.position.x += vx * std::cos(yaw) * dt - vy * std::sin(yaw) * dt; + output_pose.position.y += vx * std::sin(yaw) * dt + vy * std::cos(yaw) * dt; + + // (TODO) covariance prediction + + return output; +} + +/** + * @brief predict past or future tracked objects + * + * @param obj + * @param header + * @return TrackedObjects + */ +TrackedObjects predictPastOrFutureTrackedObjects( + const TrackedObjects & obj, const std_msgs::msg::Header & header) +{ + // for each object, predict past or future + TrackedObjects output_objects; + output_objects.header = obj.header; + output_objects.header.stamp = header.stamp; + + const auto dt = (rclcpp::Time(header.stamp) - rclcpp::Time(obj.header.stamp)).seconds(); + for (const auto & obj : obj.objects) { + output_objects.objects.push_back(predictPastOrFutureTrackedObject(obj, dt)); + } + return output_objects; +} + +/** + * @brief interpolate tracked objects + * + * @param objects1 + * @param objects2 + * @param header : header of output object + * @return TrackedObjects + */ +TrackedObjects interpolateTrackedObjects( + const TrackedObjects & objects1, const TrackedObjects & objects2, std_msgs::msg::Header header) +{ + // Assumption: time of output header is between objects1 and objects2 + // time of objects1 < header < objects2 + + // define output objects + TrackedObjects output_objects; + output_objects.header = header; + + // calc weight + const auto dt = + (rclcpp::Time(objects1.header.stamp) - rclcpp::Time(objects2.header.stamp)).seconds(); + const auto dt1 = (rclcpp::Time(objects1.header.stamp) - rclcpp::Time(header.stamp)).seconds(); + const auto weight = dt1 / dt; + + // check if object number is not zero + if (objects1.objects.size() == 0 && objects2.objects.size() == 0) { + // if objects1 and objects2 are empty, return empty objects + return output_objects; + } else if (objects1.objects.size() == 0) { + // if objects1 is empty return objects2 + for (const auto & obj2 : objects2.objects) { + output_objects.objects.push_back(predictPastOrFutureTrackedObject(obj2, dt1 - dt)); + } + return output_objects; + } else if (objects2.objects.size() == 0) { + // if objects2 is empty return objects1 + for (const auto & obj1 : objects1.objects) { + output_objects.objects.push_back(predictPastOrFutureTrackedObject(obj1, dt1)); + } + } else { + // if both objects1 and objects2 are not empty do nothing + } + + // map to handle selected objects + std::unordered_map selected_objects1; + // iterate with int + for (std::size_t i = 0; i < objects1.objects.size(); i++) { + selected_objects1[i] = false; + } + // search for objects with int iterator + for (std::size_t i = 0; i < objects1.objects.size(); i++) { + for (std::size_t j = 0; j < objects2.objects.size(); j++) { + if (objects1.objects[i].object_id == objects2.objects[j].object_id) { + // if id is same, interpolate + const auto interp_objects = + linearInterpolationForTrackedObject(objects1.objects[i], objects2.objects[j], weight); + output_objects.objects.push_back(interp_objects); + selected_objects1[i] = true; + break; + } + } + if (selected_objects1[i] == false) { + // if obj1 is not selected, predict future + const auto pred_objects = predictPastOrFutureTrackedObject(objects1.objects[i], dt1); + output_objects.objects.push_back(pred_objects); + } + } + + return output_objects; +} + +} // namespace utils + +namespace merger_utils +{ + +double mean(const double a, const double b) +{ + return (a + b) / 2.0; +} + +// object kinematics merger +// currently only support velocity fusion +autoware_auto_perception_msgs::msg::TrackedObjectKinematics objectKinematicsVXMerger( + const TrackedObject & main_obj, const TrackedObject & sub_obj, const MergePolicy policy) +{ + autoware_auto_perception_msgs::msg::TrackedObjectKinematics output_kinematics; + // copy main object at first + output_kinematics = main_obj.kinematics; + if (policy == MergePolicy::SKIP) { + return output_kinematics; + } else if (policy == MergePolicy::OVERWRITE) { + output_kinematics.twist_with_covariance.twist.linear.x = + sub_obj.kinematics.twist_with_covariance.twist.linear.x; + return output_kinematics; + } else if (policy == MergePolicy::FUSION) { + const auto main_vx = main_obj.kinematics.twist_with_covariance.twist.linear.x; + const auto sub_vx = sub_obj.kinematics.twist_with_covariance.twist.linear.x; + // inverse weight + const auto main_vx_cov = main_obj.kinematics.twist_with_covariance.covariance[0]; + const auto sub_vx_cov = sub_obj.kinematics.twist_with_covariance.covariance[0]; + double main_vx_weight, sub_vx_weight; + if (main_vx_cov == 0.0) { + main_vx_weight = 1.0 * 1e6; + } else { + main_vx_weight = 1.0 / main_vx_cov; + } + if (sub_vx_cov == 0.0) { + sub_vx_weight = 1.0 * 1e6; + } else { + sub_vx_weight = 1.0 / sub_vx_cov; + } + // merge with covariance + output_kinematics.twist_with_covariance.twist.linear.x = + (main_vx * main_vx_weight + sub_vx * sub_vx_weight) / (main_vx_weight + sub_vx_weight); + output_kinematics.twist_with_covariance.covariance[0] = 1.0 / (main_vx_weight + sub_vx_weight); + return output_kinematics; + } else { + std::cerr << "unknown merge policy in objectKinematicsMerger function." << std::endl; + return output_kinematics; + } + return output_kinematics; +} + +// object classification merger +TrackedObject objectClassificationMerger( + const TrackedObject & main_obj, const TrackedObject & sub_obj, const MergePolicy policy) +{ + if (policy == MergePolicy::SKIP) { + return main_obj; + } else if (policy == MergePolicy::OVERWRITE) { + return sub_obj; + } else if (policy == MergePolicy::FUSION) { + // merge two std::vector into one + TrackedObject dummy_obj; + dummy_obj.classification = main_obj.classification; + for (const auto & sub_class : sub_obj.classification) { + dummy_obj.classification.push_back(sub_class); + } + return dummy_obj; + } else { + std::cerr << "unknown merge policy in objectClassificationMerger function." << std::endl; + return main_obj; + } +} + +// probability merger +float probabilityMerger(const float main_prob, const float sub_prob, const MergePolicy policy) +{ + if (policy == MergePolicy::SKIP) { + return main_prob; + } else if (policy == MergePolicy::OVERWRITE) { + return sub_prob; + } else if (policy == MergePolicy::FUSION) { + return static_cast(mean(main_prob, sub_prob)); + } else { + std::cerr << "unknown merge policy in probabilityMerger function." << std::endl; + return main_prob; + } +} + +// shape merger +autoware_auto_perception_msgs::msg::Shape shapeMerger( + const autoware_auto_perception_msgs::msg::Shape & main_obj_shape, + const autoware_auto_perception_msgs::msg::Shape & sub_obj_shape, const MergePolicy policy) +{ + autoware_auto_perception_msgs::msg::Shape output_shape; + // copy main object at first + output_shape = main_obj_shape; + + if (main_obj_shape.type != sub_obj_shape.type) { + // if shape type is different, return main object + return output_shape; + } + + if (policy == MergePolicy::SKIP) { + return output_shape; + } else if (policy == MergePolicy::OVERWRITE) { + return sub_obj_shape; + } else if (policy == MergePolicy::FUSION) { + // write down fusion method for each shape type + if (main_obj_shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + // if shape type is bounding box, merge bounding box + output_shape.dimensions.x = mean(main_obj_shape.dimensions.x, sub_obj_shape.dimensions.x); + output_shape.dimensions.y = mean(main_obj_shape.dimensions.y, sub_obj_shape.dimensions.y); + output_shape.dimensions.z = mean(main_obj_shape.dimensions.z, sub_obj_shape.dimensions.z); + return output_shape; + } else if (main_obj_shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { + // if shape type is cylinder, merge cylinder + // (TODO) implement + return output_shape; + } else if (main_obj_shape.type == autoware_auto_perception_msgs::msg::Shape::POLYGON) { + // if shape type is polygon, merge polygon + // (TODO) + return output_shape; + } else { + // when type is unknown, print warning and do nothing + std::cerr << "unknown shape type in shapeMerger function." << std::endl; + return output_shape; + } + } else { + std::cerr << "unknown merge policy in shapeMerger function." << std::endl; + return output_shape; + } +} + +void updateExceptVelocity(TrackedObject & main_obj, const TrackedObject & sub_obj) +{ + const auto vx_temp = main_obj.kinematics.twist_with_covariance.twist.linear.x; + main_obj = sub_obj; + main_obj.kinematics.twist_with_covariance.twist.linear.x = vx_temp; +} + +void updateOnlyObjectVelocity(TrackedObject & main_obj, const TrackedObject & sub_obj) +{ + main_obj.kinematics = objectKinematicsVXMerger(main_obj, sub_obj, MergePolicy::OVERWRITE); +} + +void updateOnlyClassification(TrackedObject & main_obj, const TrackedObject & sub_obj) +{ + main_obj = objectClassificationMerger(main_obj, sub_obj, MergePolicy::OVERWRITE); +} + +void updateWholeTrackedObject(TrackedObject & main_obj, const TrackedObject & sub_obj) +{ + main_obj = sub_obj; +} + +} // namespace merger_utils diff --git a/perception/traffic_light_arbiter/src/traffic_light_arbiter.cpp b/perception/traffic_light_arbiter/src/traffic_light_arbiter.cpp index 3baefaa43163a..3838e93113066 100644 --- a/perception/traffic_light_arbiter/src/traffic_light_arbiter.cpp +++ b/perception/traffic_light_arbiter/src/traffic_light_arbiter.cpp @@ -16,6 +16,7 @@ #include #include +#include #include #include @@ -111,7 +112,8 @@ void TrafficLightArbiter::arbitrateAndPublish(const builtin_interfaces::msg::Tim std::unordered_map> regulatory_element_signals_map; if (map_regulatory_elements_set_ == nullptr) { - RCLCPP_WARN(get_logger(), "Received traffic signal messages before a map"); + RCLCPP_WARN_THROTTLE( + get_logger(), *get_clock(), 5000, "Received traffic signal messages before a map"); return; } @@ -126,8 +128,9 @@ void TrafficLightArbiter::arbitrateAndPublish(const builtin_interfaces::msg::Tim auto add_signal_function = [&](const auto & signal, bool priority) { const auto id = signal.traffic_signal_id; if (!map_regulatory_elements_set_->count(id)) { - RCLCPP_WARN( - get_logger(), "Received a traffic signal not present in the current map (%lu)", id); + RCLCPP_WARN_THROTTLE( + get_logger(), *get_clock(), 5000, + "Received a traffic signal not present in the current map (%lu)", id); return; } diff --git a/perception/traffic_light_classifier/CMakeLists.txt b/perception/traffic_light_classifier/CMakeLists.txt index 72dba86a00d33..22bc7521c6e31 100644 --- a/perception/traffic_light_classifier/CMakeLists.txt +++ b/perception/traffic_light_classifier/CMakeLists.txt @@ -65,59 +65,6 @@ else() set(CUDNN_AVAIL OFF) endif() -# cspell: ignore EFFICIENTNET, MOBILENET -# Download caffemodel and prototxt -set(EFFICIENTNET_BATCH_1_HASH 82baba4fcf1abe0c040cd55005e34510) -set(EFFICIENTNET_BATCH_4_HASH 21b549c2fe4fbb20d32cc019e6d70cd7) -set(EFFICIENTNET_BATCH_6_HASH 28b408710bcb24f4cdd4d746301d4e78) -set(MOBILENET_BATCH_1_HASH caa51f2080aa2df943e4f884c41898eb) -set(MOBILENET_BATCH_4_HASH c2beaf60210f471debfe72b86d076ca0) -set(MOBILENET_BATCH_6_HASH 47255a11bde479320d703f1f45db1242) -set(LAMP_LABEL_HASH e9f45efb02f2a9aa8ac27b3d5c164905) -set(AWS_DIR https://awf.ml.dev.web.auto/perception/models/traffic_light_classifier/v2) -set(DATA_PATH "${CMAKE_CURRENT_SOURCE_DIR}/data") -if(NOT EXISTS "${DATA_PATH}") - execute_process(COMMAND mkdir -p ${DATA_PATH}) -endif() -function(download FILE_NAME FILE_HASH) - message(STATUS "Checking and downloading ${FILE_NAME}") - set(FILE_PATH ${DATA_PATH}/${FILE_NAME}) - set(STATUS_CODE 0) - message(STATUS "start ${FILE_NAME}") - if(EXISTS ${FILE_PATH}) - message(STATUS "found ${FILE_NAME}") - file(MD5 ${FILE_PATH} EXISTING_FILE_HASH) - if(${FILE_HASH} STREQUAL ${EXISTING_FILE_HASH}) - message(STATUS "same ${FILE_NAME}") - message(STATUS "File already exists.") - else() - message(STATUS "diff ${FILE_NAME}") - message(STATUS "File hash changes. Downloading now ...") - file(DOWNLOAD ${AWS_DIR}/${FILE_NAME} ${FILE_PATH} STATUS DOWNLOAD_STATUS TIMEOUT 300) - list(GET DOWNLOAD_STATUS 0 STATUS_CODE) - list(GET DOWNLOAD_STATUS 1 ERROR_MESSAGE) - endif() - else() - message(STATUS "not found ${FILE_NAME}") - message(STATUS "File doesn't exists. Downloading now ...") - file(DOWNLOAD ${AWS_DIR}/${FILE_NAME} ${FILE_PATH} STATUS DOWNLOAD_STATUS TIMEOUT 300) - list(GET DOWNLOAD_STATUS 0 STATUS_CODE) - list(GET DOWNLOAD_STATUS 1 ERROR_MESSAGE) - endif() - if(${STATUS_CODE} EQUAL 0) - message(STATUS "Download completed successfully!") - else() - message(FATAL_ERROR "Error occurred during download: ${ERROR_MESSAGE}") - endif() -endfunction() -download(traffic_light_classifier_mobilenetv2_batch_1.onnx ${MOBILENET_BATCH_1_HASH}) -download(traffic_light_classifier_mobilenetv2_batch_4.onnx ${MOBILENET_BATCH_4_HASH}) -download(traffic_light_classifier_mobilenetv2_batch_6.onnx ${MOBILENET_BATCH_6_HASH}) -download(traffic_light_classifier_efficientNet_b1_batch_1.onnx ${EFFICIENTNET_BATCH_1_HASH}) -download(traffic_light_classifier_efficientNet_b1_batch_4.onnx ${EFFICIENTNET_BATCH_4_HASH}) -download(traffic_light_classifier_efficientNet_b1_batch_6.onnx ${EFFICIENTNET_BATCH_6_HASH}) -download(lamp_labels.txt 4b2cf910d97d05d464e7c26901af3d4c) - find_package(ament_cmake_auto REQUIRED) ament_auto_find_build_dependencies() find_package(OpenCV REQUIRED) @@ -175,7 +122,6 @@ if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL) ) ament_auto_package(INSTALL_TO_SHARE - data launch ) @@ -200,7 +146,6 @@ else() ) ament_auto_package(INSTALL_TO_SHARE - data launch ) diff --git a/perception/traffic_light_classifier/README.md b/perception/traffic_light_classifier/README.md index 53ba0d3be7ba2..2aecf66f8fb7b 100644 --- a/perception/traffic_light_classifier/README.md +++ b/perception/traffic_light_classifier/README.md @@ -55,6 +55,7 @@ These colors and shapes are assigned to the message as follows: | Name | Type | Description | | ----------------- | ---- | ------------------------------------------- | | `classifier_type` | int | if the value is `1`, cnn_classifier is used | +| `data_path` | str | packages data and artifacts directory path | ### Core Parameters @@ -92,67 +93,256 @@ These colors and shapes are assigned to the message as follows: | `red_max_s` | int | the maximum saturation of red color | | `red_max_v` | int | the maximum value (brightness) of red color | -## Customization of CNN model +## Training Traffic Light Classifier Model + +### Overview + +This guide provides detailed instructions on training a traffic light classifier model using the **[mmlab/mmpretrain](https://github.com/open-mmlab/mmpretrain)** repository +and deploying it using **[mmlab/mmdeploy](https://github.com/open-mmlab/mmdeploy)**. +If you wish to create a custom traffic light classifier model with your own dataset, please follow the steps outlined below. + +### Data Preparation + +#### Use Sample Dataset + +Autoware offers a sample dataset that illustrates the training procedures for +traffic light classification. This dataset comprises 1045 images categorized +into red, green, and yellow labels. To utilize this sample dataset, +please download it from **[link](https://autoware-files.s3.us-west-2.amazonaws.com/dataset/traffic_light_sample_dataset.tar.gz)** and extract it to a designated folder of your choice. + +#### Use Your Custom Dataset + +To train a traffic light classifier, adopt a structured subfolder format where each +subfolder represents a distinct class. Below is an illustrative dataset structure example; + +```python +DATASET_ROOT + ├── TRAIN + │ ├── RED + │ │ ├── 001.png + │ │ ├── 002.png + │ │ └── ... + │ │ + │ ├── GREEN + │ │ ├── 001.png + │ │ ├── 002.png + │ │ └──... + │ │ + │ ├── YELLOW + │ │ ├── 001.png + │ │ ├── 002.png + │ │ └──... + │ └── ... + │ + ├── VAL + │ └──... + │ + │ + └── TEST + └── ... -Currently, in Autoware, [MobileNetV2](https://arxiv.org/abs/1801.04381v3) and [EfficientNet-b1](https://arxiv.org/abs/1905.11946v5) are provided. -The corresponding onnx files are `data/traffic_light_classifier_mobilenetv2.onnx` and `data/traffic_light_classifier_efficientNet_b1.onnx` (These files will be downloaded during the build process). -Also, you can apply the following models shown as below, for example. -- [ResNet](https://openaccess.thecvf.com/content_cvpr_2016/html/He_Deep_Residual_Learning_CVPR_2016_paper.html) -- [MobileNetV3](https://arxiv.org/abs/1905.02244) - ... +``` + +### Installation + +#### Prerequisites + + + +**Step 1.** Download and install Miniconda from the [official website](https://mmpretrain.readthedocs.io/en/latest/get_started.html). + +**Step 2.** Create a conda virtual environment and activate it + +```bash +conda create --name tl-classifier python=3.8 -y +conda activate tl-classifier +``` + +**Step 3.** Install PyTorch + +Please ensure you have PyTorch installed, compatible with CUDA 11.6, as it is a requirement for current Autoware + +```bash +conda install pytorch==1.13.1 torchvision==0.14.1 pytorch-cuda=11.6 -c pytorch -c nvidia +``` + +#### Install mmlab/mmpretrain + +**Step 1.** Install mmpretrain from source -In order to train models and export onnx model, we recommend [open-mmlab/mmclassification](https://github.com/open-mmlab/mmclassification.git). -Please follow the [official document](https://mmclassification.readthedocs.io/en/latest/) to install and experiment with mmclassification. If you get into troubles, [FAQ page](https://mmclassification.readthedocs.io/en/latest/faq.html) would help you. +```bash +cd ~/ +git clone https://github.com/open-mmlab/mmpretrain.git +cd mmpretrain +pip install -U openmim && mim install -e . +``` -The following steps are example of a quick-start. +### Training -### step 0. Install [MMCV](https://github.com/open-mmlab/mmcv.git) and [MIM](https://github.com/open-mmlab/mim.git) +MMPretrain offers a training script that is controlled through a configuration file. +Leveraging an inheritance design pattern, you can effortlessly tailor the training script +using Python files as configuration files. -_NOTE_ : First of all, install [PyTorch](https://pytorch.org/) suitable for your CUDA version (CUDA11.6 is supported in Autoware). +In the example, we demonstrate the training steps on the MobileNetV2 model, +but you have the flexibility to employ alternative classification models such as +EfficientNetV2, EfficientNetV3, ResNet, and more. -In order to install mmcv suitable for your CUDA version, install it specifying a url. +#### Create a config file -```shell -# Install mim -$ pip install -U openmim +Generate a configuration file for your preferred model within the `configs` folder -# Install mmcv on a machine with CUDA11.6 and PyTorch1.13.0 -$ pip install mmcv-full -f https://download.openmmlab.com/mmcv/dist/cu116/torch1.13/index.html +```bash +touch ~/mmpretrain/configs/mobilenet_v2/mobilenet-v2_8xb32_custom.py ``` -### step 1. Install MMClassification +Open the configuration file in your preferred text editor and make a copy of +the provided content. Adjust the **data_root** variable to match the path of your dataset. +You are welcome to customize the configuration parameters for the model, dataset, and scheduler to +suit your preferences + +```python +# Inherit model, schedule and default_runtime from base model +_base_ = [ + '../_base_/models/mobilenet_v2_1x.py', + '../_base_/schedules/imagenet_bs256_epochstep.py', + '../_base_/default_runtime.py' +] + +# Set the number of classes to the model +# You can also change other model parameters here +# For detailed descriptions of model parameters, please refer to link below +# (Customize model)[https://mmpretrain.readthedocs.io/en/latest/advanced_guides/modules.html] +model = dict(head=dict(num_classes=3, topk=(1, 3))) + +# Set max epochs and validation interval +train_cfg = dict(by_epoch=True, max_epochs=50, val_interval=5) + +# Set optimizer and lr scheduler +optim_wrapper = dict( + optimizer=dict(type='SGD', lr=0.001, momentum=0.9)) +param_scheduler = dict(type='StepLR', by_epoch=True, step_size=1, gamma=0.98) + +dataset_type = 'CustomDataset' +data_root = "/PATH/OF/YOUR/DATASET" + +# Customize data preprocessing and dataloader pipeline for training set +# These parameters calculated for the sample dataset +data_preprocessor = dict( + mean=[0.2888 * 256, 0.2570 * 256, 0.2329 * 256], + std=[0.2106 * 256, 0.2037 * 256, 0.1864 * 256], + num_classes=3, + to_rgb=True, +) + +# Customize data preprocessing and dataloader pipeline for train set +# For detailed descriptions of data pipeline, please refer to link below +# (Customize data pipeline)[https://mmpretrain.readthedocs.io/en/latest/advanced_guides/pipeline.html] +train_pipeline = [ + dict(type='LoadImageFromFile'), + dict(type='Resize', scale=224), + dict(type='RandomFlip', prob=0.5, direction='horizontal'), + dict(type='PackInputs'), +] +train_dataloader = dict( + dataset=dict( + type=dataset_type, + data_root=data_root, + ann_file='', + data_prefix='train', + with_label=True, + pipeline=train_pipeline, + ), + num_workers=8, + batch_size=32, + sampler=dict(type='DefaultSampler', shuffle=True) +) + +# Customize data preprocessing and dataloader pipeline for test set +test_pipeline = [ + dict(type='LoadImageFromFile'), + dict(type='Resize', scale=224), + dict(type='PackInputs'), +] + +# Customize data preprocessing and dataloader pipeline for validation set +val_cfg = dict() +val_dataloader = dict( + dataset=dict( + type=dataset_type, + data_root=data_root, + ann_file='', + data_prefix='val', + with_label=True, + pipeline=test_pipeline, + ), + num_workers=8, + batch_size=32, + sampler=dict(type='DefaultSampler', shuffle=True) +) + +val_evaluator = dict(topk=(1, 3,), type='Accuracy') + +test_dataloader = val_dataloader +test_evaluator = val_evaluator -You can install MMClassification as a Python package or from source. +``` -```shell -# As a Python package -$ pip install mmcls +#### Start training -# From source -$ git clone https://github.com/open-mmlab/mmclassification.git -$ cd mmclassification -$ pip install -v -e . +```bash +cd ~/mmpretrain +python tools/train.py configs/mobilenet_v2/mobilenet-v2_8xb32_custom.py ``` -### step 2. Train your model +Training logs and weights will be saved in the `work_dirs/mobilenet-v2_8xb32_custom` folder. + +### Convert PyTorch model to ONNX model -Train model with your experiment configuration file. For the details of config file, see [here](https://mmclassification.readthedocs.io/en/latest/tutorials/config.html). +#### Install mmdeploy -```shell -# [] is optional, you can start training from pre-trained checkpoint -$ mim train mmcls YOUR_CONFIG.py [--resume-from YOUR_CHECKPOINT.pth] +The 'mmdeploy' toolset is designed for deploying your trained model onto various target devices. +With its capabilities, you can seamlessly convert PyTorch models into the ONNX format. + +```bash +# Activate your conda environment +conda activate tl-classifier + +# Install mmenigne and mmcv +mim install mmengine +mim install "mmcv>=2.0.0rc2" + +# Install mmdeploy +pip install mmdeploy==1.2.0 + +# Support onnxruntime +pip install mmdeploy-runtime==1.2.0 +pip install mmdeploy-runtime-gpu==1.2.0 +pip install onnxruntime-gpu==1.8.1 + +#Clone mmdeploy repository +cd ~/ +git clone -b main https://github.com/open-mmlab/mmdeploy.git ``` -### step 3. Export onnx model +#### Convert PyTorch model to ONNX model -In exporting onnx, use `mmclassification/tools/deployment/pytorch2onnx.py` or [open-mmlab/mmdeploy](https://github.com/open-mmlab/mmdeploy.git). +```bash +cd ~/mmdeploy -```shell -cd ~/mmclassification/tools/deployment -python3 pytorch2onnx.py YOUR_CONFIG.py ... +# Run deploy.py script +# deploy.py script takes 5 main arguments with these order; config file path, train config file path, +# checkpoint file path, demo image path, and work directory path +python tools/deploy.py \ +~/mmdeploy/configs/mmpretrain/classification_onnxruntime_static.py\ +~/mmpretrain/configs/mobilenet_v2/train_mobilenet_v2.py \ +~/mmpretrain/work_dirs/train_mobilenet_v2/epoch_300.pth \ +/SAMPLE/IAMGE/DIRECTORY \ +--work-dir mmdeploy_model/mobilenet_v2 ``` +Converted ONNX model will be saved in the `mmdeploy/mmdeploy_model/mobilenet_v2` folder. + After obtaining your onnx model, update parameters defined in the launch file (e.g. `model_file_path`, `label_file_path`, `input_h`, `input_w`...). Note that, we only support labels defined in [tier4_perception_msgs::msg::TrafficLightElement](https://github.com/tier4/tier4_autoware_msgs/blob/tier4/universe/tier4_perception_msgs/msg/traffic_light/TrafficLightElement.msg). diff --git a/perception/traffic_light_classifier/launch/traffic_light_classifier.launch.xml b/perception/traffic_light_classifier/launch/traffic_light_classifier.launch.xml index d4794443d95d9..10aa04cc585af 100644 --- a/perception/traffic_light_classifier/launch/traffic_light_classifier.launch.xml +++ b/perception/traffic_light_classifier/launch/traffic_light_classifier.launch.xml @@ -2,8 +2,9 @@ - - + + + diff --git a/perception/traffic_light_fine_detector/CMakeLists.txt b/perception/traffic_light_fine_detector/CMakeLists.txt index a0605a6aa1a90..f1f30a9da3195 100644 --- a/perception/traffic_light_fine_detector/CMakeLists.txt +++ b/perception/traffic_light_fine_detector/CMakeLists.txt @@ -69,53 +69,6 @@ else() set(CUDNN_AVAIL OFF) endif() -# Download caffemodel and prototxt -set(PRETRAINED_MODEL_BATCH_1_HASH 2b72d085022b8ee6aacff06bd722cfda) -set(PRETRAINED_MODEL_BATCH_4_HASH 4044daa86e7776ce241e94d98a09cc0e) -set(PRETRAINED_MODEL_BATCH_6_HASH 47255a11bde479320d703f1f45db1242) -set(LAMP_LABEL_HASH e9f45efb02f2a9aa8ac27b3d5c164905) -set(DATA_DIR https://awf.ml.dev.web.auto/perception/models/tlr_yolox_s/v2) - -set(DATA_PATH "${CMAKE_CURRENT_SOURCE_DIR}/data") -if(NOT EXISTS "${DATA_PATH}") - execute_process(COMMAND mkdir -p ${DATA_PATH}) -endif() -function(download FILE_NAME FILE_HASH) - message(STATUS "Checking and downloading ${FILE_NAME}") - set(FILE_PATH ${DATA_PATH}/${FILE_NAME}) - set(STATUS_CODE 0) - message(STATUS "start ${FILE_NAME}") - if(EXISTS ${FILE_PATH}) - message(STATUS "found ${FILE_NAME}") - file(MD5 ${FILE_PATH} EXISTING_FILE_HASH) - if(${FILE_HASH} STREQUAL ${EXISTING_FILE_HASH}) - message(STATUS "same ${FILE_NAME}") - message(STATUS "File already exists.") - else() - message(STATUS "diff ${FILE_NAME}") - message(STATUS "File hash changes. Downloading now ...") - file(DOWNLOAD ${DATA_DIR}/${FILE_NAME} ${FILE_PATH} STATUS DOWNLOAD_STATUS TIMEOUT 300) - list(GET DOWNLOAD_STATUS 0 STATUS_CODE) - list(GET DOWNLOAD_STATUS 1 ERROR_MESSAGE) - endif() - else() - message(STATUS "not found ${FILE_NAME}") - message(STATUS "File doesn't exists. Downloading now ...") - file(DOWNLOAD ${DATA_DIR}/${FILE_NAME} ${FILE_PATH} STATUS DOWNLOAD_STATUS TIMEOUT 300) - list(GET DOWNLOAD_STATUS 0 STATUS_CODE) - list(GET DOWNLOAD_STATUS 1 ERROR_MESSAGE) - endif() - if(${STATUS_CODE} EQUAL 0) - message(STATUS "Download completed successfully!") - else() - message(FATAL_ERROR "Error occurred during download: ${ERROR_MESSAGE}") - endif() -endfunction() -download(tlr_yolox_s_batch_1.onnx ${PRETRAINED_MODEL_BATCH_1_HASH}) -download(tlr_yolox_s_batch_4.onnx ${PRETRAINED_MODEL_BATCH_4_HASH}) -download(tlr_yolox_s_batch_6.onnx ${PRETRAINED_MODEL_BATCH_6_HASH}) -download(tlr_labels.txt ${LAMP_LABEL_HASH}) - if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL) include_directories( ${OpenCV_INCLUDE_DIRS} @@ -147,7 +100,6 @@ if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL) ) ament_auto_package(INSTALL_TO_SHARE - data launch ) diff --git a/perception/traffic_light_fine_detector/README.md b/perception/traffic_light_fine_detector/README.md index 1ed6debfeae91..9e69c22fdc17b 100644 --- a/perception/traffic_light_fine_detector/README.md +++ b/perception/traffic_light_fine_detector/README.md @@ -16,7 +16,8 @@ The model was fine-tuned on around 17,000 TIER IV internal images of Japanese tr ### Trained Onnx model -- +You can download the ONNX file using these instructions. +Please visit [autoware-documentation](https://github.com/autowarefoundation/autoware-documentation/blob/main/docs/models/index.md) for more information. ## Inner-workings / Algorithms @@ -50,12 +51,13 @@ Based on the camera image and the global ROI array detected by `map_based_detect ### Node Parameters -| Name | Type | Default Value | Description | -| -------------------------- | ------ | ------------- | ------------------------------------------------------------------ | -| `fine_detector_model_path` | string | "" | The onnx file name for yolo model | -| `fine_detector_label_path` | string | "" | The label file with label names for detected objects written on it | -| `fine_detector_precision` | string | "fp32" | The inference mode: "fp32", "fp16" | -| `approximate_sync` | bool | false | Flag for whether to ues approximate sync policy | +| Name | Type | Default Value | Description | +| -------------------------- | ------ | --------------------------- | ------------------------------------------------------------------ | +| `data_path` | string | "$(env HOME)/autoware_data" | packages data and artifacts directory path | +| `fine_detector_model_path` | string | "" | The onnx file name for yolo model | +| `fine_detector_label_path` | string | "" | The label file with label names for detected objects written on it | +| `fine_detector_precision` | string | "fp32" | The inference mode: "fp32", "fp16" | +| `approximate_sync` | bool | false | Flag for whether to ues approximate sync policy | ## Assumptions / Known limits diff --git a/perception/traffic_light_fine_detector/launch/traffic_light_fine_detector.launch.xml b/perception/traffic_light_fine_detector/launch/traffic_light_fine_detector.launch.xml index 5ce7840c28fd6..6e32d3410c260 100644 --- a/perception/traffic_light_fine_detector/launch/traffic_light_fine_detector.launch.xml +++ b/perception/traffic_light_fine_detector/launch/traffic_light_fine_detector.launch.xml @@ -1,6 +1,7 @@ - - + + + diff --git a/perception/traffic_light_fine_detector/package.xml b/perception/traffic_light_fine_detector/package.xml index c54912e256c02..8ff9f2d133b52 100644 --- a/perception/traffic_light_fine_detector/package.xml +++ b/perception/traffic_light_fine_detector/package.xml @@ -4,7 +4,7 @@ traffic_light_fine_detector 0.1.0 The traffic_light_fine_detector package - Mingyu Li + Tao Zhong Shunsuke Miura Apache License 2.0 diff --git a/perception/traffic_light_map_based_detector/include/traffic_light_map_based_detector/node.hpp b/perception/traffic_light_map_based_detector/include/traffic_light_map_based_detector/node.hpp index 9db8d7242c64d..e46431a7b199e 100644 --- a/perception/traffic_light_map_based_detector/include/traffic_light_map_based_detector/node.hpp +++ b/perception/traffic_light_map_based_detector/include/traffic_light_map_based_detector/node.hpp @@ -16,7 +16,6 @@ #define TRAFFIC_LIGHT_MAP_BASED_DETECTOR__NODE_HPP_ #include -#include #include #include diff --git a/perception/traffic_light_map_based_detector/package.xml b/perception/traffic_light_map_based_detector/package.xml index 1021757e99a07..c2d53869fa3d5 100644 --- a/perception/traffic_light_map_based_detector/package.xml +++ b/perception/traffic_light_map_based_detector/package.xml @@ -22,6 +22,7 @@ sensor_msgs tf2 tf2_eigen + tf2_geometry_msgs tf2_ros tier4_autoware_utils tier4_perception_msgs diff --git a/perception/traffic_light_map_based_detector/src/node.cpp b/perception/traffic_light_map_based_detector/src/node.cpp index fcf13e3900793..f0a5d7b7b1fde 100644 --- a/perception/traffic_light_map_based_detector/src/node.cpp +++ b/perception/traffic_light_map_based_detector/src/node.cpp @@ -17,7 +17,8 @@ #include #include #include -#include +#include +#include #include #include @@ -30,6 +31,12 @@ #include #include +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif + namespace { cv::Point2d calcRawImagePointFromPoint3D( diff --git a/perception/traffic_light_multi_camera_fusion/include/traffic_light_multi_camera_fusion/node.hpp b/perception/traffic_light_multi_camera_fusion/include/traffic_light_multi_camera_fusion/node.hpp index bdfd07ae54b28..390b0f590b637 100644 --- a/perception/traffic_light_multi_camera_fusion/include/traffic_light_multi_camera_fusion/node.hpp +++ b/perception/traffic_light_multi_camera_fusion/include/traffic_light_multi_camera_fusion/node.hpp @@ -15,8 +15,6 @@ #ifndef TRAFFIC_LIGHT_MULTI_CAMERA_FUSION__NODE_HPP_ #define TRAFFIC_LIGHT_MULTI_CAMERA_FUSION__NODE_HPP_ -#include -#include #include #include @@ -25,7 +23,7 @@ #include #include -#include +#include #include #include #include diff --git a/perception/traffic_light_multi_camera_fusion/package.xml b/perception/traffic_light_multi_camera_fusion/package.xml index eb0858152e69e..b08ccfe668085 100644 --- a/perception/traffic_light_multi_camera_fusion/package.xml +++ b/perception/traffic_light_multi_camera_fusion/package.xml @@ -4,7 +4,7 @@ traffic_light_multi_camera_fusion 0.1.0 The traffic_light_multi_camera_fusion package - Mingyu Li + Tao Zhong Shunsuke Miura Apache License 2.0 diff --git a/perception/traffic_light_multi_camera_fusion/src/node.cpp b/perception/traffic_light_multi_camera_fusion/src/node.cpp index 6c29be9858a9a..1582040648624 100644 --- a/perception/traffic_light_multi_camera_fusion/src/node.cpp +++ b/perception/traffic_light_multi_camera_fusion/src/node.cpp @@ -14,6 +14,9 @@ #include "traffic_light_multi_camera_fusion/node.hpp" +#include +#include + #include #include #include diff --git a/perception/traffic_light_occlusion_predictor/include/traffic_light_occlusion_predictor/occlusion_predictor.hpp b/perception/traffic_light_occlusion_predictor/include/traffic_light_occlusion_predictor/occlusion_predictor.hpp index e37083f81decd..7e2825197f3a7 100644 --- a/perception/traffic_light_occlusion_predictor/include/traffic_light_occlusion_predictor/occlusion_predictor.hpp +++ b/perception/traffic_light_occlusion_predictor/include/traffic_light_occlusion_predictor/occlusion_predictor.hpp @@ -25,9 +25,7 @@ #include #include -#include -#include -#include +#include #include #include #include diff --git a/perception/traffic_light_occlusion_predictor/package.xml b/perception/traffic_light_occlusion_predictor/package.xml index 0ae390c90b9a9..49a67d68057d9 100644 --- a/perception/traffic_light_occlusion_predictor/package.xml +++ b/perception/traffic_light_occlusion_predictor/package.xml @@ -4,7 +4,7 @@ traffic_light_occlusion_predictor 0.1.0 The traffic_light_occlusion_predictor package - Mingyu Li + Tao Zhong Shunsuke Miura Apache License 2.0 diff --git a/perception/traffic_light_occlusion_predictor/src/nodelet.cpp b/perception/traffic_light_occlusion_predictor/src/nodelet.cpp index 366820a725018..c460f6a623bc4 100644 --- a/perception/traffic_light_occlusion_predictor/src/nodelet.cpp +++ b/perception/traffic_light_occlusion_predictor/src/nodelet.cpp @@ -18,7 +18,6 @@ #include #include #include -#include #include #include diff --git a/perception/traffic_light_ssd_fine_detector/CMakeLists.txt b/perception/traffic_light_ssd_fine_detector/CMakeLists.txt index 76a0e3d75c20f..61fa7bcbe0b74 100644 --- a/perception/traffic_light_ssd_fine_detector/CMakeLists.txt +++ b/perception/traffic_light_ssd_fine_detector/CMakeLists.txt @@ -70,48 +70,6 @@ else() set(CUDNN_AVAIL OFF) endif() -# Download caffemodel and prototxt -set(PRETRAINED_MODEL_HASH 34ce7f2cbacbf6da8bc35769f027b73f) -set(LAMP_LABEL_HASH e9f45efb02f2a9aa8ac27b3d5c164905) - -set(DATA_PATH "${CMAKE_CURRENT_SOURCE_DIR}/data") -if(NOT EXISTS "${DATA_PATH}") - execute_process(COMMAND mkdir -p ${DATA_PATH}) -endif() -function(download FILE_NAME FILE_HASH) - message(STATUS "Checking and downloading ${FILE_NAME}") - set(FILE_PATH ${DATA_PATH}/${FILE_NAME}) - set(STATUS_CODE 0) - message(STATUS "start ${FILE_NAME}") - if(EXISTS ${FILE_PATH}) - message(STATUS "found ${FILE_NAME}") - file(MD5 ${FILE_PATH} EXISTING_FILE_HASH) - if(${FILE_HASH} STREQUAL ${EXISTING_FILE_HASH}) - message(STATUS "same ${FILE_NAME}") - message(STATUS "File already exists.") - else() - message(STATUS "diff ${FILE_NAME}") - message(STATUS "File hash changes. Downloading now ...") - file(DOWNLOAD https://awf.ml.dev.web.auto/perception/models/${FILE_NAME} ${FILE_PATH} STATUS DOWNLOAD_STATUS TIMEOUT 300) - list(GET DOWNLOAD_STATUS 0 STATUS_CODE) - list(GET DOWNLOAD_STATUS 1 ERROR_MESSAGE) - endif() - else() - message(STATUS "not found ${FILE_NAME}") - message(STATUS "File doesn't exists. Downloading now ...") - file(DOWNLOAD https://awf.ml.dev.web.auto/perception/models/${FILE_NAME} ${FILE_PATH} STATUS DOWNLOAD_STATUS TIMEOUT 300) - list(GET DOWNLOAD_STATUS 0 STATUS_CODE) - list(GET DOWNLOAD_STATUS 1 ERROR_MESSAGE) - endif() - if(${STATUS_CODE} EQUAL 0) - message(STATUS "Download completed successfully!") - else() - message(FATAL_ERROR "Error occurred during download: ${ERROR_MESSAGE}") - endif() -endfunction() -download(mb2-ssd-lite-tlr.onnx 34ce7f2cbacbf6da8bc35769f027b73f) -download(voc_labels_tl.txt e9f45efb02f2a9aa8ac27b3d5c164905) - if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL) include_directories( ${OpenCV_INCLUDE_DIRS} @@ -174,7 +132,6 @@ if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL) ) ament_auto_package(INSTALL_TO_SHARE - data launch ) diff --git a/perception/traffic_light_ssd_fine_detector/README.md b/perception/traffic_light_ssd_fine_detector/README.md index 1dd05665709f5..4dbda8421d85d 100644 --- a/perception/traffic_light_ssd_fine_detector/README.md +++ b/perception/traffic_light_ssd_fine_detector/README.md @@ -122,15 +122,16 @@ Based on the camera image and the global ROI array detected by `map_based_detect ### Node Parameters -| Name | Type | Default Value | Description | -| ------------------ | ------ | ------------------------------ | -------------------------------------------------------------------- | -| `onnx_file` | string | "./data/mb2-ssd-lite-tlr.onnx" | The onnx file name for yolo model | -| `label_file` | string | "./data/voc_labels_tl.txt" | The label file with label names for detected objects written on it | -| `dnn_header_type` | string | "pytorch" | Name of DNN trained toolbox: "pytorch" or "mmdetection" | -| `mode` | string | "FP32" | The inference mode: "FP32", "FP16", "INT8" | -| `max_batch_size` | int | 8 | The size of the batch processed at one time by inference by TensorRT | -| `approximate_sync` | bool | false | Flag for whether to ues approximate sync policy | -| `build_only` | bool | false | shutdown node after TensorRT engine file is built | +| Name | Type | Default Value | Description | +| ------------------ | ------ | ------------------------------------------------------------------------ | -------------------------------------------------------------------- | +| `data_path` | string | "$(env HOME)/autoware_data" | packages data and artifacts directory path | +| `onnx_file` | string | "$(var data_path)/traffic_light_ssd_fine_detector/mb2-ssd-lite-tlr.onnx" | The onnx file name for yolo model | +| `label_file` | string | "$(var data_path)/traffic_light_ssd_fine_detector/voc_labels_tl.txt" | The label file with label names for detected objects written on it | +| `dnn_header_type` | string | "pytorch" | Name of DNN trained toolbox: "pytorch" or "mmdetection" | +| `mode` | string | "FP32" | The inference mode: "FP32", "FP16", "INT8" | +| `max_batch_size` | int | 8 | The size of the batch processed at one time by inference by TensorRT | +| `approximate_sync` | bool | false | Flag for whether to ues approximate sync policy | +| `build_only` | bool | false | shutdown node after TensorRT engine file is built | ## Assumptions / Known limits diff --git a/perception/traffic_light_ssd_fine_detector/launch/traffic_light_ssd_fine_detector.launch.xml b/perception/traffic_light_ssd_fine_detector/launch/traffic_light_ssd_fine_detector.launch.xml index a4d61b774652a..714c4d288b603 100644 --- a/perception/traffic_light_ssd_fine_detector/launch/traffic_light_ssd_fine_detector.launch.xml +++ b/perception/traffic_light_ssd_fine_detector/launch/traffic_light_ssd_fine_detector.launch.xml @@ -1,6 +1,7 @@ - - + + + diff --git a/perception/traffic_light_visualization/include/traffic_light_map_visualizer/node.hpp b/perception/traffic_light_visualization/include/traffic_light_map_visualizer/node.hpp index 07cb21e60b137..3f83cf6e926ad 100644 --- a/perception/traffic_light_visualization/include/traffic_light_map_visualizer/node.hpp +++ b/perception/traffic_light_visualization/include/traffic_light_map_visualizer/node.hpp @@ -16,7 +16,6 @@ #define TRAFFIC_LIGHT_MAP_VISUALIZER__NODE_HPP_ #include -#include #include #include diff --git a/perception/traffic_light_visualization/src/traffic_light_map_visualizer/node.cpp b/perception/traffic_light_visualization/src/traffic_light_map_visualizer/node.cpp index a9d107e78a1c4..53e06e47a1873 100644 --- a/perception/traffic_light_visualization/src/traffic_light_map_visualizer/node.cpp +++ b/perception/traffic_light_visualization/src/traffic_light_map_visualizer/node.cpp @@ -13,6 +13,7 @@ // limitations under the License. #include +#include #include #include diff --git a/perception/traffic_light_visualization/src/traffic_light_roi_visualizer/nodelet.cpp b/perception/traffic_light_visualization/src/traffic_light_roi_visualizer/nodelet.cpp index a3b1a2b16a743..2f2d80ba43233 100644 --- a/perception/traffic_light_visualization/src/traffic_light_roi_visualizer/nodelet.cpp +++ b/perception/traffic_light_visualization/src/traffic_light_roi_visualizer/nodelet.cpp @@ -127,13 +127,25 @@ void TrafficLightRoiVisualizerNodelet::imageRoiCallback( { cv_bridge::CvImagePtr cv_ptr; try { - cv_ptr = cv_bridge::toCvCopy(input_image_msg, input_image_msg->encoding); + // try to convert to RGB8 from any input encoding, since createRect() only supports RGB8 based + // bbox drawing + cv_ptr = cv_bridge::toCvCopy(input_image_msg, sensor_msgs::image_encodings::RGB8); for (auto tl_roi : input_tl_roi_msg->rois) { - createRect(cv_ptr->image, tl_roi, cv::Scalar(0, 255, 0)); + ClassificationResult result; + bool has_correspond_traffic_signal = + getClassificationResult(tl_roi.traffic_light_id, *input_traffic_signals_msg, result); + + if (!has_correspond_traffic_signal) { + // does not have classification result + createRect(cv_ptr->image, tl_roi, cv::Scalar(255, 255, 255)); + } else { + // has classification result + createRect(cv_ptr->image, tl_roi, result); + } } } catch (cv_bridge::Exception & e) { RCLCPP_ERROR( - get_logger(), "Could not convert from '%s' to 'bgr8'.", input_image_msg->encoding.c_str()); + get_logger(), "Could not convert from '%s' to 'rgb8'.", input_image_msg->encoding.c_str()); } image_pub_.publish(cv_ptr->toImageMsg()); } @@ -182,7 +194,9 @@ void TrafficLightRoiVisualizerNodelet::imageRoughRoiCallback( { cv_bridge::CvImagePtr cv_ptr; try { - cv_ptr = cv_bridge::toCvCopy(input_image_msg, input_image_msg->encoding); + // try to convert to RGB8 from any input encoding, since createRect() only supports RGB8 based + // bbox drawing + cv_ptr = cv_bridge::toCvCopy(input_image_msg, sensor_msgs::image_encodings::RGB8); for (auto tl_rough_roi : input_tl_rough_roi_msg->rois) { // visualize rough roi createRect(cv_ptr->image, tl_rough_roi, cv::Scalar(0, 255, 0)); @@ -208,7 +222,7 @@ void TrafficLightRoiVisualizerNodelet::imageRoughRoiCallback( } } catch (cv_bridge::Exception & e) { RCLCPP_ERROR( - get_logger(), "Could not convert from '%s' to 'bgr8'.", input_image_msg->encoding.c_str()); + get_logger(), "Could not convert from '%s' to 'rgb8'.", input_image_msg->encoding.c_str()); } image_pub_.publish(cv_ptr->toImageMsg()); } diff --git a/planning/behavior_path_planner/CMakeLists.txt b/planning/behavior_path_planner/CMakeLists.txt index 0352096d02b2b..ee6e50f5a9553 100644 --- a/planning/behavior_path_planner/CMakeLists.txt +++ b/planning/behavior_path_planner/CMakeLists.txt @@ -32,6 +32,7 @@ ament_auto_add_library(behavior_path_planner_node SHARED src/utils/path_utils.cpp src/utils/path_safety_checker/safety_check.cpp src/utils/path_safety_checker/objects_filtering.cpp + src/utils/start_goal_planner_common/utils.cpp src/utils/avoidance/utils.cpp src/utils/lane_change/utils.cpp src/utils/side_shift/util.cpp @@ -49,7 +50,6 @@ ament_auto_add_library(behavior_path_planner_node SHARED src/utils/drivable_area_expansion/drivable_area_expansion.cpp src/utils/drivable_area_expansion/map_utils.cpp src/utils/drivable_area_expansion/footprints.cpp - src/utils/drivable_area_expansion/expansion.cpp src/utils/geometric_parallel_parking/geometric_parallel_parking.cpp src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp src/marker_utils/utils.cpp diff --git a/planning/behavior_path_planner/README.md b/planning/behavior_path_planner/README.md index 07d949aae8a39..0c0c7c806c119 100644 --- a/planning/behavior_path_planner/README.md +++ b/planning/behavior_path_planner/README.md @@ -173,12 +173,6 @@ struct ShiftLine Click [here](./docs/behavior_path_planner_path_generation_design.md) for details. -### Smooth goal connection - -If the target path contains a goal, modify the points of the path so that the path and the goal are connected smoothly. This process will change the shape of the path by the distance of `refine_goal_search_radius_range` from the goal. Note that this logic depends on the interpolation algorithm that will be executed in a later module (at the moment it uses spline interpolation), so it needs to be updated in the future. - -![path_goal_refinement](./image/path_goal_refinement.drawio.svg) - ## References / External links This module depends on the external [BehaviorTreeCpp](https://github.com/BehaviorTree/BehaviorTree.CPP) library. diff --git a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml index 9ecc1a63b55b0..25e42d20d144a 100644 --- a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml +++ b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml @@ -118,8 +118,6 @@ object_ignore_section_crosswalk_in_front_distance: 30.0 # [m] object_ignore_section_crosswalk_behind_distance: 30.0 # [m] # detection range - object_check_forward_distance: 150.0 # [m] - object_check_backward_distance: 2.0 # [m] object_check_goal_distance: 20.0 # [m] # filtering parking objects threshold_distance_object_is_on_center: 1.0 # [m] @@ -128,6 +126,13 @@ # lost object compensation object_last_seen_threshold: 2.0 + # detection area generation parameters + detection_area: + static: true # [-] + min_forward_distance: 50.0 # [m] + max_forward_distance: 150.0 # [m] + backward_distance: 10.0 # [m] + # For safety check safety_check: # safety check configuration @@ -139,10 +144,16 @@ check_other_object: true # [-] # collision check parameters check_all_predicted_path: false # [-] - time_resolution: 0.5 # [s] - time_horizon: 10.0 # [s] safety_check_backward_distance: 100.0 # [m] - safety_check_hysteresis_factor: 2.0 # [-] + hysteresis_factor_expand_rate: 2.0 # [-] + hysteresis_factor_safe_count: 10 # [-] + # predicted path parameters + min_velocity: 1.38 # [m/s] + max_velocity: 50.0 # [m/s] + time_resolution: 0.5 # [s] + time_horizon_for_front_object: 10.0 # [s] + time_horizon_for_rear_object: 10.0 # [s] + delay_until_departure: 0.0 # [s] # rss parameters expected_front_deceleration: -1.0 # [m/ss] expected_rear_deceleration: -1.0 # [m/ss] @@ -163,6 +174,7 @@ hard_road_shoulder_margin: 0.3 # [m] max_right_shift_length: 5.0 max_left_shift_length: 5.0 + max_deviation_from_lane: 0.5 # [m] # avoidance distance parameters longitudinal: prepare_time: 2.0 # [s] diff --git a/planning/behavior_path_planner/config/behavior_path_planner.param.yaml b/planning/behavior_path_planner/config/behavior_path_planner.param.yaml index 95c60612bfdb7..d333cbfd778cb 100644 --- a/planning/behavior_path_planner/config/behavior_path_planner.param.yaml +++ b/planning/behavior_path_planner/config/behavior_path_planner.param.yaml @@ -1,6 +1,7 @@ /**: ros__parameters: verbose: false + max_iteration_num: 100 planning_hz: 10.0 backward_path_length: 5.0 forward_path_length: 300.0 diff --git a/planning/behavior_path_planner/config/drivable_area_expansion.param.yaml b/planning/behavior_path_planner/config/drivable_area_expansion.param.yaml index 1609cdbc60b7a..c0b6f259c7726 100644 --- a/planning/behavior_path_planner/config/drivable_area_expansion.param.yaml +++ b/planning/behavior_path_planner/config/drivable_area_expansion.param.yaml @@ -5,15 +5,19 @@ drivable_area_left_bound_offset: 0.0 drivable_area_types_to_skip: [road_border] - # Dynamic expansion by projecting the ego footprint along the path + # Dynamic expansion by using the path curvature dynamic_expansion: - enabled: false + enabled: true + print_runtime: false + max_expansion_distance: 0.0 # [m] maximum distance by which the drivable area can be expended (0.0 means no limit) + smoothing: + curvature_average_window: 3 # window size used for smoothing the curvatures using a moving window average + max_bound_rate: 1.0 # [m/m] maximum rate of change of the bound lateral distance over its arc length + extra_arc_length: 2.0 # [m] extra arc length where an expansion distance is initially applied ego: - extra_footprint_offset: - front: 0.5 # [m] extra length to add to the front of the ego footprint - rear: 0.5 # [m] extra length to add to the rear of the ego footprint - left: 0.5 # [m] extra length to add to the left of the ego footprint - right: 0.5 # [m] extra length to add to the rear of the ego footprint + extra_wheelbase: 0.0 # [m] extra length to add to the wheelbase + extra_front_overhang: 0.5 # [m] extra length to add to the front overhang + extra_width: 1.0 # [m] extra length to add to the width dynamic_objects: avoid: true # if true, the drivable area is not expanded in the predicted path of dynamic objects extra_footprint_offset: @@ -21,17 +25,11 @@ rear: 0.5 # [m] extra length to add to the rear of the dynamic object footprint left: 0.5 # [m] extra length to add to the left of the dynamic object footprint right: 0.5 # [m] extra length to add to the rear of the dynamic object footprint - expansion: - method: polygon # method used to expand the drivable area. Either 'lanelet' or 'polygon'. - # 'lanelet': add lanelets overlapped by the ego footprints - # 'polygon': add polygons built around sections of the ego footprint that go out of the drivable area - max_distance: 0.0 # [m] maximum distance by which the drivable area can be expended (0.0 means no limit) - max_path_arc_length: 50.0 # [m] maximum arc length along the path where the ego footprint is projected (0.0 means no limit) - extra_arc_length: 0.5 # [m] extra expansion arc length around an ego footprint + path_preprocessing: + max_arc_length: 50.0 # [m] maximum arc length along the path where the ego footprint is projected (0.0 means no limit) + resample_interval: 2.0 # [m] fixed interval between resampled path points (0.0 means path points are directly used) + reuse_max_deviation: 0.5 # [m] if the path changes by more than this value, the curvatures are recalculated. Otherwise they are reused. avoid_linestring: types: # linestring types in the lanelet maps that will not be crossed when expanding the drivable area - road_border distance: 0.0 # [m] distance to keep between the drivable area and the linestrings to avoid - compensate: - enable: true # if true, when the drivable area cannot be expanded in one direction to completely include the ego footprint, it is expanded in the opposite direction - extra_distance: 3.0 # [m] extra distance to add to the compensation diff --git a/planning/behavior_path_planner/config/dynamic_avoidance/dynamic_avoidance.param.yaml b/planning/behavior_path_planner/config/dynamic_avoidance/dynamic_avoidance.param.yaml index 701f05eb89ba1..127b70fbf7bcb 100644 --- a/planning/behavior_path_planner/config/dynamic_avoidance/dynamic_avoidance.param.yaml +++ b/planning/behavior_path_planner/config/dynamic_avoidance/dynamic_avoidance.param.yaml @@ -2,7 +2,7 @@ ros__parameters: dynamic_avoidance: common: - enable_debug_info: true + enable_debug_info: false use_hatched_road_markings: true # avoidance is performed for the object type with true @@ -16,6 +16,7 @@ motorcycle: true pedestrian: false + max_obstacle_vel: 100.0 # [m/s] min_obstacle_vel: 0.0 # [m/s] successive_num_to_entry_dynamic_avoidance_condition: 5 @@ -42,9 +43,13 @@ max_object_angle: 0.785 drivable_area_generation: + polygon_generation_method: "object_path_base" # choose "ego_path_base" and "object_path_base" + object_path_base: + min_longitudinal_polygon_margin: 3.0 # [m] + lat_offset_from_obstacle: 0.8 # [m] max_lat_offset_to_avoid: 0.5 # [m] - max_time_for_object_lat_shift: 2.0 # [s] + max_time_for_object_lat_shift: 0.0 # [s] lpf_gain_for_lat_avoid_to_offset: 0.9 # [-] # for same directional object diff --git a/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml b/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml index 39d05a7fb761b..5ffac49847ce6 100644 --- a/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml +++ b/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml @@ -2,14 +2,17 @@ ros__parameters: goal_planner: # general params - minimum_request_length: 100.0 th_arrived_distance: 1.0 th_stopped_velocity: 0.01 th_stopped_time: 2.0 # It must be greater than the state_machine's. + center_line_path_interval: 1.0 # goal search goal_search: - search_priority: "efficient_path" # "efficient_path" or "close_goal" + goal_priority: "minimum_weighted_distance" # "minimum_weighted_distance" or "minimum_longitudinal_distance" + minimum_weighted_distance: + lateral_weight: 40.0 + prioritize_goals_before_objects: true parking_policy: "left_side" # "left_side" or "right_side" forward_goal_search_length: 20.0 backward_goal_search_length: 20.0 @@ -17,30 +20,35 @@ longitudinal_margin: 3.0 max_lateral_offset: 0.5 lateral_offset_interval: 0.25 - ignore_distance_from_lane_start: 15.0 + ignore_distance_from_lane_start: 0.0 margin_from_boundary: 0.5 # occupancy grid map occupancy_grid: - use_occupancy_grid: true - use_occupancy_grid_for_longitudinal_margin: false + use_occupancy_grid_for_goal_search: true + use_occupancy_grid_for_goal_longitudinal_margin: false + use_occupancy_grid_for_path_collision_check: false occupancy_grid_collision_check_margin: 0.0 theta_size: 360 obstacle_threshold: 60 # object recognition object_recognition: - use_object_recognition: true + use_object_recognition: false object_recognition_collision_check_margin: 0.6 # this should be larger than `surround_check_distance` of surround_obstacle_checker object_recognition_collision_check_max_extra_stopping_margin: 1.0 + th_moving_object_velocity: 1.0 # pull over pull_over: + minimum_request_length: 100.0 pull_over_velocity: 3.0 pull_over_minimum_velocity: 1.38 decide_path_distance: 10.0 maximum_deceleration: 1.0 maximum_jerk: 1.0 + path_priority: "efficient_path" # "efficient_path" or "close_goal" + efficient_path_order: ["SHIFT", "ARC_FORWARD", "ARC_BACKWARD"] # only lane based pull over(exclude freespace parking) # shift parking shift_parking: @@ -104,6 +112,69 @@ neighbor_radius: 8.0 margin: 1.0 + stop_condition: + maximum_deceleration_for_stop: 1.0 + maximum_jerk_for_stop: 1.0 + path_safety_check: + # EgoPredictedPath + ego_predicted_path: + min_velocity: 0.0 + acceleration: 1.0 + max_velocity: 1.0 + time_horizon_for_front_object: 10.0 + time_horizon_for_rear_object: 10.0 + time_resolution: 0.5 + delay_until_departure: 1.0 + # For target object filtering + target_filtering: + safety_check_time_horizon: 5.0 + safety_check_time_resolution: 1.0 + # detection range + object_check_forward_distance: 10.0 + object_check_backward_distance: 100.0 + ignore_object_velocity_threshold: 1.0 + # ObjectTypesToCheck + object_types_to_check: + check_car: true + check_truck: true + check_bus: true + check_trailer: true + check_bicycle: true + check_motorcycle: true + check_pedestrian: true + check_unknown: false + # ObjectLaneConfiguration + object_lane_configuration: + check_current_lane: true + check_right_side_lane: true + check_left_side_lane: true + check_shoulder_lane: true + check_other_lane: false + include_opposite_lane: false + invert_opposite_lane: false + check_all_predicted_path: true + use_all_predicted_path: true + use_predicted_path_outside_lanelet: false + + # For safety check + safety_check_params: + # safety check configuration + enable_safety_check: true + # collision check parameters + check_all_predicted_path: true + publish_debug_marker: false + rss_params: + rear_vehicle_reaction_time: 2.0 + rear_vehicle_safety_time_margin: 1.0 + lateral_distance_max_threshold: 2.0 + longitudinal_distance_min_threshold: 3.0 + longitudinal_velocity_delta_time: 0.8 + # hysteresis factor to expand/shrink polygon with the value + hysteresis_factor_expand_rate: 1.0 + # temporary + backward_path_length: 30.0 + forward_path_length: 100.0 + # debug debug: print_debug_info: false diff --git a/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml b/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml index 925e70d9084d6..d3c0a22e867f9 100644 --- a/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml +++ b/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml @@ -28,15 +28,36 @@ # safety check safety_check: - expected_front_deceleration: -1.0 - expected_rear_deceleration: -1.0 - expected_front_deceleration_for_abort: -1.0 - expected_rear_deceleration_for_abort: -2.0 - rear_vehicle_reaction_time: 2.0 - rear_vehicle_safety_time_margin: 1.0 - lateral_distance_max_threshold: 2.0 - longitudinal_distance_min_threshold: 3.0 - longitudinal_velocity_delta_time: 0.8 + allow_loose_check_for_cancel: true + execution: + expected_front_deceleration: -1.0 + expected_rear_deceleration: -1.0 + rear_vehicle_reaction_time: 2.0 + rear_vehicle_safety_time_margin: 1.0 + lateral_distance_max_threshold: 2.0 + longitudinal_distance_min_threshold: 3.0 + longitudinal_velocity_delta_time: 0.8 + cancel: + expected_front_deceleration: -1.0 + expected_rear_deceleration: -2.0 + rear_vehicle_reaction_time: 1.5 + rear_vehicle_safety_time_margin: 0.8 + lateral_distance_max_threshold: 1.0 + longitudinal_distance_min_threshold: 2.5 + longitudinal_velocity_delta_time: 0.6 + stuck: + expected_front_deceleration: -1.0 + expected_rear_deceleration: -1.0 + rear_vehicle_reaction_time: 2.0 + rear_vehicle_safety_time_margin: 1.0 + lateral_distance_max_threshold: 2.0 + longitudinal_distance_min_threshold: 3.0 + longitudinal_velocity_delta_time: 0.8 + + # lane expansion for object filtering + lane_expansion: + left_offset: 0.0 # [m] + right_offset: 0.0 # [m] # lateral acceleration map lateral_acceleration: @@ -55,6 +76,16 @@ motorcycle: true pedestrian: true + # lane change regulations + regulation: + crosswalk: false + intersection: false + + # ego vehicle stuck detection + stuck_detection: + velocity: 0.1 # [m/s] + stop_time: 3.0 # [s] + # collision check enable_prepare_segment_collision_check: true prepare_segment_ignore_object_velocity_thresh: 0.1 # [m/s] diff --git a/planning/behavior_path_planner/config/scene_module_manager.param.yaml b/planning/behavior_path_planner/config/scene_module_manager.param.yaml index e89c5539a0965..5797704e8a0ca 100644 --- a/planning/behavior_path_planner/config/scene_module_manager.param.yaml +++ b/planning/behavior_path_planner/config/scene_module_manager.param.yaml @@ -5,80 +5,90 @@ ros__parameters: external_request_lane_change_left: enable_module: false - enable_rtc: true + enable_rtc: false enable_simultaneous_execution_as_approved_module: false enable_simultaneous_execution_as_candidate_module: true + keep_last: false priority: 6 max_module_size: 1 external_request_lane_change_right: enable_module: false - enable_rtc: true + enable_rtc: false enable_simultaneous_execution_as_approved_module: false enable_simultaneous_execution_as_candidate_module: true + keep_last: false priority: 6 max_module_size: 1 lane_change_left: enable_module: true - enable_rtc: true + enable_rtc: false enable_simultaneous_execution_as_approved_module: true enable_simultaneous_execution_as_candidate_module: true + keep_last: false priority: 5 max_module_size: 1 lane_change_right: enable_module: true - enable_rtc: true + enable_rtc: false enable_simultaneous_execution_as_approved_module: true enable_simultaneous_execution_as_candidate_module: true + keep_last: false priority: 5 max_module_size: 1 start_planner: enable_module: true - enable_rtc: true + enable_rtc: false enable_simultaneous_execution_as_approved_module: false enable_simultaneous_execution_as_candidate_module: false + keep_last: false priority: 0 max_module_size: 1 side_shift: enable_module: true - enable_rtc: true + enable_rtc: false enable_simultaneous_execution_as_approved_module: false enable_simultaneous_execution_as_candidate_module: false + keep_last: false priority: 2 max_module_size: 1 goal_planner: enable_module: true - enable_rtc: true + enable_rtc: false enable_simultaneous_execution_as_approved_module: false enable_simultaneous_execution_as_candidate_module: false + keep_last: true priority: 1 max_module_size: 1 avoidance: enable_module: true - enable_rtc: true + enable_rtc: false enable_simultaneous_execution_as_approved_module: true enable_simultaneous_execution_as_candidate_module: false + keep_last: false priority: 4 max_module_size: 1 avoidance_by_lc: enable_module: false - enable_rtc: true + enable_rtc: false enable_simultaneous_execution_as_approved_module: false enable_simultaneous_execution_as_candidate_module: false + keep_last: false priority: 3 max_module_size: 1 dynamic_avoidance: enable_module: false - enable_rtc: true + enable_rtc: false enable_simultaneous_execution_as_approved_module: true enable_simultaneous_execution_as_candidate_module: true + keep_last: false priority: 7 max_module_size: 1 diff --git a/planning/behavior_path_planner/config/start_planner/start_planner.param.yaml b/planning/behavior_path_planner/config/start_planner/start_planner.param.yaml index bea78664c65a3..4a42da9bc5ac3 100644 --- a/planning/behavior_path_planner/config/start_planner/start_planner.param.yaml +++ b/planning/behavior_path_planner/config/start_planner/start_planner.param.yaml @@ -7,6 +7,8 @@ collision_check_margin: 1.0 collision_check_distance_from_end: 1.0 th_moving_object_velocity: 1.0 + th_distance_to_middle_of_the_road: 0.1 + center_line_path_interval: 1.0 # shift pull out enable_shift_pull_out: true check_shift_path_lane_departure: false @@ -72,3 +74,66 @@ max_planning_time: 150.0 neighbor_radius: 8.0 margin: 1.0 + + stop_condition: + maximum_deceleration_for_stop: 1.0 + maximum_jerk_for_stop: 1.0 + path_safety_check: + # EgoPredictedPath + ego_predicted_path: + min_velocity: 0.0 + acceleration: 1.0 + max_velocity: 1.0 + time_horizon_for_front_object: 10.0 + time_horizon_for_rear_object: 10.0 + time_resolution: 0.5 + delay_until_departure: 1.0 + # For target object filtering + target_filtering: + safety_check_time_horizon: 5.0 + safety_check_time_resolution: 1.0 + # detection range + object_check_forward_distance: 10.0 + object_check_backward_distance: 100.0 + ignore_object_velocity_threshold: 1.0 + # ObjectTypesToCheck + object_types_to_check: + check_car: true + check_truck: true + check_bus: true + check_trailer: true + check_bicycle: true + check_motorcycle: true + check_pedestrian: true + check_unknown: false + # ObjectLaneConfiguration + object_lane_configuration: + check_current_lane: true + check_right_side_lane: true + check_left_side_lane: true + check_shoulder_lane: true + check_other_lane: false + include_opposite_lane: false + invert_opposite_lane: false + check_all_predicted_path: true + use_all_predicted_path: true + use_predicted_path_outside_lanelet: false + + # For safety check + safety_check_params: + # safety check configuration + enable_safety_check: true + # collision check parameters + check_all_predicted_path: true + publish_debug_marker: false + rss_params: + rear_vehicle_reaction_time: 2.0 + rear_vehicle_safety_time_margin: 1.0 + lateral_distance_max_threshold: 2.0 + longitudinal_distance_min_threshold: 3.0 + longitudinal_velocity_delta_time: 0.8 + # hysteresis factor to expand/shrink polygon + hysteresis_factor_expand_rate: 1.0 + # temporary + backward_path_length: 30.0 + forward_path_length: 100.0 diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_avoidance_design.md b/planning/behavior_path_planner/docs/behavior_path_planner_avoidance_design.md index 693d610951bbb..da96567ed542a 100644 --- a/planning/behavior_path_planner/docs/behavior_path_planner_avoidance_design.md +++ b/planning/behavior_path_planner/docs/behavior_path_planner_avoidance_design.md @@ -192,7 +192,7 @@ The avoidance target should be limited to stationary objects (you should not avo Not only the length from the centerline, but also the length from the road shoulder is calculated and used for the filtering process. It calculates the ratio of _the actual length between the the object's center and the center line_ `shift_length` and _the maximum length the object can shift_ `shiftable_length`. $$ -l_D = l_a - \frac{width}{2} \\ +l_D = l_a - \frac{width}{2}, \\ ratio = \frac{l_d}{l_D} $$ @@ -201,7 +201,7 @@ $$ - $l_a$ : distance between centerline and most left boundary. - $width$ : object width -The closer the object is to the shoulder, the larger the value of $ratio$ (theoretical max value is 1.0), and it compares the value and `object_check_shiftable_ratio` to determine whether the object is a parked-car. +The closer the object is to the shoulder, the larger the value of $ratio$ (theoretical max value is 1.0), and it compares the value and `object_check_shiftable_ratio` to determine whether the object is a parked-car. If the road has no road shoulders, it uses `object_check_min_road_shoulder_width` as a road shoulder width virtually. ![fig2](../image/avoidance/parked-car-detection.svg) @@ -692,7 +692,7 @@ namespace: `avoidance.target_filtering.` | object_check_backward_distance | [m] | double | Backward distance to search the avoidance target. | 2.0 | | object_check_goal_distance | [m] | double | Backward distance to search the avoidance target. | 20.0 | | object_check_shiftable_ratio | [m] | double | Vehicles around the center line within this distance will be excluded from avoidance target. | 0.6 | -| object_check_min_road_shoulder_width | [m] | double | Vehicles around the center line within this distance will be excluded from avoidance target. | 0.5 | +| object_check_min_road_shoulder_width | [m] | double | Width considered as a road shoulder if the lane does not have a road shoulder target. | 0.5 | | object_last_seen_threshold | [s] | double | For the compensation of the detection lost. The object is registered once it is observed as an avoidance target. When the detection loses, the timer will start and the object will be un-registered when the time exceeds this limit. | 2.0 | ### Safety check parameters diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_goal_planner_design.md b/planning/behavior_path_planner/docs/behavior_path_planner_goal_planner_design.md index 99dadc959f5a5..888d5d9ff9826 100644 --- a/planning/behavior_path_planner/docs/behavior_path_planner_goal_planner_design.md +++ b/planning/behavior_path_planner/docs/behavior_path_planner_goal_planner_design.md @@ -86,8 +86,12 @@ Either one is activated when all conditions are met. ### fixed_goal_planner -- The distance between the goal and ego-vehicle is shorter than `minimum_request_length`. - Route is set with `allow_goal_modification=false` by default. +- ego-vehicle is in the same lane as the goal. + +If the target path contains a goal, modify the points of the path so that the path and the goal are connected smoothly. This process will change the shape of the path by the distance of `refine_goal_search_radius_range` from the goal. Note that this logic depends on the interpolation algorithm that will be executed in a later module (at the moment it uses spline interpolation), so it needs to be updated in the future. + +![path_goal_refinement](../image/path_goal_refinement.drawio.svg) @@ -95,7 +99,7 @@ Either one is activated when all conditions are met. #### pull over on road lane -- The distance between the goal and ego-vehicle is shorter than `minimum_request_length`. +- The distance between the goal and ego-vehicle is shorter than `pull_over_minimum_request_length`. - Route is set with `allow_goal_modification=true` . - We can set this option with [SetRoute](https://github.com/autowarefoundation/autoware_adapi_msgs/blob/main/autoware_adapi_v1_msgs/routing/srv/SetRoute.srv#L2) api service. - We support `2D Rough Goal Pose` with the key bind `r` in RViz, but in the future there will be a panel of tools to manipulate various Route API from RViz. @@ -105,7 +109,7 @@ Either one is activated when all conditions are met. #### pull over on shoulder lane -- The distance between the goal and ego-vehicle is shorter than `minimum_request_length`. +- The distance between the goal and ego-vehicle is shorter than `pull_over_minimum_request_length`. - Goal is set in the `road_shoulder`. @@ -118,17 +122,12 @@ Either one is activated when all conditions are met. ## General parameters for goal_planner -| Name | Unit | Type | Description | Default value | -| :------------------------- | :----- | :----- | :-------------------------------------------------------------------------------------------------------------------------------------- | :------------ | -| minimum_request_length | [m] | double | when the ego-vehicle approaches the goal by this distance or a safe distance to stop, the module is activated. | 100.0 | -| th_arrived_distance | [m] | double | distance threshold for arrival of path termination | 1.0 | -| th_stopped_velocity | [m/s] | double | velocity threshold for arrival of path termination | 0.01 | -| th_stopped_time | [s] | double | time threshold for arrival of path termination | 2.0 | -| pull_over_velocity | [m/s] | double | decelerate to this speed by the goal search area | 3.0 | -| pull_over_minimum_velocity | [m/s] | double | speed of pull_over after stopping once. this prevents excessive acceleration. | 1.38 | -| margin_from_boundary | [m] | double | distance margin from edge of the shoulder lane | 0.5 | -| decide_path_distance | [m] | double | decide path if it approaches this distance relative to the parking position. after that, no path planning and goal search are performed | 10.0 | -| maximum_deceleration | [m/s2] | double | maximum deceleration. it prevents sudden deceleration when a parking path cannot be found suddenly | 1.0 | +| Name | Unit | Type | Description | Default value | +| :------------------------ | :---- | :----- | :------------------------------------------------- | :------------ | +| th_arrived_distance | [m] | double | distance threshold for arrival of path termination | 1.0 | +| th_stopped_velocity | [m/s] | double | velocity threshold for arrival of path termination | 0.01 | +| th_stopped_time | [s] | double | time threshold for arrival of path termination | 2.0 | +| center_line_path_interval | [m] | double | reference center line path point interval | 1.0 | ## **collision check** @@ -138,23 +137,24 @@ Generate footprints from ego-vehicle path points and determine obstacle collisio #### Parameters for occupancy grid based collision check -| Name | Unit | Type | Description | Default value | -| :----------------------------------------- | :--- | :----- | :-------------------------------------------------------------------------------------------------------------- | :------------ | -| use_occupancy_grid | [-] | bool | flag whether to use occupancy grid for collision check | true | -| use_occupancy_grid_for_longitudinal_margin | [-] | bool | flag whether to use occupancy grid for keeping longitudinal margin | false | -| occupancy_grid_collision_check_margin | [m] | double | margin to calculate ego-vehicle cells from footprint. | 0.0 | -| theta_size | [-] | int | size of theta angle to be considered. angular resolution for collision check will be 2$\pi$ / theta_size [rad]. | 360 | -| obstacle_threshold | [-] | int | threshold of cell values to be considered as obstacles | 60 | +| Name | Unit | Type | Description | Default value | +| :---------------------------------------------- | :--- | :----- | :-------------------------------------------------------------------------------------------------------------- | :------------ | +| use_occupancy_grid_for_goal_search | [-] | bool | flag whether to use occupancy grid for goal search collision check | true | +| use_occupancy_grid_for_goal_longitudinal_margin | [-] | bool | flag whether to use occupancy grid for keeping longitudinal margin | false | +| use_occupancy_grid_for_path_collision_check | [-] | bool | flag whether to use occupancy grid for collision check | false | +| occupancy_grid_collision_check_margin | [m] | double | margin to calculate ego-vehicle cells from footprint. | 0.0 | +| theta_size | [-] | int | size of theta angle to be considered. angular resolution for collision check will be 2$\pi$ / theta_size [rad]. | 360 | +| obstacle_threshold | [-] | int | threshold of cell values to be considered as obstacles | 60 | ### **object recognition based collision check** #### Parameters for object recognition based collision check -| Name | Unit | Type | Description | Default value | -| :----------------------------------------------------------- | :--- | :----- | :--------------------------------------------------------- | :------------ | ---------------------------------------------------------------------------------------------------------- | -| use_object_recognition | [-] | bool | flag whether to use object recognition for collision check | true | -| object_recognition_collision_check_margin | [m] | double | margin to calculate ego-vehicle cells from footprint. | 0.6 | -| object_recognition_collision_check_max_extra_stopping_margin | [m] | double | | 1.0 | 　maximum value when adding longitudinal distance margin for collision check considering stopping distance | +| Name | Unit | Type | Description | Default value | +| :----------------------------------------------------------- | :--- | :----- | :------------------------------------------------------------------------------------------------------- | :------------ | +| use_object_recognition | [-] | bool | flag whether to use object recognition for collision check | true | +| object_recognition_collision_check_margin | [m] | double | margin to calculate ego-vehicle cells from footprint. | 0.6 | +| object_recognition_collision_check_max_extra_stopping_margin | [m] | double | maximum value when adding longitudinal distance margin for collision check considering stopping distance | 1.0 | ## **Goal Search** @@ -165,21 +165,34 @@ searched for in certain range of the shoulder lane. ### Parameters for goal search -| Name | Unit | Type | Description | Default value | -| :------------------------------ | :--- | :----- | :----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------- | -| search_priority | [-] | string | In case `efficient_path` use a goal that can generate an efficient path( priority is `shift_parking` -> `arc_forward_parking` -> `arc_backward_parking`). In case `close_goal` use the closest goal to the original one. | efficient_path | -| forward_goal_search_length | [m] | double | length of forward range to be explored from the original goal | 20.0 | -| backward_goal_search_length | [m] | double | length of backward range to be explored from the original goal | 20.0 | -| goal_search_interval | [m] | double | distance interval for goal search | 2.0 | -| longitudinal_margin | [m] | double | margin between ego-vehicle at the goal position and obstacles | 3.0 | -| max_lateral_offset | [m] | double | maximum offset of goal search in the lateral direction | 0.5 | -| lateral_offset_interval | [m] | double | distance interval of goal search in the lateral direction | 0.25 | -| ignore_distance_from_lane_start | [m] | double | distance from start of pull over lanes for ignoring goal candidates | 15.0 | - -## **Path Generation** +| Name | Unit | Type | Description | Default value | +| :------------------------------ | :--- | :----- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------ | :-------------------------- | +| goal_priority | [-] | string | In case `minimum_weighted_distance`, sort with smaller longitudinal distances taking precedence over smaller lateral distances. In case `minimum_longitudinal_distance`, sort with weighted lateral distance against longitudinal distance. | `minimum_weighted_distance` | +| prioritize_goals_before_objects | [-] | bool | If there are objects that may need to be avoided, prioritize the goal in front of them | true | +| forward_goal_search_length | [m] | double | length of forward range to be explored from the original goal | 20.0 | +| backward_goal_search_length | [m] | double | length of backward range to be explored from the original goal | 20.0 | +| goal_search_interval | [m] | double | distance interval for goal search | 2.0 | +| longitudinal_margin | [m] | double | margin between ego-vehicle at the goal position and obstacles | 3.0 | +| max_lateral_offset | [m] | double | maximum offset of goal search in the lateral direction | 0.5 | +| lateral_offset_interval | [m] | double | distance interval of goal search in the lateral direction | 0.25 | +| ignore_distance_from_lane_start | [m] | double | distance from start of pull over lanes for ignoring goal candidates | 0.0 | +| ignore_distance_from_lane_start | [m] | double | distance from start of pull over lanes for ignoring goal candidates | 0.0 | +| margin_from_boundary | [m] | double | distance margin from edge of the shoulder lane | 0.5 | + +## **Pull Over** There are three path generation methods. -The path is generated with a certain margin (default: `0.5 m`) from left boundary of shoulder lane. +The path is generated with a certain margin (default: `0.5 m`) from the boundary of shoulder lane. + +| Name | Unit | Type | Description | Default value | +| :------------------------------- | :----- | :----- | :----------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :--------------------------------------- | +| pull_over_minimum_request_length | [m] | double | when the ego-vehicle approaches the goal by this distance or a safe distance to stop, pull over is activated. | 100.0 | +| pull_over_velocity | [m/s] | double | decelerate to this speed by the goal search area | 3.0 | +| pull_over_minimum_velocity | [m/s] | double | speed of pull_over after stopping once. this prevents excessive acceleration. | 1.38 | +| decide_path_distance | [m] | double | decide path if it approaches this distance relative to the parking position. after that, no path planning and goal search are performed | 10.0 | +| maximum_deceleration | [m/s2] | double | maximum deceleration. it prevents sudden deceleration when a parking path cannot be found suddenly | 1.0 | +| path_priority | [-] | string | In case `efficient_path` use a goal that can generate an efficient path which is set in `efficient_path_order`. In case `close_goal` use the closest goal to the original one. | efficient_path | +| efficient_path_order | [-] | string | efficient order of pull over planner along lanes　excluding freespace pull over | ["SHIFT", "ARC_FORWARD", "ARC_BACKWARD"] | ### **shift parking** @@ -257,7 +270,8 @@ If the vehicle gets stuck with `lane_parking`, run `freespace_parking`. To run this feature, you need to set `parking_lot` to the map, `activate_by_scenario` of [costmap_generator](../../costmap_generator/README.md) to `false` and `enable_freespace_parking` to `true` ![pull_over_freespace_parking_flowchart](../image/pull_over_freespace_parking_flowchart.drawio.svg) -\*Series execution with `avoidance_module` in the flowchart is under development. + +Simultaneous execution with `avoidance_module` in the flowchart is under development. diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_lane_change_design.md b/planning/behavior_path_planner/docs/behavior_path_planner_lane_change_design.md index fddd535931d94..2e5e486afc7c3 100644 --- a/planning/behavior_path_planner/docs/behavior_path_planner_lane_change_design.md +++ b/planning/behavior_path_planner/docs/behavior_path_planner_lane_change_design.md @@ -55,12 +55,12 @@ where `common_param` is vehicle common parameter, which defines vehicle common m The `longitudinal_acceleration_resolution` is determine by the following ```C++ -longitudinal_acceleration_resolution = (maximum_longitudinal_acceleration - minimum_longitudinal_deceleration) / longitudinal_acceleration_sampling_num +longitudinal_acceleration_resolution = (maximum_longitudinal_acceleration - minimum_longitudinal_acceleration) / longitudinal_acceleration_sampling_num ``` Note that when the `current_velocity` is lower than `minimum_lane_changing_velocity`, the vehicle needs to accelerate its velocity to `minimum_lane_changing_velocity`. Therefore, longitudinal acceleration becomes positive value (not decelerate). -The following figure illustrates when `lane_change_sampling_num = 4`. Assuming that `maximum_deceleration = 1.0` then `a0 == 0.0 == no deceleration`, `a1 == 0.25`, `a2 == 0.5`, `a3 == 0.75` and `a4 == 1.0 == maximum_deceleration`. `a0` is the expected lane change trajectories should ego vehicle do not decelerate, and `a1`'s path is the expected lane change trajectories should ego vehicle decelerate at `0.25 m/s^2`. +The following figure illustrates when `longitudinal_acceleration_sampling_num = 4`. Assuming that `maximum_deceleration = 1.0` then `a0 == 0.0 == no deceleration`, `a1 == 0.25`, `a2 == 0.5`, `a3 == 0.75` and `a4 == 1.0 == maximum_deceleration`. `a0` is the expected lane change trajectories should ego vehicle do not decelerate, and `a1`'s path is the expected lane change trajectories should ego vehicle decelerate at `0.25 m/s^2`. ![path_samples](../image/lane_change/lane_change-candidate_path_samples.png) @@ -79,12 +79,12 @@ The maximum and minimum lateral accelerations are defined in the lane change par | 4.0 | 0.3 | 0.4 | | 6.0 | 0.3 | 0.5 | -In this case, when the current velocity of the ego vehicle is 3.0, the minimum and maximum lateral accelerations are 0.2 and 0.35 respectively. These values are obtained by linearly interpolating the second and third rows of the map, which provide the minimum and maximum lateral acceleration values. +In this case, when the current velocity of the ego vehicle is 3.0, the minimum and maximum lateral accelerations are 0.25 and 0.4 respectively. These values are obtained by linearly interpolating the second and third rows of the map, which provide the minimum and maximum lateral acceleration values. Within this range, we sample the lateral acceleration for the ego vehicle. Similar to the method used for sampling longitudinal acceleration, the resolution of lateral acceleration (lateral_acceleration_resolution) is determined by the following: ```C++ -lateral_acceleration_resolution = (maximum_lateral_acceleration - minimum_lateral_deceleration) / lateral_acceleration_sampling_num +lateral_acceleration_resolution = (maximum_lateral_acceleration - minimum_lateral_acceleration) / lateral_acceleration_sampling_num ``` #### Candidate Path's validity check @@ -163,7 +163,7 @@ First, we divide the target objects into obstacles in the target lane, obstacles ![object lanes](../image/lane_change/lane_objects.drawio.svg) -Furthermore, to change lanes behind a vehicle waiting at a traffic light, we skip the safety check for the stopping vehicles near the traffic light. +Furthermore, to change lanes behind a vehicle waiting at a traffic light, we skip the safety check for the stopping vehicles near the traffic light.　The explanation for parked car detection is written in [documentation for avoidance module](../docs/behavior_path_planner_avoidance_design.md). ##### Collision check in prepare phase @@ -186,6 +186,14 @@ The following figure illustrates when the lane is blocked in multiple lane chang ![multiple-lane-changes](../image/lane_change/lane_change-when_cannot_change_lanes.png) +### Lane change regulations + +If you want to regulate lane change on crosswalks or intersections, the lane change module finds a lane change path excluding it includes crosswalks or intersections. +To regulate lane change on crosswalks or intersections, change `regulation.crosswalk` or `regulation.intersection` to `true`. +If the ego vehicle gets stuck, to avoid stuck, it enables lane change in crosswalk/intersection. +If the ego vehicle stops more than `stuck_detection.stop_time` seconds, it is regarded as a stuck. +If the ego vehicle velocity is smaller than `stuck_detection.velocity`, it is regarded as stopping. + ### Aborting lane change The abort process may result in three different outcome; Cancel, Abort and Stop/Cruise. @@ -276,11 +284,10 @@ The following parameters are configurable in `lane_change.param.yaml`. | `prediction_time_resolution` | [s] | double | Time resolution for object's path interpolation and collision check. | 0.5 | | `longitudinal_acceleration_sampling_num` | [-] | int | Number of possible lane-changing trajectories that are being influenced by longitudinal acceleration | 5 | | `lateral_acceleration_sampling_num` | [-] | int | Number of possible lane-changing trajectories that are being influenced by lateral acceleration | 3 | -| `object_check_min_road_shoulder_width` | [m] | double | Vehicles around the center line within this distance will be excluded from parking objects | 0.5 | +| `object_check_min_road_shoulder_width` | [m] | double | Width considered as a road shoulder if the lane does not have a road shoulder | 0.5 | | `object_shiftable_ratio_threshold` | [-] | double | Vehicles around the center line within this distance ratio will be excluded from parking objects | 0.6 | | `min_length_for_turn_signal_activation` | [m] | double | Turn signal will be activated if the ego vehicle approaches to this length from minimum lane change length | 10.0 | | `length_ratio_for_turn_signal_deactivation` | [-] | double | Turn signal will be deactivated if the ego vehicle approaches to this length ratio for lane change finish point | 0.8 | -| `object_shiftable_ratio_threshold` | [-] | double | Vehicles around the center line within this distance ratio will be excluded from parking objects | 0.6 | | `max_longitudinal_acc` | [-] | double | maximum longitudinal acceleration for lane change | 1.0 | | `min_longitudinal_acc` | [-] | double | maximum longitudinal deceleration for lane change | -1.0 | | `lateral_acceleration.velocity` | [m/s] | double | Reference velocity for lateral acceleration calculation (look up table) | [0.0, 4.0, 10.0] | @@ -295,6 +302,20 @@ The following parameters are configurable in `lane_change.param.yaml`. | `target_object.motorcycle` | [-] | boolean | Include motorcycle objects for safety check | true | | `target_object.pedestrian` | [-] | boolean | Include pedestrian objects for safety check | true | +### Lane change regulations + +| Name | Unit | Type | Description | Default value | +| :------------------------ | ---- | ------- | ------------------------------------- | ------------- | +| `regulation.crosswalk` | [-] | boolean | Regulate lane change on crosswalks | false | +| `regulation.intersection` | [-] | boolean | Regulate lane change on intersections | false | + +### Ego vehicle stuck detection + +| Name | Unit | Type | Description | Default value | +| :-------------------------- | ----- | ------ | --------------------------------------------------- | ------------- | +| `stuck_detection.velocity` | [m/s] | double | Velocity threshold for ego vehicle stuck detection | 0.1 | +| `stuck_detection.stop_time` | [s] | double | Stop time threshold for ego vehicle stuck detection | 3.0 | + ### Collision checks during lane change The following parameters are configurable in `behavior_path_planner.param.yaml` and `lane_change.param.yaml`. diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_start_planner_design.md b/planning/behavior_path_planner/docs/behavior_path_planner_start_planner_design.md index 2b98775e4fe81..131202038c270 100644 --- a/planning/behavior_path_planner/docs/behavior_path_planner_start_planner_design.md +++ b/planning/behavior_path_planner/docs/behavior_path_planner_start_planner_design.md @@ -8,11 +8,11 @@ This module is activated when a new route is received. Use cases are as follows - start smoothly from the current ego position to centerline. - - ![case1](../image/start_from_road_lane.drawio.svg) + ![case1](../image/start_from_road_lane.drawio.svg) - pull out from the side of the road lane to centerline. - - ![case2](../image/start_from_road_side.drawio.svg) + ![case2](../image/start_from_road_side.drawio.svg) - pull out from the shoulder lane to the road lane centerline. - - ![case3](../image/start_from_road_shoulder.drawio.svg) + ![case3](../image/start_from_road_shoulder.drawio.svg) ## Design @@ -59,16 +59,18 @@ PullOutPath --o PullOutPlannerBase ## General parameters for start_planner -| Name | Unit | Type | Description | Default value | -| :---------------------------------------------------------- | :---- | :----- | :------------------------------------------------------------------- | :------------ | -| th_arrived_distance_m | [m] | double | distance threshold for arrival of path termination | 1.0 | -| th_stopped_velocity_mps | [m/s] | double | velocity threshold for arrival of path termination | 0.01 | -| th_stopped_time_sec | [s] | double | time threshold for arrival of path termination | 1.0 | -| th_turn_signal_on_lateral_offset | [m] | double | lateral distance threshold for turning on blinker | 1.0 | -| intersection_search_length | [m] | double | check if intersections exist within this length | 30.0 | -| length_ratio_for_turn_signal_deactivation_near_intersection | [m] | double | deactivate turn signal of this module near intersection | 0.5 | -| collision_check_margin | [m] | double | Obstacle collision check margin | 1.0 | -| collision_check_distance_from_end | [m] | double | collision check distance from end point. currently only for pull out | 15.0 | +| Name | Unit | Type | Description | Default value | +| :---------------------------------------------------------- | :---- | :----- | :-------------------------------------------------------------------------- | :------------ | +| th_arrived_distance_m | [m] | double | distance threshold for arrival of path termination | 1.0 | +| th_distance_to_middle_of_the_road | [m] | double | distance threshold to determine if the vehicle is on the middle of the road | 0.1 | +| th_stopped_velocity_mps | [m/s] | double | velocity threshold for arrival of path termination | 0.01 | +| th_stopped_time_sec | [s] | double | time threshold for arrival of path termination | 1.0 | +| th_turn_signal_on_lateral_offset | [m] | double | lateral distance threshold for turning on blinker | 1.0 | +| intersection_search_length | [m] | double | check if intersections exist within this length | 30.0 | +| length_ratio_for_turn_signal_deactivation_near_intersection | [m] | double | deactivate turn signal of this module near intersection | 0.5 | +| collision_check_margin | [m] | double | Obstacle collision check margin | 1.0 | +| collision_check_distance_from_end | [m] | double | collision check distance from end point. currently only for pull out | 15.0 | +| center_line_path_interval | [m] | double | reference center line path point interval | 1.0 | ## Safety check with static obstacles @@ -80,8 +82,108 @@ PullOutPath --o PullOutPlannerBase ## Safety check with dynamic obstacles +### **Basic concept of safety check against dynamic obstacles** + +This is based on the concept of RSS. For the logic used, refer to the link below. +See [safety check feature explanation](../docs/behavior_path_planner_safety_check.md) + +### **Collision check performed range** + +A collision check with dynamic objects is primarily performed between the shift start point and end point. The range for safety check varies depending on the type of path generated, so it will be explained for each pattern. + +#### **Shift pull out** + +For the "shift pull out", safety verification starts at the beginning of the shift and ends at the shift's conclusion. + +#### **Geometric pull out** + +Since there's a stop at the midpoint during the shift, this becomes the endpoint for safety verification. After stopping, safety verification resumes. + +#### **Backward pull out start point search** + +During backward movement, no safety check is performed. Safety check begins at the point where the backward movement ends. + +![collision_check_range](../image/start_planner/collision_check_range.drawio.svg) + +### **Ego vehicle's velocity planning** + WIP +### **Safety check in free space area** + +WIP + +## Parameters for safety check + +### Stop Condition Parameters + +Parameters under `stop_condition` define the criteria for stopping conditions. + +| Name | Unit | Type | Description | Default value | +| :---------------------------- | :------ | :----- | :-------------------------------------- | :------------ | +| maximum_deceleration_for_stop | [m/s^2] | double | Maximum deceleration allowed for a stop | 1.0 | +| maximum_jerk_for_stop | [m/s^3] | double | Maximum jerk allowed for a stop | 1.0 | + +### Ego Predicted Path Parameters + +Parameters under `path_safety_check.ego_predicted_path` specify the ego vehicle's predicted path characteristics. + +| Name | Unit | Type | Description | Default value | +| :---------------------------- | :------ | :----- | :--------------------------------------------------- | :------------ | +| min_velocity | [m/s] | double | Minimum velocity of the ego vehicle's predicted path | 0.0 | +| acceleration | [m/s^2] | double | Acceleration for the ego vehicle's predicted path | 1.0 | +| max_velocity | [m/s] | double | Maximum velocity of the ego vehicle's predicted path | 1.0 | +| time_horizon_for_front_object | [s] | double | Time horizon for predicting front objects | 10.0 | +| time_horizon_for_rear_object | [s] | double | Time horizon for predicting rear objects | 10.0 | +| time_resolution | [s] | double | Time resolution for the ego vehicle's predicted path | 0.5 | +| delay_until_departure | [s] | double | Delay until the ego vehicle departs | 1.0 | + +### Target Object Filtering Parameters + +Parameters under `target_filtering` are related to filtering target objects for safety check. + +| Name | Unit | Type | Description | Default value | +| :---------------------------------------------- | :---- | :----- | :------------------------------------------------- | :------------ | +| safety_check_time_horizon | [s] | double | Time horizon for safety check | 5.0 | +| safety_check_time_resolution | [s] | double | Time resolution for safety check | 1.0 | +| object_check_forward_distance | [m] | double | Forward distance for object detection | 10.0 | +| object_check_backward_distance | [m] | double | Backward distance for object detection | 100.0 | +| ignore_object_velocity_threshold | [m/s] | double | Velocity threshold below which objects are ignored | 1.0 | +| object_types_to_check.check_car | - | bool | Flag to check cars | true | +| object_types_to_check.check_truck | - | bool | Flag to check trucks | true | +| object_types_to_check.check_bus | - | bool | Flag to check buses | true | +| object_types_to_check.check_trailer | - | bool | Flag to check trailers | true | +| object_types_to_check.check_bicycle | - | bool | Flag to check bicycles | true | +| object_types_to_check.check_motorcycle | - | bool | Flag to check motorcycles | true | +| object_types_to_check.check_pedestrian | - | bool | Flag to check pedestrians | true | +| object_types_to_check.check_unknown | - | bool | Flag to check unknown object types | false | +| object_lane_configuration.check_current_lane | - | bool | Flag to check the current lane | true | +| object_lane_configuration.check_right_side_lane | - | bool | Flag to check the right side lane | true | +| object_lane_configuration.check_left_side_lane | - | bool | Flag to check the left side lane | true | +| object_lane_configuration.check_shoulder_lane | - | bool | Flag to check the shoulder lane | true | +| object_lane_configuration.check_other_lane | - | bool | Flag to check other lanes | false | +| include_opposite_lane | - | bool | Flag to include the opposite lane in check | false | +| invert_opposite_lane | - | bool | Flag to invert the opposite lane check | false | +| check_all_predicted_path | - | bool | Flag to check all predicted paths | true | +| use_all_predicted_path | - | bool | Flag to use all predicted paths | true | +| use_predicted_path_outside_lanelet | - | bool | Flag to use predicted paths outside of lanelets | false | + +### Safety Check Parameters + +Parameters under `safety_check_params` define the configuration for safety check. + +| Name | Unit | Type | Description | Default value | +| :--------------------------------------------- | :--- | :----- | :------------------------------------------ | :------------ | +| enable_safety_check | - | bool | Flag to enable safety check | true | +| check_all_predicted_path | - | bool | Flag to check all predicted paths | true | +| publish_debug_marker | - | bool | Flag to publish debug markers | false | +| rss_params.rear_vehicle_reaction_time | [s] | double | Reaction time for rear vehicles | 2.0 | +| rss_params.rear_vehicle_safety_time_margin | [s] | double | Safety time margin for rear vehicles | 1.0 | +| rss_params.lateral_distance_max_threshold | [m] | double | Maximum lateral distance threshold | 2.0 | +| rss_params.longitudinal_distance_min_threshold | [m] | double | Minimum longitudinal distance threshold | 3.0 | +| rss_params.longitudinal_velocity_delta_time | [s] | double | Delta time for longitudinal velocity | 0.8 | +| hysteresis_factor_expand_rate | - | double | Rate to expand/shrink the hysteresis factor | 1.0 | + ## **Path Generation** There are two path generation methods. @@ -111,9 +213,7 @@ Pull out distance is calculated by the speed, lateral deviation, and the lateral | maximum_lateral_jerk | [m/s3] | double | maximum lateral jerk | 2.0 | | minimum_lateral_jerk | [m/s3] | double | minimum lateral jerk | 0.1 | | minimum_shift_pull_out_distance | [m] | double | minimum shift pull out distance. if calculated pull out distance is shorter than this, use this for path generation. | 0.0 | -| maximum_curvature | [m] | double | maximum curvature. The pull out distance is calculated so that the curvature is smaller than this value. | - -| 0.07 | +| maximum_curvature | [m] | double | maximum curvature. The pull out distance is calculated so that the curvature is smaller than this value. | 0.07 | ### **geometric pull out** @@ -126,14 +226,14 @@ See also [[1]](https://www.sciencedirect.com/science/article/pii/S14746670153474 #### parameters for geometric pull out -| Name | Unit | Type | Description | Default value | -| :-------------------------- | :---- | :----- | :--------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | -| enable_geometric_pull_out | [-] | bool | flag whether to enable geometric pull out | true | -| divide_pull_out_path | [-] | bool | flag whether to divide arc paths.　The path is assumed to be divided because the curvature is not continuous. But it requires a stop during the departure. | false | -| geometric_pull_out_velocity | [m/s] | double | velocity of geometric pull out | 1.0 | -| arc_path_interval | [m] | double | path points interval of arc paths of geometric pull out | 1.0 | -| lane_departure_margin | [m] | double | margin of deviation to lane right | 0.2 | -| pull_out_max_steer_angle | [rad] | double | maximum steer angle for path generation | 0.26 | +| Name | Unit | Type | Description | Default value | +| :-------------------------- | :---- | :----- | :-------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | +| enable_geometric_pull_out | [-] | bool | flag whether to enable geometric pull out | true | +| divide_pull_out_path | [-] | bool | flag whether to divide arc paths. The path is assumed to be divided because the curvature is not continuous. But it requires a stop during the departure. | false | +| geometric_pull_out_velocity | [m/s] | double | velocity of geometric pull out | 1.0 | +| arc_path_interval | [m] | double | path points interval of arc paths of geometric pull out | 1.0 | +| lane_departure_margin | [m] | double | margin of deviation to lane right | 0.2 | +| pull_out_max_steer_angle | [rad] | double | maximum steer angle for path generation | 0.26 | ## **backward pull out start point search** @@ -153,3 +253,27 @@ If a safe path cannot be generated from the current position, search backwards f | backward_search_resolution | [m] | double | distance interval for searching backward pull out start point | 2.0 | | backward_path_update_duration | [s] | double | time interval for searching backward pull out start point. this prevents chattering between back driving and pull_out | 3.0 | | ignore_distance_from_lane_end | [m] | double | distance from end of pull out lanes for ignoring start candidates | 15.0 | + +### **freespace pull out** + +If the vehicle gets stuck with pull out along lanes, execute freespace pull out. +To run this feature, you need to set `parking_lot` to the map, `activate_by_scenario` of [costmap_generator](../../costmap_generator/README.md) to `false` and `enable_freespace_planner` to `true` + + + +#### Unimplemented parts / limitations for freespace pull out + +- When a short path is generated, the ego does can not drive with it. +- Complex cases take longer to generate or fail. +- The drivable area is not guaranteed to fit in the parking_lot. + +#### Parameters freespace parking + +| Name | Unit | Type | Description | Default value | +| :----------------------------- | :--- | :----- | :--------------------------------------------------------------------------------------------------------------------------------------- | :------------ | +| enable_freespace_planner | [-] | bool | this flag activates a free space pullout that is executed when a vehicle is stuck due to obstacles in the lanes where the ego is located | true | +| end_pose_search_start_distance | [m] | double | distance from ego to the start point of the search for the end point in the freespace_pull_out driving lane | 20.0 | +| end_pose_search_end_distance | [m] | double | distance from ego to the end point of the search for the end point in the freespace_pull_out driving lane | 30.0 | +| end_pose_search_interval | [m] | bool | interval to search for the end point in the freespace_pull_out driving lane | 2.0 | + +See [freespace_planner](../../freespace_planner/README.md) for other parameters. diff --git a/planning/behavior_path_planner/image/start_planner/collision_check_range.drawio.svg b/planning/behavior_path_planner/image/start_planner/collision_check_range.drawio.svg new file mode 100644 index 0000000000000..064f3602d6b6a --- /dev/null +++ b/planning/behavior_path_planner/image/start_planner/collision_check_range.drawio.svg @@ -0,0 +1,119 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
safety check range
+
+ + +
+
no-safety check range
+
+ + +
+
safety check start point with vehicle frame
+
+ + +
+
safety check end point with vehicle frame
+
+ + 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 54cfccc494e8a..420a5a8aa6ee5 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 @@ -17,14 +17,9 @@ #include "behavior_path_planner/data_manager.hpp" #include "behavior_path_planner/planner_manager.hpp" -#include "behavior_path_planner/scene_module/avoidance/manager.hpp" -#include "behavior_path_planner/scene_module/dynamic_avoidance/manager.hpp" -#include "behavior_path_planner/scene_module/goal_planner/manager.hpp" -#include "behavior_path_planner/scene_module/lane_change/manager.hpp" #include "behavior_path_planner/scene_module/scene_module_interface.hpp" -#include "behavior_path_planner/scene_module/side_shift/manager.hpp" -#include "behavior_path_planner/scene_module/start_planner/manager.hpp" #include "behavior_path_planner/steering_factor_interface.hpp" +#include "tier4_autoware_utils/ros/logger_level_configure.hpp" #include "tier4_planning_msgs/msg/detail/lane_change_debug_msg_array__struct.hpp" #include @@ -65,7 +60,6 @@ using autoware_auto_vehicle_msgs::msg::HazardLightsCommand; using autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand; using autoware_planning_msgs::msg::LaneletRoute; using autoware_planning_msgs::msg::PoseWithUuidStamped; -using geometry_msgs::msg::TwistStamped; using nav_msgs::msg::OccupancyGrid; using nav_msgs::msg::Odometry; using rcl_interfaces::msg::SetParametersResult; @@ -87,6 +81,9 @@ class BehaviorPathPlannerNode : public rclcpp::Node // Getter method for waiting approval modules std::vector getWaitingApprovalModules(); + // Getter method for running modules + std::vector getRunningModules(); + private: rclcpp::Subscription::SharedPtr route_subscriber_; rclcpp::Subscription::SharedPtr vector_map_subscriber_; @@ -218,6 +215,8 @@ class BehaviorPathPlannerNode : public rclcpp::Node Path convertToPath( const std::shared_ptr & path_candidate_ptr, const bool is_ready, const std::shared_ptr & planner_data); + + std::unique_ptr logger_configure_; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp index 5b252cef92714..9280a81e643ca 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp @@ -18,6 +18,7 @@ #include "behavior_path_planner/parameters.hpp" #include "behavior_path_planner/turn_signal_decider.hpp" #include "behavior_path_planner/utils/drivable_area_expansion/parameters.hpp" +#include "motion_utils/trajectory/trajectory.hpp" #include #include @@ -29,12 +30,13 @@ #include #include #include +#include #include #include #include #include -#include +#include #include #include @@ -139,15 +141,15 @@ struct PlannerData OccupancyGrid::ConstSharedPtr costmap{}; LateralOffset::ConstSharedPtr lateral_offset{}; OperationModeState::ConstSharedPtr operation_mode{}; - PathWithLaneId::SharedPtr reference_path{std::make_shared()}; PathWithLaneId::SharedPtr prev_output_path{std::make_shared()}; std::optional prev_modified_goal{}; std::optional prev_route_id{}; - lanelet::ConstLanelets current_lanes{}; std::shared_ptr route_handler{std::make_shared()}; BehaviorPathPlannerParameters parameters{}; drivable_area_expansion::DrivableAreaExpansionParameters drivable_area_expansion_parameters{}; + mutable std::vector drivable_area_expansion_prev_path_poses{}; + mutable std::vector drivable_area_expansion_prev_curvatures{}; mutable TurnSignalDecider turn_signal_decider; TurnIndicatorsCommand getTurnSignal( diff --git a/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/avoidance/debug.hpp b/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/avoidance/debug.hpp index ac24591a0a283..100e8046f8d7f 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/avoidance/debug.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/avoidance/debug.hpp @@ -25,8 +25,7 @@ #include #include -#include -#include +#include #include #include @@ -59,8 +58,6 @@ MarkerArray createTargetObjectsMarkerArray(const ObjectDataArray & objects, cons MarkerArray createOtherObjectsMarkerArray(const ObjectDataArray & objects, const std::string & ns); -MarkerArray createUnsafeObjectsMarkerArray(const ObjectDataArray & objects, std::string && ns); - MarkerArray makeOverhangToRoadShoulderMarkerArray( const behavior_path_planner::ObjectDataArray & objects, std::string && ns); diff --git a/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/colors.hpp b/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/colors.hpp new file mode 100644 index 0000000000000..0dee53d4f68af --- /dev/null +++ b/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/colors.hpp @@ -0,0 +1,94 @@ +// Copyright 2023 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__MARKER_UTILS__COLORS_HPP_ +#define BEHAVIOR_PATH_PLANNER__MARKER_UTILS__COLORS_HPP_ + +#include "tier4_autoware_utils/ros/marker_helper.hpp" + +#include "std_msgs/msg/color_rgba.hpp" + +#include + +namespace marker_utils::colors +{ +using std_msgs::msg::ColorRGBA; + +inline ColorRGBA red(float a = 0.99) +{ + return tier4_autoware_utils::createMarkerColor(1., 0., 0., a); +} + +inline ColorRGBA green(float a = 0.99) +{ + return tier4_autoware_utils::createMarkerColor(0., 1., 0., a); +} + +inline ColorRGBA blue(float a = 0.99) +{ + return tier4_autoware_utils::createMarkerColor(0., 0., 1., a); +} + +inline ColorRGBA yellow(float a = 0.99) +{ + return tier4_autoware_utils::createMarkerColor(1., 1., 0., a); +} + +inline ColorRGBA aqua(float a = 0.99) +{ + return tier4_autoware_utils::createMarkerColor(0., 1., 1., a); +} + +inline ColorRGBA magenta(float a = 0.99) +{ + return tier4_autoware_utils::createMarkerColor(1., 0., 1., a); +} + +inline ColorRGBA medium_orchid(float a = 0.99) +{ + return tier4_autoware_utils::createMarkerColor(0.729, 0.333, 0.827, a); +} + +inline ColorRGBA light_pink(float a = 0.99) +{ + return tier4_autoware_utils::createMarkerColor(1., 0.713, 0.756, a); +} + +inline ColorRGBA light_yellow(float a = 0.99) +{ + return tier4_autoware_utils::createMarkerColor(1., 1., 0.878, a); +} + +inline ColorRGBA light_steel_blue(float a = 0.99) +{ + return tier4_autoware_utils::createMarkerColor(0.690, 0.768, 0.870, a); +} + +inline ColorRGBA white(float a = 0.99) +{ + return tier4_autoware_utils::createMarkerColor(1., 1., 1., a); +} + +inline ColorRGBA grey(float a = 0.99) +{ + return tier4_autoware_utils::createMarkerColor(.5, .5, .5, a); +} + +inline std::vector colors_list(float a = 0.99) +{ + return {red(a), green(a), blue(a), yellow(a), aqua(a), + magenta(a), medium_orchid(a), light_pink(a), light_yellow(a), light_steel_blue(a)}; +} +} // namespace marker_utils::colors + +#endif // BEHAVIOR_PATH_PLANNER__MARKER_UTILS__COLORS_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/lane_change/debug.hpp b/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/lane_change/debug.hpp index 6bcc9cee877b4..1d0c9f5c2ce65 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/lane_change/debug.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/lane_change/debug.hpp @@ -17,6 +17,7 @@ #include "behavior_path_planner/marker_utils/utils.hpp" #include "behavior_path_planner/utils/lane_change/lane_change_path.hpp" +#include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include #include @@ -25,22 +26,16 @@ namespace marker_utils::lane_change_markers { using behavior_path_planner::LaneChangePath; -using marker_utils::CollisionCheckDebug; +using behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObjects; using visualization_msgs::msg::MarkerArray; -MarkerArray showObjectInfo( - const std::unordered_map & obj_debug_vec, std::string && ns); MarkerArray showAllValidLaneChangePath( const std::vector & lanes, std::string && ns); -MarkerArray showLerpedPose( - const std::unordered_map & obj_debug_vec, std::string && ns); -MarkerArray showEgoPredictedPaths( - const std::unordered_map & obj_debug_vec, std::string && ns); -MarkerArray showPolygon( - const std::unordered_map & obj_debug_vec, std::string && ns); -MarkerArray showPolygonPose( - const std::unordered_map & obj_debug_vec, std::string && ns); MarkerArray createLaneChangingVirtualWallMarker( const geometry_msgs::msg::Pose & lane_changing_pose, const std::string & module_name, const rclcpp::Time & now, const std::string & ns); +MarkerArray showFilteredObjects( + const ExtendedPredictedObjects & current_lane_objects, + const ExtendedPredictedObjects & target_lane_objects, + const ExtendedPredictedObjects & other_lane_objects, const std::string & ns); } // namespace marker_utils::lane_change_markers #endif // BEHAVIOR_PATH_PLANNER__MARKER_UTILS__LANE_CHANGE__DEBUG_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/utils.hpp index b3cb53ec18349..86f1a390261c6 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/utils.hpp @@ -15,10 +15,8 @@ #define BEHAVIOR_PATH_PLANNER__MARKER_UTILS__UTILS_HPP_ #include "behavior_path_planner/data_manager.hpp" +#include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include "behavior_path_planner/utils/path_shifter/path_shifter.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" - -#include #include #include @@ -26,10 +24,10 @@ #include #include -#include +#include +#include #include -#include #include namespace marker_utils @@ -39,6 +37,10 @@ using autoware_auto_perception_msgs::msg::PredictedPath; using autoware_auto_planning_msgs::msg::PathWithLaneId; using behavior_path_planner::DrivableLanes; using behavior_path_planner::ShiftLineArray; +using behavior_path_planner::utils::path_safety_checker::CollisionCheckDebugMap; +using behavior_path_planner::utils::path_safety_checker::CollisionCheckDebugPair; +using behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObject; +using behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObjects; using geometry_msgs::msg::Point; using geometry_msgs::msg::Polygon; using geometry_msgs::msg::Pose; @@ -49,50 +51,16 @@ using tier4_autoware_utils::Polygon2d; using visualization_msgs::msg::Marker; using visualization_msgs::msg::MarkerArray; -struct CollisionCheckDebug -{ - std::string failed_reason; - std::size_t lane_id{0}; - Pose current_pose{}; - Twist current_twist{}; - Twist object_twist{}; - Pose expected_ego_pose{}; - Pose expected_obj_pose{}; - Pose relative_to_ego{}; - double rss_longitudinal{0.0}; - double ego_to_obj_margin{0.0}; - double longitudinal_offset{0.0}; - double lateral_offset{0.0}; - bool is_front{false}; - bool allow_lane_change{false}; - std::vector lerped_path; - std::vector ego_predicted_path{}; - Polygon2d ego_polygon{}; - Polygon2d obj_polygon{}; -}; -using CollisionCheckDebugMap = std::unordered_map; - -constexpr std::array, 10> colorsList() -{ - constexpr std::array red = {1., 0., 0.}; - constexpr std::array green = {0., 1., 0.}; - constexpr std::array blue = {0., 0., 1.}; - constexpr std::array yellow = {1., 1., 0.}; - constexpr std::array aqua = {0., 1., 1.}; - constexpr std::array magenta = {1., 0., 1.}; - constexpr std::array medium_orchid = {0.729, 0.333, 0.827}; - constexpr std::array light_pink = {1, 0.713, 0.756}; - constexpr std::array light_yellow = {1, 1, 0.878}; - constexpr std::array light_steel_blue = {0.690, 0.768, 0.870}; - return {red, green, blue, yellow, aqua, - magenta, medium_orchid, light_pink, light_yellow, light_steel_blue}; -} - inline int64_t bitShift(int64_t original_id) { return original_id << (sizeof(int32_t) * 8 / 2); } +CollisionCheckDebugPair createObjectDebug(const ExtendedPredictedObject & obj); + +void updateCollisionCheckDebugMap( + CollisionCheckDebugMap & debug_map, CollisionCheckDebugPair & object_debug, bool is_safe); + MarkerArray createPoseMarkerArray( const Pose & pose, std::string && ns, const int32_t & id, const float & r, const float & g, const float & b); @@ -131,6 +99,19 @@ MarkerArray createObjectsMarkerArray( MarkerArray createDrivableLanesMarkerArray( const std::vector & drivable_lanes, std::string && ns); +MarkerArray createPredictedPathMarkerArray( + const PredictedPath & ego_predicted_path, const vehicle_info_util::VehicleInfo & vehicle_info, + std::string && ns, const int32_t & id, const float & r, const float & g, const float & b); + +MarkerArray showPolygon(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns); + +MarkerArray showPredictedPath(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns); + +MarkerArray showSafetyCheckInfo(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns); + +MarkerArray showFilteredObjects( + const ExtendedPredictedObjects & predicted_objects, const std::string & ns, + const ColorRGBA & color, int32_t id); } // namespace marker_utils #endif // BEHAVIOR_PATH_PLANNER__MARKER_UTILS__UTILS_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp index d6b99dca70618..a4e6fc18ab050 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp @@ -18,7 +18,6 @@ #include #include -#include #include #include @@ -28,6 +27,7 @@ struct ModuleConfigParameters bool enable_rtc{false}; bool enable_simultaneous_execution_as_approved_module{false}; bool enable_simultaneous_execution_as_candidate_module{false}; + bool keep_last{false}; uint8_t priority{0}; uint8_t max_module_size{0}; }; @@ -76,6 +76,7 @@ struct LateralAccelerationMap struct BehaviorPathPlannerParameters { bool verbose; + size_t max_iteration_num{100}; ModuleConfigParameters config_avoidance; ModuleConfigParameters config_avoidance_by_lc; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp index 29857590d4eda..1d1ad56342f4d 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp @@ -18,16 +18,17 @@ #include "behavior_path_planner/scene_module/scene_module_interface.hpp" #include "behavior_path_planner/scene_module/scene_module_manager_interface.hpp" #include "behavior_path_planner/scene_module/scene_module_visitor.hpp" -#include "behavior_path_planner/utils/lane_following/module_data.hpp" +#include "tier4_autoware_utils/ros/debug_publisher.hpp" +#include "tier4_autoware_utils/system/stop_watch.hpp" -#include #include #include #include +#include + #include -#include #include #include #include @@ -42,6 +43,8 @@ using tier4_autoware_utils::StopWatch; using tier4_planning_msgs::msg::StopReasonArray; using SceneModulePtr = std::shared_ptr; using SceneModuleManagerPtr = std::shared_ptr; +using DebugPublisher = tier4_autoware_utils::DebugPublisher; +using DebugDoubleMsg = tier4_debug_msgs::msg::Float64Stamped; enum Action { ADD = 0, @@ -91,7 +94,7 @@ struct SceneModuleStatus class PlannerManager { public: - PlannerManager(rclcpp::Node & node, const bool verbose); + PlannerManager(rclcpp::Node & node, const size_t max_iteration_num, const bool verbose); /** * @brief run all candidate and approved modules. @@ -236,6 +239,11 @@ class PlannerManager */ void print() const; + /** + * @brief publish processing time of each module. + */ + void publishProcessingTime() const; + /** * @brief visit each module and get debug information. */ @@ -261,6 +269,8 @@ class PlannerManager const auto result = module_ptr->run(); module_ptr->unlockRTCCommand(); + module_ptr->postProcess(); + module_ptr->updateCurrentState(); module_ptr->publishRTCStatus(); @@ -278,32 +288,7 @@ class PlannerManager * @param planner data. * @return reference path. */ - BehaviorModuleOutput getReferencePath(const std::shared_ptr & data) const - { - const auto & route_handler = data->route_handler; - const auto & pose = data->self_odometry->pose.pose; - const auto p = data->parameters; - - constexpr double extra_margin = 10.0; - const auto backward_length = - std::max(p.backward_path_length, p.backward_path_length + extra_margin); - - const auto lanelet_sequence = route_handler->getLaneletSequence( - root_lanelet_.get(), pose, backward_length, std::numeric_limits::max()); - - lanelet::ConstLanelet closest_lane{}; - if (lanelet::utils::query::getClosestLaneletWithConstrains( - lanelet_sequence, pose, &closest_lane, p.ego_nearest_dist_threshold, - p.ego_nearest_yaw_threshold)) { - return utils::getReferencePath(closest_lane, data); - } - - if (lanelet::utils::query::getClosestLanelet(lanelet_sequence, pose, &closest_lane)) { - return utils::getReferencePath(closest_lane, data); - } - - return {}; // something wrong. - } + BehaviorModuleOutput getReferencePath(const std::shared_ptr & data) const; /** * @brief stop and unregister the module from manager. @@ -364,16 +349,10 @@ class PlannerManager * @param planner data. * @return root lanelet. */ - lanelet::ConstLanelet updateRootLanelet( - const std::shared_ptr & data, bool success_lane_change = false) const + lanelet::ConstLanelet updateRootLanelet(const std::shared_ptr & data) const { lanelet::ConstLanelet ret{}; - if (success_lane_change) { - data->route_handler->getClosestPreferredLaneletWithinRoute( - data->self_odometry->pose.pose, &ret); - } else { - data->route_handler->getClosestLaneletWithinRoute(data->self_odometry->pose.pose, &ret); - } + data->route_handler->getClosestLaneletWithinRoute(data->self_odometry->pose.pose, &ret); RCLCPP_DEBUG(logger_, "update start lanelet. id:%ld", ret.id()); return ret; } @@ -450,6 +429,8 @@ class PlannerManager std::vector candidate_module_ptrs_; + std::unique_ptr debug_publisher_ptr_; + mutable rclcpp::Logger logger_; mutable rclcpp::Clock clock_; @@ -462,6 +443,8 @@ class PlannerManager mutable std::shared_ptr debug_msg_ptr_; + size_t max_iteration_num_{100}; + bool verbose_{false}; }; } // namespace behavior_path_planner 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 7889e91767155..cbf40e59535be 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 @@ -219,6 +219,8 @@ class AvoidanceModule : public SceneModuleInterface */ void insertStopPoint(const bool use_constraints_for_decel, ShiftedPath & shifted_path) const; + void insertReturnDeadLine(const bool use_constraints_for_decel, ShiftedPath & shifted_path) const; + /** * @brief insert stop point in output path. * @param target path. @@ -263,10 +265,10 @@ class AvoidanceModule : public SceneModuleInterface * @brief fill candidate shift lines. * @param avoidance data. * @param debug data. - * @details in this function, three shift line sets are generated. + * @details in this function, following two shift line arrays are generated. * - unapproved raw shift lines. * - unapproved new shift lines. - * - safe new shift lines. (avoidance path is generated from this shift line set.) + * and check whether the new shift lines are safe or not. */ void fillShiftLine(AvoidancePlanningData & data, DebugData & debug) const; @@ -311,8 +313,31 @@ class AvoidanceModule : public SceneModuleInterface * @param debug data. * @return processed shift lines. */ - AvoidLineArray calcRawShiftLinesFromObjects( - AvoidancePlanningData & data, DebugData & debug) const; + AvoidOutlines generateAvoidOutline(AvoidancePlanningData & data, DebugData & debug) const; + + /* + * @brief merge avoid outlines. + * @param original shift lines. + * @param debug data. + * @return processed shift lines. + */ + AvoidOutlines applyMergeProcess(const AvoidOutlines & outlines, DebugData & debug) const; + + /* + * @brief fill gap between two shift lines. + * @param original shift lines. + * @param debug data. + * @return processed shift lines. + */ + AvoidOutlines applyFillGapProcess(const AvoidOutlines & outlines, DebugData & debug) const; + + /* + * @brief generate candidate shift lines. + * @param one-shot shift lines. + * @param debug data. + */ + AvoidLineArray generateCandidateShiftLine( + const AvoidLineArray & shift_lines, DebugData & debug) const; /** * @brief clean up raw shift lines. @@ -325,10 +350,15 @@ class AvoidanceModule : public SceneModuleInterface * 3. merge raw shirt lines. * 4. trim unnecessary shift lines. */ - AvoidLineArray applyPreProcessToRawShiftLines( - AvoidLineArray & current_raw_shift_points, DebugData & debug) const; + AvoidLineArray applyPreProcess(const AvoidOutlines & outlines, DebugData & debug) const; - AvoidLineArray getFillGapShiftLines(const AvoidLineArray & shift_lines) const; + /* + * @brief fill gap among shift lines. + * @param original shift lines. + * @param debug data. + * @return processed shift lines. + */ + AvoidLineArray applyFillGapProcess(const AvoidLineArray & shift_lines, DebugData & debug) const; /* * @brief merge negative & positive shift lines. @@ -336,7 +366,28 @@ class AvoidanceModule : public SceneModuleInterface * @param debug data. * @return processed shift lines. */ - AvoidLineArray mergeShiftLines(const AvoidLineArray & raw_shift_lines, DebugData & debug) const; + AvoidLineArray applyMergeProcess(const AvoidLineArray & shift_lines, DebugData & debug) const; + + /* + * @brief add return shift line from ego position. + * @param current raw shift line. + * @param current registered shift line. + * @param debug data. + */ + AvoidLineArray applyCombineProcess( + const AvoidLineArray & shift_lines, const AvoidLineArray & registered_lines, + [[maybe_unused]] DebugData & debug) const; + + /* + * @brief add return shift line from ego position. + * @param shift lines which the return shift is added. + * Pick up the last shift point, which is the most farthest from ego, from the current candidate + * avoidance points and registered points in the shifter. If the last shift length of the point is + * non-zero, add a return-shift to center line from the point. If there is no shift point in + * candidate avoidance points nor registered points, and base_shift > 0, add a return-shift to + * center line from ego. + */ + AvoidLineArray addReturnShiftLine(const AvoidLineArray & shift_lines, DebugData & debug) const; /* * @brief extract shift lines from total shift lines based on their gradient. @@ -356,7 +407,7 @@ class AvoidanceModule : public SceneModuleInterface * - Change the shift length to the previous one if the deviation is small. * - Remove unnecessary return shift (back to the center line). */ - AvoidLineArray trimShiftLine(const AvoidLineArray & shift_lines, DebugData & debug) const; + AvoidLineArray applyTrimProcess(const AvoidLineArray & shift_lines, DebugData & debug) const; /* * @brief extract new shift lines based on current shifted path. the module makes a RTC request @@ -364,24 +415,7 @@ class AvoidanceModule : public SceneModuleInterface * @param candidate shift lines. * @return new shift lines. */ - AvoidLineArray findNewShiftLine(const AvoidLineArray & shift_lines) const; - - /* - * @brief add return shift line from ego position. - * @param shift lines which the return shift is added. - * Pick up the last shift point, which is the most farthest from ego, from the current candidate - * avoidance points and registered points in the shifter. If the last shift length of the point is - * non-zero, add a return-shift to center line from the point. If there is no shift point in - * candidate avoidance points nor registered points, and base_shift > 0, add a return-shift to - * center line from ego. - */ - void addReturnShiftLineFromEgo(AvoidLineArray & shift_lines) const; - - /* - * @brief fill gap between two shift lines. - * @param original shift lines. - */ - void fillShiftLineGap(AvoidLineArray & shift_lines) const; + AvoidLineArray findNewShiftLine(const AvoidLineArray & shift_lines, DebugData & debug) const; /* * @brief generate total shift line. total shift line has shift length and gradient array. @@ -397,27 +431,21 @@ class AvoidanceModule : public SceneModuleInterface * @param threshold. shift length is quantized by this value. (if it is 0.3[m], the output shift * length is 0.0, 0.3, 0.6...) */ - void quantizeShiftLine(AvoidLineArray & shift_lines, const double threshold) const; + void applyQuantizeProcess(AvoidLineArray & shift_lines, const double threshold) const; /* * @brief trim shift line whose relative longitudinal distance is less than threshold. * @param target shift lines. * @param threshold. */ - void trimSmallShiftLine(AvoidLineArray & shift_lines, const double threshold) const; + void applySmallShiftFilter(AvoidLineArray & shift_lines, const double threshold) const; /* * @brief merge multiple shift lines whose relative gradient is less than threshold. * @param target shift lines. * @param threshold. */ - void trimSimilarGradShiftLine(AvoidLineArray & shift_lines, const double threshold) const; - - /* - * @brief trim invalid shift lines whose gradient it too large to follow. - * @param target shift lines. - */ - void trimSharpReturn(AvoidLineArray & shift_lines, const double threshold) const; + void applySimilarGradFilter(AvoidLineArray & shift_lines, const double threshold) const; /** * @brief add new shift line to path shifter if the RTC status is activated. @@ -432,7 +460,8 @@ class AvoidanceModule : public SceneModuleInterface void addNewShiftLines(PathShifter & path_shifter, const AvoidLineArray & shift_lines) const; /** - * @brief validate shift lines. + * @brief once generate avoidance path from new shift lines, and calculate lateral offset between + * ego and the path. * @param new shift lines. * @param path shifter. * @return result. if there is huge gap between the ego position and candidate path, return false. @@ -448,13 +477,6 @@ class AvoidanceModule : public SceneModuleInterface */ TurnSignalInfo calcTurnSignalInfo(const ShiftedPath & path) const; - // TODO(murooka) judge when and which way to extend drivable area. current implementation is keep - // extending during avoidance module - // TODO(murooka) freespace during turning in intersection where there is no neighbor lanes - // NOTE: Assume that there is no situation where there is an object in the middle lane of more - // than two lanes since which way to avoid is not obvious - void generateExtendedDrivableArea(BehaviorModuleOutput & output) const; - /** * @brief fill debug markers. */ @@ -556,6 +578,8 @@ class AvoidanceModule : public SceneModuleInterface bool arrived_path_end_{false}; + bool safe_{true}; + std::shared_ptr parameters_; helper::avoidance::AvoidanceHelper helper_; @@ -576,6 +600,8 @@ class AvoidanceModule : public SceneModuleInterface ObjectDataArray registered_objects_; + mutable size_t safe_count_{0}; + mutable ObjectDataArray ego_stopped_objects_; mutable ObjectDataArray stopped_objects_; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp index dd0e06fffb255..6c522c79a8774 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp @@ -18,8 +18,10 @@ #include "behavior_path_planner/scene_module/scene_module_interface.hpp" #include +#include #include +#include #include #include #include @@ -35,10 +37,13 @@ namespace behavior_path_planner { +using autoware_auto_perception_msgs::msg::PredictedPath; +using tier4_autoware_utils::Polygon2d; + struct MinMaxValue { - double min_value; - double max_value; + double min_value{0.0}; + double max_value{0.0}; }; struct DynamicAvoidanceParameters @@ -56,6 +61,7 @@ struct DynamicAvoidanceParameters bool avoid_bicycle{false}; bool avoid_motorcycle{false}; bool avoid_pedestrian{false}; + double max_obstacle_vel{0.0}; double min_obstacle_vel{0.0}; int successive_num_to_entry_dynamic_avoidance_condition{0}; int successive_num_to_exit_dynamic_avoidance_condition{0}; @@ -74,7 +80,7 @@ struct DynamicAvoidanceParameters double max_oncoming_crossing_object_angle{0.0}; // drivable area generation - std::string polygon_generation_method{}; + double min_obj_path_based_lon_polygon_margin{0.0}; double lat_offset_from_obstacle{0.0}; double max_lat_offset_to_avoid{0.0}; double max_time_for_lat_shift{0.0}; @@ -93,6 +99,10 @@ struct DynamicAvoidanceParameters class DynamicAvoidanceModule : public SceneModuleInterface { public: + enum class PolygonGenerationMethod { + EGO_PATH_BASE = 0, + OBJECT_PATH_BASE, + }; struct DynamicAvoidanceObject { DynamicAvoidanceObject( @@ -112,12 +122,12 @@ class DynamicAvoidanceModule : public SceneModuleInterface } } - std::string uuid; - geometry_msgs::msg::Pose pose; + std::string uuid{}; + geometry_msgs::msg::Pose pose{}; autoware_auto_perception_msgs::msg::Shape shape; - double vel; - double lat_vel; - bool is_object_on_ego_path; + double vel{0.0}; + double lat_vel{0.0}; + bool is_object_on_ego_path{false}; std::optional latest_time_inside_ego_path{std::nullopt}; std::vector predicted_paths{}; @@ -125,17 +135,20 @@ class DynamicAvoidanceModule : public SceneModuleInterface // Therefore, they has to be initialized as nullopt. std::optional lon_offset_to_avoid{std::nullopt}; std::optional lat_offset_to_avoid{std::nullopt}; - bool is_collision_left; + bool is_collision_left{false}; bool should_be_avoided{false}; + PolygonGenerationMethod polygon_generation_method{PolygonGenerationMethod::OBJECT_PATH_BASE}; void update( const MinMaxValue & arg_lon_offset_to_avoid, const MinMaxValue & arg_lat_offset_to_avoid, - const bool arg_is_collision_left, const bool arg_should_be_avoided) + const bool arg_is_collision_left, const bool arg_should_be_avoided, + const PolygonGenerationMethod & arg_polygon_generation_method) { lon_offset_to_avoid = arg_lon_offset_to_avoid; lat_offset_to_avoid = arg_lat_offset_to_avoid; is_collision_left = arg_is_collision_left; should_be_avoided = arg_should_be_avoided; + polygon_generation_method = arg_polygon_generation_method; } }; @@ -145,8 +158,8 @@ class DynamicAvoidanceModule : public SceneModuleInterface : max_count_(arg_max_count), min_count_(arg_min_count) { } - int max_count_; - int min_count_; + int max_count_{0}; + int min_count_{0}; void initialize() { current_uuids_.clear(); } void updateObject(const std::string & uuid, const DynamicAvoidanceObject & object) @@ -229,11 +242,12 @@ class DynamicAvoidanceModule : public SceneModuleInterface void updateObject( const std::string & uuid, const MinMaxValue & lon_offset_to_avoid, const MinMaxValue & lat_offset_to_avoid, const bool is_collision_left, - const bool should_be_avoided) + const bool should_be_avoided, const PolygonGenerationMethod & polygon_generation_method) { if (object_map_.count(uuid) != 0) { object_map_.at(uuid).update( - lon_offset_to_avoid, lat_offset_to_avoid, is_collision_left, should_be_avoided); + lon_offset_to_avoid, lat_offset_to_avoid, is_collision_left, should_be_avoided, + polygon_generation_method); } } @@ -311,7 +325,8 @@ class DynamicAvoidanceModule : public SceneModuleInterface void updateTargetObjects(); bool willObjectCutIn( const std::vector & ego_path, const PredictedPath & predicted_path, - const double obj_tangent_vel, const LatLonOffset & lat_lon_offset) const; + const double obj_tangent_vel, const LatLonOffset & lat_lon_offset, + PolygonGenerationMethod & polygon_generation_method) const; DecisionWithReason willObjectCutOut( const double obj_tangent_vel, const double obj_normal_vel, const bool is_object_left, const std::optional & prev_object) const; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp index 8d9c8e147f532..343302a022a69 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp @@ -24,11 +24,15 @@ #include "behavior_path_planner/utils/goal_planner/goal_searcher.hpp" #include "behavior_path_planner/utils/goal_planner/shift_pull_over.hpp" #include "behavior_path_planner/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp" +#include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" +#include "behavior_path_planner/utils/start_goal_planner_common/common_module_data.hpp" +#include "behavior_path_planner/utils/utils.hpp" #include #include #include #include +#include #include #include @@ -57,6 +61,13 @@ using freespace_planning_algorithms::PlannerCommonParam; using freespace_planning_algorithms::RRTStar; using freespace_planning_algorithms::RRTStarParam; +using behavior_path_planner::utils::path_safety_checker::EgoPredictedPathParams; +using behavior_path_planner::utils::path_safety_checker::ObjectsFilteringParams; +using behavior_path_planner::utils::path_safety_checker::PoseWithVelocityStamped; +using behavior_path_planner::utils::path_safety_checker::SafetyCheckParams; +using behavior_path_planner::utils::path_safety_checker::TargetObjectsOnLane; +using tier4_autoware_utils::Polygon2d; + enum class PathType { NONE = 0, SHIFT, @@ -65,24 +76,114 @@ enum class PathType { FREESPACE, }; -struct PUllOverStatus +#define DEFINE_SETTER_GETTER(TYPE, NAME) \ +public: \ + void set_##NAME(const TYPE & value) \ + { \ + const std::lock_guard lock(mutex_); \ + NAME##_ = value; \ + } \ + \ + TYPE get_##NAME() const \ + { \ + const std::lock_guard lock(mutex_); \ + return NAME##_; \ + } + +class PullOverStatus { - std::shared_ptr pull_over_path{}; - std::shared_ptr lane_parking_pull_over_path{}; - size_t current_path_idx{0}; - bool require_increment_{true}; // if false, keep current path idx. - std::shared_ptr prev_stop_path{nullptr}; - lanelet::ConstLanelets current_lanes{}; - lanelet::ConstLanelets pull_over_lanes{}; - std::vector lanes{}; // current + pull_over - bool has_decided_path{false}; - bool is_safe{false}; - bool prev_is_safe{false}; - bool has_decided_velocity{false}; - bool has_requested_approval{false}; - bool is_ready{false}; +public: + explicit PullOverStatus(std::recursive_mutex & mutex) : mutex_(mutex) {} + + // Reset all data members to their initial states + void reset() + { + std::lock_guard lock(mutex_); + pull_over_path_ = nullptr; + lane_parking_pull_over_path_ = nullptr; + current_path_idx_ = 0; + require_increment_ = true; + prev_stop_path_ = nullptr; + prev_stop_path_after_approval_ = nullptr; + current_lanes_.clear(); + pull_over_lanes_.clear(); + lanes_.clear(); + has_decided_path_ = false; + is_safe_static_objects_ = false; + is_safe_dynamic_objects_ = false; + prev_is_safe_ = false; + prev_is_safe_dynamic_objects_ = false; + has_decided_velocity_ = false; + has_requested_approval_ = false; + is_ready_ = false; + } + + DEFINE_SETTER_GETTER(std::shared_ptr, pull_over_path) + DEFINE_SETTER_GETTER(std::shared_ptr, lane_parking_pull_over_path) + DEFINE_SETTER_GETTER(size_t, current_path_idx) + DEFINE_SETTER_GETTER(bool, require_increment) + DEFINE_SETTER_GETTER(std::shared_ptr, prev_stop_path) + DEFINE_SETTER_GETTER(std::shared_ptr, prev_stop_path_after_approval) + DEFINE_SETTER_GETTER(lanelet::ConstLanelets, current_lanes) + DEFINE_SETTER_GETTER(lanelet::ConstLanelets, pull_over_lanes) + DEFINE_SETTER_GETTER(std::vector, lanes) + DEFINE_SETTER_GETTER(bool, has_decided_path) + DEFINE_SETTER_GETTER(bool, is_safe_static_objects) + DEFINE_SETTER_GETTER(bool, is_safe_dynamic_objects) + DEFINE_SETTER_GETTER(bool, prev_is_safe) + DEFINE_SETTER_GETTER(bool, prev_is_safe_dynamic_objects) + DEFINE_SETTER_GETTER(bool, has_decided_velocity) + DEFINE_SETTER_GETTER(bool, has_requested_approval) + DEFINE_SETTER_GETTER(bool, is_ready) + DEFINE_SETTER_GETTER(std::shared_ptr, last_approved_time) + DEFINE_SETTER_GETTER(std::shared_ptr, last_increment_time) + DEFINE_SETTER_GETTER(std::shared_ptr, last_path_update_time) + DEFINE_SETTER_GETTER(std::shared_ptr, last_approved_pose) + DEFINE_SETTER_GETTER(std::optional, modified_goal_pose) + DEFINE_SETTER_GETTER(Pose, refined_goal_pose) + DEFINE_SETTER_GETTER(GoalCandidates, goal_candidates) + DEFINE_SETTER_GETTER(std::vector, pull_over_path_candidates) + DEFINE_SETTER_GETTER(std::optional, closest_start_pose) + +private: + std::shared_ptr pull_over_path_{nullptr}; + std::shared_ptr lane_parking_pull_over_path_{nullptr}; + size_t current_path_idx_{0}; + bool require_increment_{true}; + std::shared_ptr prev_stop_path_{nullptr}; + std::shared_ptr prev_stop_path_after_approval_{nullptr}; + lanelet::ConstLanelets current_lanes_{}; + lanelet::ConstLanelets pull_over_lanes_{}; + std::vector lanes_{}; + bool has_decided_path_{false}; + bool is_safe_static_objects_{false}; + bool is_safe_dynamic_objects_{false}; + bool prev_is_safe_{false}; + bool prev_is_safe_dynamic_objects_{false}; + bool has_decided_velocity_{false}; + bool has_requested_approval_{false}; + bool is_ready_{false}; + + // save last time and pose + std::shared_ptr last_approved_time_; + std::shared_ptr last_increment_time_; + std::shared_ptr last_path_update_time_; + std::shared_ptr last_approved_pose_; + + // goal modification + std::optional modified_goal_pose_; + Pose refined_goal_pose_{}; + GoalCandidates goal_candidates_{}; + + // pull over path + std::vector pull_over_path_candidates_; + std::optional closest_start_pose_; + + std::recursive_mutex & mutex_; }; +#undef DEFINE_SETTER_GETTER + struct FreespacePlannerDebugData { bool is_planning{false}; @@ -93,6 +194,7 @@ struct FreespacePlannerDebugData struct GoalPlannerDebugData { FreespacePlannerDebugData freespace_planner{}; + std::vector ego_polygons_expanded{}; }; class GoalPlannerModule : public SceneModuleInterface @@ -106,6 +208,13 @@ class GoalPlannerModule : public SceneModuleInterface void updateModuleParams(const std::any & parameters) override { parameters_ = std::any_cast>(parameters); + if (parameters_->safety_check_params.enable_safety_check) { + ego_predicted_path_params_ = + std::make_shared(parameters_->ego_predicted_path_params); + objects_filtering_params_ = + std::make_shared(parameters_->objects_filtering_params); + safety_check_params_ = std::make_shared(parameters_->safety_check_params); + } } // TODO(someone): remove this, and use base class function @@ -134,10 +243,16 @@ class GoalPlannerModule : public SceneModuleInterface bool canTransitIdleToRunningState() override { return false; } - PUllOverStatus status_; + mutable StartGoalPlannerData goal_planner_data_; std::shared_ptr parameters_; + mutable std::shared_ptr ego_predicted_path_params_; + mutable std::shared_ptr objects_filtering_params_; + mutable std::shared_ptr safety_check_params_; + + vehicle_info_util::VehicleInfo vehicle_info_{}; + // planner std::vector> pull_over_planners_; std::unique_ptr freespace_planner_; @@ -145,29 +260,19 @@ class GoalPlannerModule : public SceneModuleInterface // goal searcher std::shared_ptr goal_searcher_; - std::optional modified_goal_pose_; - Pose refined_goal_pose_; - GoalCandidates goal_candidates_; // collision detector // need to be shared_ptr to be used in planner and goal searcher std::shared_ptr occupancy_grid_map_; - // pull over path - std::vector pull_over_path_candidates_; - std::optional closest_start_pose_; - // check stopped and stuck state std::deque odometry_buffer_stopped_; std::deque odometry_buffer_stuck_; tier4_autoware_utils::LinearRing2d vehicle_footprint_; - // save last time and pose - std::unique_ptr last_approved_time_; - std::unique_ptr last_increment_time_; - std::unique_ptr last_path_update_time_; - std::unique_ptr last_approved_pose_; + std::recursive_mutex mutex_; + PullOverStatus status_; // approximate distance from the start point to the end point of pull_over. // this is used as an assumed value to decelerate, etc., before generating the actual path. @@ -177,12 +282,10 @@ class GoalPlannerModule : public SceneModuleInterface // for parking policy bool left_side_parking_{true}; - mutable bool allow_goal_modification_{false}; // need to be set in isExecutionRequested // pre-generate lane parking paths in a separate thread rclcpp::TimerBase::SharedPtr lane_parking_timer_; rclcpp::CallbackGroup::SharedPtr lane_parking_timer_cb_group_; - std::mutex mutex_; // generate freespace parking paths in a separate thread rclcpp::TimerBase::SharedPtr freespace_parking_timer_; @@ -206,7 +309,7 @@ class GoalPlannerModule : public SceneModuleInterface const Pose & search_start_offset_pose, PathWithLaneId & path) const; PathWithLaneId generateStopPath(); PathWithLaneId generateFeasibleStopPath(); - boost::optional calcFeasibleDecelDistance(const double target_velocity) const; + void keepStoppedWithCurrentPath(PathWithLaneId & path); double calcSignedArcLengthFromEgo(const PathWithLaneId & path, const Pose & pose) const; @@ -215,10 +318,8 @@ class GoalPlannerModule : public SceneModuleInterface bool isStopped( std::deque & odometry_buffer, const double time); bool hasFinishedCurrentPath(); - bool hasFinishedGoalPlanner(); bool isOnModifiedGoal() const; double calcModuleRequestLength() const; - void resetStatus(); bool needPathUpdate(const double path_update_duration) const; bool isStuck(); bool hasDecidedPath() const; @@ -241,6 +342,9 @@ class GoalPlannerModule : public SceneModuleInterface BehaviorModuleOutput planWithGoalModification(); BehaviorModuleOutput planWaitingApprovalWithGoalModification(); void selectSafePullOverPath(); + std::vector sortPullOverPathCandidatesByGoalPriority( + const std::vector & pull_over_path_candidates, + const GoalCandidates & goal_candidates) const; // deal with pull over partial paths PathWithLaneId getCurrentPath() const; @@ -254,6 +358,17 @@ class GoalPlannerModule : public SceneModuleInterface // output setter void setOutput(BehaviorModuleOutput & output); void setStopPath(BehaviorModuleOutput & output); + + /** + * @brief Sets a stop path in the current path based on safety conditions and previous paths. + * + * This function sets a stop path in the current path. Depending on whether the previous safety + * judgement against dynamic objects were safe or if a previous stop path existed, it either + * generates a new stop path or uses the previous stop path. + * + * @param output BehaviorModuleOutput + */ + void setStopPathFromCurrentPath(BehaviorModuleOutput & output); void setModifiedGoal(BehaviorModuleOutput & output) const; void setTurnSignalInfo(BehaviorModuleOutput & output) const; @@ -275,6 +390,14 @@ class GoalPlannerModule : public SceneModuleInterface // rtc std::pair calcDistanceToPathChange() const; + // safety check + void initializeSafetyCheckParameters(); + SafetyCheckParams createSafetyCheckParams() const; + void updateSafetyCheckTargetObjectsData( + const PredictedObjects & filtered_objects, const TargetObjectsOnLane & target_objects_on_lane, + const std::vector & ego_predicted_path) const; + bool isSafePath() const; + // debug void setDebugData(); void printParkingPositionError() const; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/manager.hpp index c410999b2aaa9..4a7eecfc68caf 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/manager.hpp @@ -40,14 +40,14 @@ class GoalPlannerModuleManager : public SceneModuleManagerInterface void updateModuleParams(const std::vector & parameters) override; + bool isAlwaysExecutableModule() const override; + bool isSimultaneousExecutableAsApprovedModule() const override; bool isSimultaneousExecutableAsCandidateModule() const override; private: std::shared_ptr parameters_; - - bool left_side_parking_; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp index 6ca06ff21c4a9..472564a061f04 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp @@ -43,11 +43,11 @@ namespace behavior_path_planner { using autoware_auto_planning_msgs::msg::PathWithLaneId; +using behavior_path_planner::utils::path_safety_checker::CollisionCheckDebugMap; using data::lane_change::PathSafetyStatus; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; -using marker_utils::CollisionCheckDebugMap; using route_handler::Direction; using tier4_autoware_utils::StopWatch; using tier4_planning_msgs::msg::LaneChangeDebugMsg; @@ -154,6 +154,11 @@ class LaneChangeBase return object_debug_after_approval_; } + const LaneChangeTargetObjects & getDebugFilteredObjects() const + { + return debug_filtered_objects_; + } + const Pose & getEgoPose() const { return planner_data_->self_odometry->pose.pose; } const Point & getEgoPosition() const { return getEgoPose().position; } @@ -209,6 +214,10 @@ class LaneChangeBase return direction_; } + boost::optional getStopPose() const { return lane_change_stop_pose_; } + + void resetStopPose() { lane_change_stop_pose_ = boost::none; } + protected: virtual lanelet::ConstLanelets getCurrentLanes() const = 0; @@ -221,7 +230,8 @@ class LaneChangeBase virtual bool getLaneChangePaths( const lanelet::ConstLanelets & original_lanelets, const lanelet::ConstLanelets & target_lanelets, Direction direction, - LaneChangePaths * candidate_paths, const bool check_safety) const = 0; + LaneChangePaths * candidate_paths, const utils::path_safety_checker::RSSparams rss_params, + const bool is_stuck, const bool check_safety) const = 0; virtual TurnSignalInfo calcTurnSignalInfo() = 0; @@ -244,6 +254,7 @@ class LaneChangeBase PathWithLaneId prev_module_path_{}; DrivableAreaInfo prev_drivable_area_info_{}; TurnSignalInfo prev_turn_signal_info_{}; + boost::optional lane_change_stop_pose_{boost::none}; PathWithLaneId prev_approved_path_{}; @@ -257,8 +268,12 @@ class LaneChangeBase mutable LaneChangePaths debug_valid_path_{}; mutable CollisionCheckDebugMap object_debug_{}; mutable CollisionCheckDebugMap object_debug_after_approval_{}; + mutable LaneChangeTargetObjects debug_filtered_objects_{}; mutable double object_debug_lifetime_{0.0}; mutable StopWatch stop_watch_; + + rclcpp::Logger logger_ = rclcpp::get_logger("lane_change"); + mutable rclcpp::Clock clock_{RCL_ROS_TIME}; }; } // namespace behavior_path_planner #endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__BASE_CLASS_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp index 5d13507f5206c..54883489f2fe6 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp @@ -26,6 +26,8 @@ namespace behavior_path_planner { using autoware_auto_planning_msgs::msg::PathWithLaneId; +using behavior_path_planner::utils::path_safety_checker::CollisionCheckDebug; +using behavior_path_planner::utils::path_safety_checker::CollisionCheckDebugMap; using behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObject; using behavior_path_planner::utils::path_safety_checker::PoseWithVelocityAndPolygonStamped; using behavior_path_planner::utils::path_safety_checker::PoseWithVelocityStamped; @@ -33,8 +35,6 @@ using behavior_path_planner::utils::path_safety_checker::PredictedPathWithPolygo using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; -using marker_utils::CollisionCheckDebug; -using marker_utils::CollisionCheckDebugMap; using route_handler::Direction; using tier4_planning_msgs::msg::LaneChangeDebugMsg; using tier4_planning_msgs::msg::LaneChangeDebugMsgArray; @@ -108,7 +108,7 @@ class NormalLaneChange : public LaneChangeBase const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes) const; - double calcPrepareDuration( + std::vector calcPrepareDuration( const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes) const; @@ -129,9 +129,16 @@ class NormalLaneChange : public LaneChangeBase const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, const Direction direction = Direction::NONE) const; + bool hasEnoughLengthToCrosswalk( + const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes) const; + + bool hasEnoughLengthToIntersection( + const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes) const; + bool getLaneChangePaths( const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, Direction direction, LaneChangePaths * candidate_paths, + const utils::path_safety_checker::RSSparams rss_params, const bool is_stuck, const bool check_safety = true) const override; TurnSignalInfo calcTurnSignalInfo() override; @@ -140,14 +147,34 @@ class NormalLaneChange : public LaneChangeBase PathSafetyStatus isLaneChangePathSafe( const LaneChangePath & lane_change_path, const LaneChangeTargetObjects & target_objects, - const utils::path_safety_checker::RSSparams & rss_params, - std::unordered_map & debug_data) const; + const utils::path_safety_checker::RSSparams & rss_params, const bool is_stuck, + CollisionCheckDebugMap & debug_data) const; LaneChangeTargetObjectIndices filterObject( - const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, + const PredictedObjects & objects, const lanelet::ConstLanelets & current_lanes, + const lanelet::ConstLanelets & target_lanes, const lanelet::ConstLanelets & target_backward_lanes) const; - rclcpp::Logger logger_ = rclcpp::get_logger("lane_change").get_child(getModuleTypeStr()); + //! @brief Check if the ego vehicle is in stuck by a stationary obstacle. + //! @param obstacle_check_distance Distance to check ahead for any objects that might be + //! obstructing ego path. It makes sense to use values like the maximum lane change distance. + bool isVehicleStuck( + const lanelet::ConstLanelets & current_lanes, const double obstacle_check_distance) const; + + bool isVehicleStuck(const lanelet::ConstLanelets & current_lanes) const; + + double calcMaximumLaneChangeLength( + const lanelet::ConstLanelet & current_terminal_lanelet, const double max_acc) const; + + std::pair calcCurrentMinMaxAcceleration() const; + + void setStopPose(const Pose & stop_pose); + + void updateStopTime(); + + double getStopTime() const { return stop_time_; } + + double stop_time_{0.0}; }; } // namespace behavior_path_planner #endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__NORMAL_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 3405ae446615d..7f24683e5680c 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 @@ -16,16 +16,22 @@ #define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__SCENE_MODULE_INTERFACE_HPP_ #include "behavior_path_planner/data_manager.hpp" +#include "behavior_path_planner/marker_utils/utils.hpp" #include "behavior_path_planner/scene_module/scene_module_visitor.hpp" #include "behavior_path_planner/utils/utils.hpp" #include #include #include +#include +#include +#include #include #include #include +#include #include +#include #include #include @@ -50,12 +56,8 @@ namespace behavior_path_planner { using autoware_adapi_v1_msgs::msg::SteeringFactor; using autoware_auto_planning_msgs::msg::PathWithLaneId; -using motion_utils::createDeadLineVirtualWallMarker; -using motion_utils::createSlowDownVirtualWallMarker; -using motion_utils::createStopVirtualWallMarker; using rtc_interface::RTCInterface; using steering_factor_interface::SteeringFactorInterface; -using tier4_autoware_utils::appendMarkerArray; using tier4_autoware_utils::calcOffsetPose; using tier4_autoware_utils::generateUUID; using tier4_planning_msgs::msg::AvoidanceDebugMsgArray; @@ -79,16 +81,16 @@ class SceneModuleInterface public: SceneModuleInterface( const std::string & name, rclcpp::Node & node, - const std::unordered_map> & rtc_interface_ptr_map) + std::unordered_map> rtc_interface_ptr_map) : name_{name}, logger_{node.get_logger().get_child(name)}, clock_{node.get_clock()}, - rtc_interface_ptr_map_(rtc_interface_ptr_map), + rtc_interface_ptr_map_(std::move(rtc_interface_ptr_map)), steering_factor_interface_ptr_( std::make_unique(&node, utils::convertToSnakeCase(name))) { - for (auto itr = rtc_interface_ptr_map_.begin(); itr != rtc_interface_ptr_map_.end(); ++itr) { - uuid_map_.emplace(itr->first, generateUUID()); + for (const auto & [module_name, ptr] : rtc_interface_ptr_map_) { + uuid_map_.emplace(module_name, generateUUID()); } } @@ -130,6 +132,12 @@ class SceneModuleInterface */ virtual void updateData() {} + /** + * @brief After executing run(), update the module-specific status and/or data used for internal + * processing that are not defined in ModuleStatus. + */ + virtual void postProcess() {} + /** * @brief Execute module. Once this function is executed, * it will continue to run as long as it is in the RUNNING state. @@ -176,9 +184,9 @@ class SceneModuleInterface */ void publishRTCStatus() { - for (auto itr = rtc_interface_ptr_map_.begin(); itr != rtc_interface_ptr_map_.end(); ++itr) { - if (itr->second) { - itr->second->publishCooperateStatus(clock_->now()); + for (const auto & [module_name, ptr] : rtc_interface_ptr_map_) { + if (ptr) { + ptr->publishCooperateStatus(clock_->now()); } } } @@ -193,18 +201,18 @@ class SceneModuleInterface void lockRTCCommand() { - for (auto itr = rtc_interface_ptr_map_.begin(); itr != rtc_interface_ptr_map_.end(); ++itr) { - if (itr->second) { - itr->second->lockCommandUpdate(); + for (const auto & [module_name, ptr] : rtc_interface_ptr_map_) { + if (ptr) { + ptr->lockCommandUpdate(); } } } void unlockRTCCommand() { - for (auto itr = rtc_interface_ptr_map_.begin(); itr != rtc_interface_ptr_map_.end(); ++itr) { - if (itr->second) { - itr->second->unlockCommandUpdate(); + for (const auto & [module_name, ptr] : rtc_interface_ptr_map_) { + if (ptr) { + ptr->unlockCommandUpdate(); } } } @@ -398,10 +406,10 @@ class SceneModuleInterface virtual void updateRTCStatus(const double start_distance, const double finish_distance) { - for (auto itr = rtc_interface_ptr_map_.begin(); itr != rtc_interface_ptr_map_.end(); ++itr) { - if (itr->second) { - itr->second->updateCooperateStatus( - uuid_map_.at(itr->first), isExecutionReady(), start_distance, finish_distance, + for (const auto & [module_name, ptr] : rtc_interface_ptr_map_) { + if (ptr) { + ptr->updateCooperateStatus( + uuid_map_.at(module_name), isExecutionReady(), start_distance, finish_distance, clock_->now()); } } @@ -483,9 +491,9 @@ class SceneModuleInterface void removeRTCStatus() { - for (auto itr = rtc_interface_ptr_map_.begin(); itr != rtc_interface_ptr_map_.end(); ++itr) { - if (itr->second) { - itr->second->clearCooperateStatus(); + for (const auto & [module_name, ptr] : rtc_interface_ptr_map_) { + if (ptr) { + ptr->clearCooperateStatus(); } } } diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_manager_interface.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_manager_interface.hpp index cb47c33b6901c..38101416a80b3 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_manager_interface.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_manager_interface.hpp @@ -55,6 +55,7 @@ class SceneModuleManagerInterface enable_simultaneous_execution_as_candidate_module_( config.enable_simultaneous_execution_as_candidate_module), enable_rtc_(config.enable_rtc), + keep_last_(config.keep_last), max_module_num_(config.max_module_size), priority_(config.priority) { @@ -214,16 +215,37 @@ class SceneModuleManagerInterface bool canLaunchNewModule() const { return observers_.size() < max_module_num_; } + /** + * Determine if the module is always executable, regardless of the state of other modules. + * + * When this returns true: + * - The module can be executed even if other modules are not marked as 'simultaneously + * executable'. + * - Conversely, the presence of this module will not prevent other modules + * from executing, even if they are not marked as 'simultaneously executable'. + */ + virtual bool isAlwaysExecutableModule() const { return false; } + virtual bool isSimultaneousExecutableAsApprovedModule() const { + if (isAlwaysExecutableModule()) { + return true; + } + return enable_simultaneous_execution_as_approved_module_; } virtual bool isSimultaneousExecutableAsCandidateModule() const { + if (isAlwaysExecutableModule()) { + return true; + } + return enable_simultaneous_execution_as_candidate_module_; } + bool isKeepLast() const { return keep_last_; } + void setData(const std::shared_ptr & planner_data) { planner_data_ = planner_data; } void reset() @@ -290,6 +312,8 @@ class SceneModuleManagerInterface private: bool enable_rtc_; + bool keep_last_; + size_t max_module_num_; size_t priority_; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp index 2e8c9e57f8842..8124f8f993906 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp @@ -17,16 +17,17 @@ #include "behavior_path_planner/scene_module/scene_module_interface.hpp" #include "behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp" +#include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include "behavior_path_planner/utils/path_shifter/path_shifter.hpp" +#include "behavior_path_planner/utils/start_goal_planner_common/common_module_data.hpp" #include "behavior_path_planner/utils/start_planner/freespace_pull_out.hpp" #include "behavior_path_planner/utils/start_planner/geometric_pull_out.hpp" #include "behavior_path_planner/utils/start_planner/pull_out_path.hpp" #include "behavior_path_planner/utils/start_planner/shift_pull_out.hpp" #include "behavior_path_planner/utils/start_planner/start_planner_parameters.hpp" +#include "behavior_path_planner/utils/utils.hpp" #include -#include -#include #include #include @@ -43,6 +44,11 @@ namespace behavior_path_planner { +using behavior_path_planner::utils::path_safety_checker::EgoPredictedPathParams; +using behavior_path_planner::utils::path_safety_checker::ObjectsFilteringParams; +using behavior_path_planner::utils::path_safety_checker::PoseWithVelocityStamped; +using behavior_path_planner::utils::path_safety_checker::SafetyCheckParams; +using behavior_path_planner::utils::path_safety_checker::TargetObjectsOnLane; using geometry_msgs::msg::PoseArray; using lane_departure_checker::LaneDepartureChecker; @@ -53,10 +59,16 @@ struct PullOutStatus PlannerType planner_type{PlannerType::NONE}; PathWithLaneId backward_path{}; lanelet::ConstLanelets pull_out_lanes{}; - bool is_safe_static_objects{false}; // current path is safe against static objects + bool found_pull_out_path{false}; // current path is safe against static objects bool is_safe_dynamic_objects{false}; // current path is safe against dynamic objects - bool back_finished{false}; + bool driving_forward{false}; // if ego is driving on backward path, this is set to false + bool backward_driving_complete{ + false}; // after backward driving is complete, this is set to true (warning: this is set to + // false at next cycle after backward driving is complete) Pose pull_out_start_pose{}; + bool prev_is_safe_dynamic_objects{false}; + std::shared_ptr prev_stop_path_after_approval{nullptr}; + bool has_stop_point{false}; PullOutStatus() {} }; @@ -80,16 +92,24 @@ class StartPlannerModule : public SceneModuleInterface bool isExecutionRequested() const override; bool isExecutionReady() const override; // TODO(someone): remove this, and use base class function - [[deprecated]] ModuleStatus updateState() override; + [[deprecated]] void updateCurrentState() override; BehaviorModuleOutput plan() override; BehaviorModuleOutput planWaitingApproval() override; CandidateOutput planCandidate() const override; - + void processOnEntry() override; void processOnExit() override; + void updateData() override; void setParameters(const std::shared_ptr & parameters) { parameters_ = parameters; + if (parameters->safety_check_params.enable_safety_check) { + ego_predicted_path_params_ = + std::make_shared(parameters_->ego_predicted_path_params); + objects_filtering_params_ = + std::make_shared(parameters_->objects_filtering_params); + safety_check_params_ = std::make_shared(parameters_->safety_check_params); + } } void resetStatus(); @@ -99,7 +119,7 @@ class StartPlannerModule : public SceneModuleInterface } // Condition to disable simultaneous execution - bool isBackFinished() const { return status_.back_finished; } + bool isDrivingForward() const { return status_.driving_forward; } bool isFreespacePlanning() const { return status_.planner_type == PlannerType::FREESPACE; } private: @@ -109,17 +129,22 @@ class StartPlannerModule : public SceneModuleInterface bool canTransitIdleToRunningState() override { return false; } + void initializeSafetyCheckParameters(); + std::shared_ptr parameters_; + mutable std::shared_ptr ego_predicted_path_params_; + mutable std::shared_ptr objects_filtering_params_; + mutable std::shared_ptr safety_check_params_; vehicle_info_util::VehicleInfo vehicle_info_; std::vector> start_planners_; PullOutStatus status_; + mutable StartGoalPlannerData start_planner_data_; std::deque odometry_buffer_; std::unique_ptr last_route_received_time_; std::unique_ptr last_pull_out_start_update_time_; - std::unique_ptr last_approved_pose_; // generate freespace pull out paths in a separate thread std::unique_ptr freespace_planner_; @@ -131,7 +156,8 @@ class StartPlannerModule : public SceneModuleInterface std::mutex mutex_; PathWithLaneId getFullPath() const; - std::vector searchPullOutStartPoses(); + PathWithLaneId calcStartPoseCandidatesBackwardPath() const; + std::vector searchPullOutStartPoses(const PathWithLaneId & start_pose_candidates) const; std::shared_ptr lane_departure_checker_; @@ -141,20 +167,25 @@ class StartPlannerModule : public SceneModuleInterface void incrementPathIndex(); PathWithLaneId getCurrentPath() const; void planWithPriority( - const std::vector & start_pose_candidates, const Pose & goal_pose, - const std::string search_priority); + const std::vector & start_pose_candidates, const Pose & refined_start_pose, + const Pose & goal_pose, const std::string search_priority); PathWithLaneId generateStopPath() const; lanelet::ConstLanelets getPathRoadLanes(const PathWithLaneId & path) const; std::vector generateDrivableLanes(const PathWithLaneId & path) const; void updatePullOutStatus(); + void updateStatusAfterBackwardDriving(); static bool isOverlappedWithLane( const lanelet::ConstLanelet & candidate_lanelet, const tier4_autoware_utils::LinearRing2d & vehicle_footprint); bool hasFinishedPullOut() const; - void checkBackFinished(); + bool isBackwardDrivingComplete() const; bool isStopped(); bool isStuck(); bool hasFinishedCurrentPath(); + void updateSafetyCheckTargetObjectsData( + const PredictedObjects & filtered_objects, const TargetObjectsOnLane & target_objects_on_lane, + const std::vector & ego_predicted_path) const; + bool isSafePath() const; void setDrivableAreaInfo(BehaviorModuleOutput & output) const; // check if the goal is located behind the ego in the same route segment. @@ -163,6 +194,7 @@ class StartPlannerModule : public SceneModuleInterface // generate BehaviorPathOutput with stopping path and update status BehaviorModuleOutput generateStopOutput(); + SafetyCheckParams createSafetyCheckParams() const; // freespace planner void onFreespacePlannerTimer(); bool planFreespacePath(); 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 index 8afe0576ec1aa..3a334cf67edbf 100644 --- 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 @@ -15,15 +15,13 @@ #ifndef BEHAVIOR_PATH_PLANNER__STEERING_FACTOR_INTERFACE_HPP_ #define BEHAVIOR_PATH_PLANNER__STEERING_FACTOR_INTERFACE_HPP_ -#include "rclcpp/rclcpp.hpp" +#include -#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 #include -#include namespace steering_factor_interface { 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 b550f132c27da..3a983d66eb98c 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,7 +22,7 @@ #include #include -#include +#include #include #include diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp index 1f47b804e185b..74464be0fa5ea 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp @@ -15,18 +15,20 @@ #ifndef BEHAVIOR_PATH_PLANNER__UTILS__AVOIDANCE__AVOIDANCE_MODULE_DATA_HPP_ #define BEHAVIOR_PATH_PLANNER__UTILS__AVOIDANCE__AVOIDANCE_MODULE_DATA_HPP_ -#include "behavior_path_planner/marker_utils/utils.hpp" +#include "behavior_path_planner/data_manager.hpp" #include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include "behavior_path_planner/utils/path_shifter/path_shifter.hpp" #include -#include +#include #include #include +#include #include -#include +#include +#include #include #include @@ -48,7 +50,7 @@ using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using geometry_msgs::msg::TransformStamped; -using marker_utils::CollisionCheckDebug; +using behavior_path_planner::utils::path_safety_checker::CollisionCheckDebug; struct ObjectParameter { @@ -108,72 +110,66 @@ struct AvoidanceParameters // use intersection area for avoidance bool use_intersection_areas{false}; - // // constrains - // bool use_constraints_for_decel{false}; - - // // policy - // bool use_relaxed_margin_immediately{false}; - // max deceleration for - double max_deceleration; + double max_deceleration{0.0}; // max jerk - double max_jerk; + double max_jerk{0.0}; // comfortable deceleration - double nominal_deceleration; + double nominal_deceleration{0.0}; // comfortable jerk - double nominal_jerk; + double nominal_jerk{0.0}; // To prevent large acceleration while avoidance. - double max_acceleration; + double max_acceleration{0.0}; // upper distance for envelope polygon expansion. - double upper_distance_for_polygon_expansion; + double upper_distance_for_polygon_expansion{0.0}; // lower distance for envelope polygon expansion. - double lower_distance_for_polygon_expansion; + double lower_distance_for_polygon_expansion{0.0}; // Vehicles whose distance to the center of the path is // less than this will not be considered for avoidance. - double threshold_distance_object_is_on_center; + double threshold_distance_object_is_on_center{0.0}; // execute only when there is no intersection behind of the stopped vehicle. - double object_ignore_section_traffic_light_in_front_distance; + double object_ignore_section_traffic_light_in_front_distance{0.0}; // execute only when there is no crosswalk near the stopped vehicle. - double object_ignore_section_crosswalk_in_front_distance; + double object_ignore_section_crosswalk_in_front_distance{0.0}; // execute only when there is no crosswalk near the stopped vehicle. - double object_ignore_section_crosswalk_behind_distance; + double object_ignore_section_crosswalk_behind_distance{0.0}; // distance to avoid object detection - double object_check_forward_distance; - - // continue to detect backward vehicles as avoidance targets until they are this distance away - double object_check_backward_distance; + bool use_static_detection_area{true}; + double object_check_min_forward_distance{0.0}; + double object_check_max_forward_distance{0.0}; + double object_check_backward_distance{0.0}; // if the distance between object and goal position is less than this parameter, the module ignore // the object. - double object_check_goal_distance; + double object_check_goal_distance{0.0}; // use in judge whether the vehicle is parking object on road shoulder - double object_check_shiftable_ratio; + double object_check_shiftable_ratio{0.0}; // minimum road shoulder width. maybe 0.5 [m] - double object_check_min_road_shoulder_width; + double object_check_min_road_shoulder_width{0.0}; // force avoidance - double threshold_time_force_avoidance_for_stopped_vehicle; + double threshold_time_force_avoidance_for_stopped_vehicle{0.0}; // when complete avoidance motion, there is a distance margin with the object // for longitudinal direction - double longitudinal_collision_margin_min_distance; + double longitudinal_collision_margin_min_distance{0.0}; // when complete avoidance motion, there is a time margin with the object // for longitudinal direction - double longitudinal_collision_margin_time; + double longitudinal_collision_margin_time{0.0}; // parameters for safety check area bool enable_safety_check{false}; @@ -187,41 +183,40 @@ struct AvoidanceParameters // parameters for collision check. bool check_all_predicted_path{false}; - double safety_check_time_horizon{0.0}; - double safety_check_time_resolution{0.0}; // find adjacent lane vehicles - double safety_check_backward_distance; + double safety_check_backward_distance{0.0}; // transit hysteresis (unsafe to safe) - double safety_check_hysteresis_factor; + size_t hysteresis_factor_safe_count; + double hysteresis_factor_expand_rate{0.0}; // keep target velocity in yield maneuver - double yield_velocity; + double yield_velocity{0.0}; // maximum stop distance - double stop_max_distance; + double stop_max_distance{0.0}; // stop buffer - double stop_buffer; + double stop_buffer{0.0}; // start avoidance after this time to avoid sudden path change - double prepare_time; + double prepare_time{0.0}; // Even if the vehicle speed is zero, avoidance will start after a distance of this much. - double min_prepare_distance; + double min_prepare_distance{0.0}; // minimum slow down speed - double min_slow_down_speed; + double min_slow_down_speed{0.0}; // slow down speed buffer - double buf_slow_down_speed; + double buf_slow_down_speed{0.0}; // nominal avoidance sped - double nominal_avoidance_speed; + double nominal_avoidance_speed{0.0}; // module try to return original path to keep this distance from edge point of the path. - double remain_buffer_distance; + double remain_buffer_distance{0.0}; // The margin is configured so that the generated avoidance trajectory does not come near to the // road shoulder. @@ -232,46 +227,49 @@ struct AvoidanceParameters double hard_road_shoulder_margin{1.0}; // Even if the obstacle is very large, it will not avoid more than this length for right direction - double max_right_shift_length; + double max_right_shift_length{0.0}; // Even if the obstacle is very large, it will not avoid more than this length for left direction - double max_left_shift_length; + double max_left_shift_length{0.0}; + + // Validate vehicle departure from driving lane. + double max_deviation_from_lane{0.0}; // To prevent large acceleration while avoidance. - double max_lateral_acceleration; + double max_lateral_acceleration{0.0}; // For the compensation of the detection lost. Once an object is observed, it is registered and // will be used for planning from the next time. If the object is not observed, it counts up the // lost_count and the registered object will be removed when the count exceeds this max count. - double object_last_seen_threshold; + double object_last_seen_threshold{0.0}; // The avoidance path generation is performed when the shift distance of the // avoidance points is greater than this threshold. // In multiple targets case: if there are multiple vehicles in a row to be avoided, no new // avoidance path will be generated unless their lateral margin difference exceeds this value. - double lateral_execution_threshold; + double lateral_execution_threshold{0.0}; // shift lines whose shift length is less than threshold is added a request with other large shift // line. - double lateral_small_shift_threshold; + double lateral_small_shift_threshold{0.0}; // use for judge if the ego is shifting or not. - double lateral_avoid_check_threshold; + double lateral_avoid_check_threshold{0.0}; // For shift line generation process. The continuous shift length is quantized by this value. - double quantize_filter_threshold; + double quantize_filter_threshold{0.0}; // For shift line generation process. Merge small shift lines. (First step) - double same_grad_filter_1_threshold; + double same_grad_filter_1_threshold{0.0}; // For shift line generation process. Merge small shift lines. (Second step) - double same_grad_filter_2_threshold; + double same_grad_filter_2_threshold{0.0}; // For shift line generation process. Merge small shift lines. (Third step) - double same_grad_filter_3_threshold; + double same_grad_filter_3_threshold{0.0}; // For shift line generation process. Remove sharp(=jerky) shift line. - double sharp_shift_filter_threshold; + double sharp_shift_filter_threshold{0.0}; // policy bool use_shorten_margin_immediately{false}; @@ -297,8 +295,11 @@ struct AvoidanceParameters // parameters depend on object class std::unordered_map object_parameters; + // ego predicted path params. + utils::path_safety_checker::EgoPredictedPathParams ego_predicted_path_params{}; + // rss parameters - utils::path_safety_checker::RSSparams rss_params; + utils::path_safety_checker::RSSparams rss_params{}; // clip left and right bounds for objects bool enable_bound_clipping{false}; @@ -392,6 +393,9 @@ using ObjectDataArray = std::vector; */ struct AvoidLine : public ShiftLine { + // object side + bool object_on_right = true; + // Distance from ego to start point in Frenet double start_longitudinal = 0.0; @@ -415,6 +419,21 @@ struct AvoidLine : public ShiftLine }; using AvoidLineArray = std::vector; +struct AvoidOutline +{ + AvoidOutline(const AvoidLine & avoid_line, const AvoidLine & return_line) + : avoid_line{avoid_line}, return_line{return_line} + { + } + + AvoidLine avoid_line{}; + + AvoidLine return_line{}; + + AvoidLineArray middle_lines{}; +}; +using AvoidOutlines = std::vector; + /* * avoidance state */ @@ -466,16 +485,24 @@ struct AvoidancePlanningData boost::optional stop_target_object{boost::none}; // raw shift point - AvoidLineArray unapproved_raw_sl{}; + AvoidLineArray raw_shift_line{}; // new shift point - AvoidLineArray unapproved_new_sl{}; + AvoidLineArray new_shift_line{}; // safe shift point - AvoidLineArray safe_new_sl{}; + AvoidLineArray safe_shift_line{}; + + std::vector drivable_lanes{}; + + lanelet::BasicLineString3d right_bound{}; + + lanelet::BasicLineString3d left_bound{}; bool safe{false}; + bool valid{false}; + bool comfortable{false}; bool avoid_required{false}; @@ -522,22 +549,27 @@ struct DebugData lanelet::ConstLineStrings3d bounds; - AvoidLineArray current_shift_lines; // in path shifter - AvoidLineArray new_shift_lines; // in path shifter + // combine process + AvoidLineArray step1_registered_shift_line; + AvoidLineArray step1_current_shift_line; + AvoidLineArray step1_filled_shift_line; + AvoidLineArray step1_merged_shift_line; + AvoidLineArray step1_combined_shift_line; + AvoidLineArray step1_return_shift_line; + AvoidLineArray step1_front_shift_line; + + // create outline process + AvoidLineArray step2_merged_shift_line; - AvoidLineArray registered_raw_shift; - AvoidLineArray current_raw_shift; - AvoidLineArray extra_return_shift; + // trimming process + AvoidLineArray step3_quantize_filtered; + AvoidLineArray step3_noise_filtered; + AvoidLineArray step3_grad_filtered_1st; + AvoidLineArray step3_grad_filtered_2nd; + AvoidLineArray step3_grad_filtered_3rd; - AvoidLineArray merged; - AvoidLineArray gap_filled; - AvoidLineArray trim_similar_grad_shift; - AvoidLineArray quantized; - AvoidLineArray trim_small_shift; - AvoidLineArray trim_similar_grad_shift_second; - AvoidLineArray trim_similar_grad_shift_third; - AvoidLineArray trim_momentary_return; - AvoidLineArray trim_too_sharp_shift; + // registered process + AvoidLineArray step4_new_shift_line; // shift length std::vector pos_shift; @@ -554,9 +586,6 @@ struct DebugData // shift path std::vector proposed_spline_shift; - // future pose - PathWithLaneId path_with_planned_velocity; - // avoidance require objects ObjectDataArray unavoidable_objects; @@ -566,6 +595,10 @@ struct DebugData // tmp for plot PathWithLaneId center_line; + // collision check debug map + utils::path_safety_checker::CollisionCheckDebugMap collision_check; + + // debug msg array AvoidanceDebugMsgArray avoidance_debug_msg_array; }; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/helper.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/helper.hpp index 77fe2e29c4978..11388dd6bb74a 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/helper.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/helper.hpp @@ -173,6 +173,22 @@ class AvoidanceHelper : std::max(shift_length, getRightShiftBound()); } + double getForwardDetectionRange() const + { + if (parameters_->use_static_detection_area) { + return parameters_->object_check_max_forward_distance; + } + + const auto max_shift_length = std::max( + std::abs(parameters_->max_right_shift_length), std::abs(parameters_->max_left_shift_length)); + const auto dynamic_distance = PathShifter::calcLongitudinalDistFromJerk( + max_shift_length, getLateralMinJerkLimit(), getEgoSpeed()); + + return std::clamp( + 1.5 * dynamic_distance, parameters_->object_check_min_forward_distance, + parameters_->object_check_max_forward_distance); + } + void alignShiftLinesOrder(AvoidLineArray & lines, const bool align_shift_length = true) const { if (lines.empty()) { diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp index 6b7830147ebf9..0237fb458ea0b 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp @@ -18,11 +18,8 @@ #include "behavior_path_planner/data_manager.hpp" #include "behavior_path_planner/utils/avoidance/avoidance_module_data.hpp" #include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" -#include "behavior_path_planner/utils/path_safety_checker/safety_check.hpp" -#include #include -#include #include #include @@ -70,7 +67,7 @@ void fillLongitudinalAndLengthByClosestEnvelopeFootprint( const PathWithLaneId & path, const Point & ego_pos, ObjectData & obj); double calcEnvelopeOverhangDistance( - const ObjectData & object_data, const Pose & base_pose, Point & overhang_pose); + const ObjectData & object_data, const PathWithLaneId & path, Point & overhang_pose); void setEndData( AvoidLine & al, const double length, const geometry_msgs::msg::Pose & end, const size_t end_idx, @@ -80,6 +77,9 @@ void setStartData( AvoidLine & al, const double start_shift_length, const geometry_msgs::msg::Pose & start, const size_t start_idx, const double start_dist); +Polygon2d createEnvelopePolygon( + const Polygon2d & object_polygon, const Pose & closest_pose, const double envelope_buffer); + Polygon2d createEnvelopePolygon( const ObjectData & object_data, const Pose & closest_pose, const double envelope_buffer); @@ -87,10 +87,6 @@ std::vector generateObstaclePolygonsForDrivableArea( const ObjectDataArray & objects, const std::shared_ptr & parameters, const double vehicle_width); -std::vector convertToPredictedPath( - const PathWithLaneId & path, const std::shared_ptr & planner_data, - const std::shared_ptr & parameters); - double getLongitudinalVelocity(const Pose & p_ref, const Pose & p_target, const double v); bool isCentroidWithinLanelets( @@ -149,6 +145,11 @@ double extendToRoadShoulderDistanceWithPolygon( void fillAdditionalInfoFromPoint(const AvoidancePlanningData & data, AvoidLineArray & lines); +void fillAdditionalInfoFromLongitudinal(const AvoidancePlanningData & data, AvoidLine & line); + +void fillAdditionalInfoFromLongitudinal( + const AvoidancePlanningData & data, AvoidOutlines & outlines); + void fillAdditionalInfoFromLongitudinal(const AvoidancePlanningData & data, AvoidLineArray & lines); AvoidLine fillAdditionalInfo(const AvoidancePlanningData & data, const AvoidLine & line); @@ -156,9 +157,6 @@ AvoidLine fillAdditionalInfo(const AvoidancePlanningData & data, const AvoidLine AvoidLineArray combineRawShiftLinesWithUniqueCheck( const AvoidLineArray & base_lines, const AvoidLineArray & added_lines); -ExtendedPredictedObject transform( - const PredictedObject & object, const std::shared_ptr & parameters); - std::vector getSafetyCheckTargetObjects( const AvoidancePlanningData & data, const std::shared_ptr & planner_data, const std::shared_ptr & parameters, const bool is_right_shift); @@ -166,7 +164,11 @@ std::vector getSafetyCheckTargetObjects( std::pair separateObjectsByPath( const PathWithLaneId & path, const std::shared_ptr & planner_data, const AvoidancePlanningData & data, const std::shared_ptr & parameters, - DebugData & debug); + const double object_check_forward_distance, const bool is_running, DebugData & debug); + +DrivableLanes generateExpandDrivableLanes( + const lanelet::ConstLanelet & lanelet, const std::shared_ptr & planner_data, + const std::shared_ptr & parameters); } // namespace behavior_path_planner::utils::avoidance #endif // BEHAVIOR_PATH_PLANNER__UTILS__AVOIDANCE__UTILS_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/drivable_area_expansion.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/drivable_area_expansion.hpp index 53ef91473fe24..3f6f107cdce51 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/drivable_area_expansion.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/drivable_area_expansion.hpp @@ -15,6 +15,7 @@ #ifndef BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__DRIVABLE_AREA_EXPANSION_HPP_ #define BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__DRIVABLE_AREA_EXPANSION_HPP_ +#include "behavior_path_planner/data_manager.hpp" #include "behavior_path_planner/utils/drivable_area_expansion/parameters.hpp" #include "behavior_path_planner/utils/drivable_area_expansion/types.hpp" @@ -22,31 +23,76 @@ #include +#include +#include + namespace drivable_area_expansion { -/// @brief Expand the drivable area based on the projected ego footprint along the path -/// @param[in] path path whose drivable area will be expanded -/// @param[in] params expansion parameters -/// @param[in] dynamic_objects dynamic objects -/// @param[in] route_handler route handler -/// @param[in] path_lanes lanelets of the path -void expandDrivableArea( - PathWithLaneId & path, const DrivableAreaExpansionParameters & params, - const PredictedObjects & dynamic_objects, const route_handler::RouteHandler & route_handler, - const lanelet::ConstLanelets & path_lanes); - -/// @brief Create a polygon combining the drivable area of a path and some expansion polygons -/// @param[in] path path and its drivable area -/// @param[in] expansion_polygons polygons to add to the drivable area -/// @return expanded drivable area polygon -polygon_t createExpandedDrivableAreaPolygon( - const PathWithLaneId & path, const multi_polygon_t & expansion_polygons); - -/// @brief Update the drivable area of the given path with the given polygon -/// @details this function splits the polygon into a left and right bound and sets it in the path -/// @param[in] path path whose drivable area will be expanded -/// @param[in] expanded_drivable_area polygon of the new drivable area -void updateDrivableAreaBounds(PathWithLaneId & path, const polygon_t & expanded_drivable_area); +/// @brief Expand the drivable area based on the path curvature and the vehicle dimensions +/// @param[inout] path path whose drivable area will be expanded +/// @param[inout] planner_data planning data (params, dynamic objects, vehicle info, ...) +void expand_drivable_area( + PathWithLaneId & path, + const std::shared_ptr planner_data); + +/// @brief prepare path poses and try to reuse their previously calculated curvatures +/// @details poses are reused if they do not deviate too much from the current path +/// @param [in] path input path +/// @param [inout] prev_poses previous poses to reuse +/// @param [inout] prev_curvatures previous curvatures to reuse +/// @param [in] ego_point current ego point +/// @param [in] params parameters for reuse criteria and resampling interval +void update_path_poses_and_previous_curvatures( + const PathWithLaneId & path, std::vector & prev_poses, + std::vector & prev_curvatures, const Point & ego_point, + const DrivableAreaExpansionParameters & params); + +/// @brief calculate the minimum lane width based on the path curvature and the vehicle dimensions +/// @cite Lim, H., Kim, C., and Jo, A., "Model Predictive Control-Based Lateral Control of +/// Autonomous Large-Size Bus on Road with Large Curvature," SAE Technical Paper 2021-01-0099, 2021, +/// https://doi.org/10.4271/2021-01-0099 +/// @param [in] curvature curvature +/// @param [in] params parameters containing the vehicle dimensions +/// @return minimum lane width +double calculate_minimum_lane_width( + const double curvature_radius, const DrivableAreaExpansionParameters & params); + +/// @brief smooth the bound by applying a limit on its rate of change +/// @details rate of change is the lateral distance from the path over the arc length along the path +/// @param [inout] bound_distances bound distances (lateral distance from the path) +/// @param [in] bound_points bound points +/// @param [in] max_rate [m/m] maximum rate of lateral deviation over arc length +void apply_bound_change_rate_limit( + std::vector & distances, const std::vector & bound, const double max_rate); + +/// @brief calculate the maximum distance by which a bound can be expanded +/// @param [in] path_poses input path +/// @param [in] bound bound points +/// @param [in] uncrossable_segments segments that limit the bound expansion, indexed in a Rtree +/// @param [in] uncrossable_polygons polygons that limit the bound expansion +/// @param [in] params parameters with the buffer distance to keep with lines, +/// and the static maximum expansion distance +std::vector calculate_maximum_distance( + const std::vector & path_poses, const std::vector bound, + const SegmentRtree & uncrossable_lines, const std::vector & uncrossable_polygons, + const DrivableAreaExpansionParameters & params); + +/// @brief expand a bound by the given lateral distances away from the path +/// @param [inout] bound bound points to expand +/// @param [in] path_poses input path +/// @param [in] distances new lateral distances of the bound points away from the path +void expand_bound( + std::vector & bound, const std::vector & path_poses, + const std::vector & distances); + +/// @brief calculate smoothed curvatures +/// @details smoothing is done using a moving average +/// @param [in] poses input poses used to calculate the curvatures +/// @param [in] smoothing_window_size size of the window used for the moving average +/// @return smoothed curvatures of the input poses +std::vector calculate_smoothed_curvatures( + const std::vector & poses, const size_t smoothing_window_size); + } // namespace drivable_area_expansion #endif // BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__DRIVABLE_AREA_EXPANSION_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/expansion.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/expansion.hpp deleted file mode 100644 index 70cc8a8bc5925..0000000000000 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/expansion.hpp +++ /dev/null @@ -1,85 +0,0 @@ -// Copyright 2023 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__UTILS__DRIVABLE_AREA_EXPANSION__EXPANSION_HPP_ -#define BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__EXPANSION_HPP_ - -#include "behavior_path_planner/utils/drivable_area_expansion/parameters.hpp" -#include "behavior_path_planner/utils/drivable_area_expansion/types.hpp" - -#include - -#include - -#include -#include - -namespace drivable_area_expansion -{ -/// @brief Calculate the distance limit required for the polygon to not cross the limit lines -/// @details Calculate the minimum distance from base_ls to an intersection of limit_lines and -/// expansion_polygon -/// @param[in] base_ls base linestring from which the distance is calculated -/// @param[in] expansion_polygon polygon to consider -/// @param[in] limit_lines lines we do not want to cross -/// @return distance limit -double calculateDistanceLimit( - const linestring_t & base_ls, const polygon_t & expansion_polygon, - const multi_linestring_t & limit_lines); - -/// @brief Calculate the distance limit required for the polygon to not cross the limit polygons. -/// @details Calculate the minimum distance from base_ls to an intersection of limit_polygons and -/// expansion_polygon -/// @param[in] base_ls base linestring from which the distance is calculated -/// @param[in] expansion_polygon polygon to consider -/// @param[in] limit_polygons polygons we do not want to cross -/// @return distance limit -double calculateDistanceLimit( - const linestring_t & base_ls, const polygon_t & expansion_polygon, - const multi_polygon_t & limit_polygons); - -/// @brief Create a polygon from a base line with a given expansion distance -/// @param[in] base_ls base linestring from which the polygon is created -/// @param[in] dist desired expansion distance from the base line -/// @param[in] is_left_side desired side of the expansion from the base line -/// @return expansion polygon -polygon_t createExpansionPolygon( - const linestring_t & base_ls, const double dist, const bool is_left_side); - -/// @brief Create polygons for the area where the drivable area should be expanded -/// @param[in] path path and its drivable area -/// @param[in] path_footprints polygons of the ego footprint projected along the path -/// @param[in] predicted_paths polygons of the dynamic objects' predicted paths -/// @param[in] uncrossable_lines lines that should not be crossed by the expanded drivable area -/// @param[in] params expansion parameters -/// @return expansion polygons -multi_polygon_t createExpansionPolygons( - const PathWithLaneId & path, const multi_polygon_t & path_footprints, - const multi_polygon_t & predicted_paths, const multi_linestring_t & uncrossable_lines, - const DrivableAreaExpansionParameters & params); - -/// @brief Create polygons for the area where the drivable area should be expanded -/// @param[in] path_lanes lanelets of the current path -/// @param[in] route_handler route handler -/// @param[in] path_footprints polygons of the ego footprint projected along the path -/// @param[in] predicted_paths polygons of the dynamic objects' predicted paths -/// @param[in] params expansion parameters -/// @return expansion polygons -multi_polygon_t createExpansionLaneletPolygons( - const lanelet::ConstLanelets & path_lanes, const route_handler::RouteHandler & route_handler, - const multi_polygon_t & path_footprints, const multi_polygon_t & predicted_paths, - const DrivableAreaExpansionParameters & params); -} // namespace drivable_area_expansion - -#endif // BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__EXPANSION_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/footprints.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/footprints.hpp index e5e1c76f2521f..9591ac0582e04 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/footprints.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/footprints.hpp @@ -20,7 +20,6 @@ #include -#include "autoware_auto_planning_msgs/msg/detail/path_point__struct.hpp" #include #include #include @@ -31,11 +30,6 @@ #include #include -#include -#include -#include -#include - namespace drivable_area_expansion { /// @brief translate a polygon by some (x,y) vector @@ -43,27 +37,20 @@ namespace drivable_area_expansion /// @param[in] x translation distance on the x axis /// @param[in] y translation distance on the y axis /// @return translated polygon -polygon_t translatePolygon(const polygon_t & polygon, const double x, const double y); +Polygon2d translate_polygon(const Polygon2d & polygon, const double x, const double y); /// @brief create the footprint of a pose and its base footprint /// @param[in] pose the origin pose of the footprint /// @param[in] base_footprint the base axis-aligned footprint /// @return footprint polygon -polygon_t createFootprint(const geometry_msgs::msg::Pose & pose, const polygon_t base_footprint); +Polygon2d create_footprint(const geometry_msgs::msg::Pose & pose, const Polygon2d base_footprint); /// @brief create footprints of the predicted paths of an object /// @param [in] objects objects from which to create polygons /// @param[in] params expansion parameters containing extra offsets to add to the dynamic objects /// @return footprint polygons of the object's predicted paths -multi_polygon_t createObjectFootprints( +MultiPolygon2d create_object_footprints( const autoware_auto_perception_msgs::msg::PredictedObjects & objects, const DrivableAreaExpansionParameters & params); - -/// @brief create the footprint polygon from a path -/// @param[in] path the path for which to create a footprint -/// @param[in] params expansion parameters defining how to create the footprint -/// @return footprint polygons of the path -multi_polygon_t createPathFootprints( - const PathWithLaneId & path, const DrivableAreaExpansionParameters & params); } // namespace drivable_area_expansion #endif // BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__FOOTPRINTS_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/map_utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/map_utils.hpp index 4524bd2be2299..8c6bdb8a6943b 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/map_utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/map_utils.hpp @@ -15,6 +15,7 @@ #ifndef BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__MAP_UTILS_HPP_ #define BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__MAP_UTILS_HPP_ +#include "behavior_path_planner/utils/drivable_area_expansion/parameters.hpp" #include "behavior_path_planner/utils/drivable_area_expansion/types.hpp" #include @@ -24,18 +25,20 @@ namespace drivable_area_expansion { -/// @brief Extract uncrossable linestrings from the lanelet map +/// @brief Extract uncrossable segments from the lanelet map that are in range of ego /// @param[in] lanelet_map lanelet map -/// @param[in] uncrossable_types types that cannot be crossed -/// @return the uncrossable linestrings -multi_linestring_t extractUncrossableLines( - const lanelet::LaneletMap & lanelet_map, const std::vector & uncrossable_types); +/// @param[in] ego_point point of the current ego position +/// @param[in] params parameters with linestring types that cannot be crossed and maximum range +/// @return the uncrossable segments stored in a rtree +SegmentRtree extract_uncrossable_segments( + const lanelet::LaneletMap & lanelet_map, const Point & ego_point, + const DrivableAreaExpansionParameters & params); /// @brief Determine if the given linestring has one of the given types /// @param[in] ls linestring to check /// @param[in] types type strings to check /// @return true if the linestring has one of the given types -bool hasTypes(const lanelet::ConstLineString3d & ls, const std::vector & types); +bool has_types(const lanelet::ConstLineString3d & ls, const std::vector & types); } // namespace drivable_area_expansion #endif // BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__MAP_UTILS_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/parameters.hpp index a4c11c68d13ec..2f9604223a248 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/parameters.hpp @@ -15,8 +15,6 @@ #ifndef BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__PARAMETERS_HPP_ #define BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__PARAMETERS_HPP_ -#include "behavior_path_planner/utils/drivable_area_expansion/types.hpp" - #include #include @@ -32,12 +30,9 @@ struct DrivableAreaExpansionParameters static constexpr auto DRIVABLE_AREA_LEFT_BOUND_OFFSET_PARAM = "drivable_area_left_bound_offset"; static constexpr auto DRIVABLE_AREA_TYPES_TO_SKIP_PARAM = "drivable_area_types_to_skip"; static constexpr auto ENABLED_PARAM = "dynamic_expansion.enabled"; - static constexpr auto EGO_EXTRA_OFFSET_FRONT = - "dynamic_expansion.ego.extra_footprint_offset.front"; - static constexpr auto EGO_EXTRA_OFFSET_REAR = "dynamic_expansion.ego.extra_footprint_offset.rear"; - static constexpr auto EGO_EXTRA_OFFSET_LEFT = "dynamic_expansion.ego.extra_footprint_offset.left"; - static constexpr auto EGO_EXTRA_OFFSET_RIGHT = - "dynamic_expansion.ego.extra_footprint_offset.right"; + static constexpr auto EGO_EXTRA_FRONT_OVERHANG = "dynamic_expansion.ego.extra_front_overhang"; + static constexpr auto EGO_EXTRA_WHEELBASE = "dynamic_expansion.ego.extra_wheelbase"; + static constexpr auto EGO_EXTRA_WIDTH = "dynamic_expansion.ego.extra_width"; static constexpr auto DYN_OBJECTS_EXTRA_OFFSET_FRONT = "dynamic_expansion.dynamic_objects.extra_footprint_offset.front"; static constexpr auto DYN_OBJECTS_EXTRA_OFFSET_REAR = @@ -46,43 +41,49 @@ struct DrivableAreaExpansionParameters "dynamic_expansion.dynamic_objects.extra_footprint_offset.left"; static constexpr auto DYN_OBJECTS_EXTRA_OFFSET_RIGHT = "dynamic_expansion.dynamic_objects.extra_footprint_offset.right"; - static constexpr auto EXPANSION_METHOD_PARAM = "dynamic_expansion.expansion.method"; - static constexpr auto MAX_EXP_DIST_PARAM = "dynamic_expansion.expansion.max_distance"; + static constexpr auto MAX_EXP_DIST_PARAM = "dynamic_expansion.max_expansion_distance"; + static constexpr auto RESAMPLE_INTERVAL_PARAM = + "dynamic_expansion.path_preprocessing.resample_interval"; static constexpr auto MAX_PATH_ARC_LENGTH_PARAM = - "dynamic_expansion.expansion.max_path_arc_length"; - static constexpr auto EXTRA_ARC_LENGTH_PARAM = "dynamic_expansion.expansion.extra_arc_length"; + "dynamic_expansion.path_preprocessing.max_arc_length"; + static constexpr auto MAX_REUSE_DEVIATION_PARAM = + "dynamic_expansion.path_preprocessing.reuse_max_deviation"; static constexpr auto AVOID_DYN_OBJECTS_PARAM = "dynamic_expansion.dynamic_objects.avoid"; static constexpr auto AVOID_LINESTRING_TYPES_PARAM = "dynamic_expansion.avoid_linestring.types"; static constexpr auto AVOID_LINESTRING_DIST_PARAM = "dynamic_expansion.avoid_linestring.distance"; - static constexpr auto COMPENSATE_PARAM = "dynamic_expansion.avoid_linestring.compensate.enable"; - static constexpr auto EXTRA_COMPENSATE_PARAM = - "dynamic_expansion.avoid_linestring.compensate.extra_distance"; + static constexpr auto SMOOTHING_CURVATURE_WINDOW_PARAM = + "dynamic_expansion.smoothing.curvature_average_window"; + static constexpr auto SMOOTHING_MAX_BOUND_RATE_PARAM = + "dynamic_expansion.smoothing.max_bound_rate"; + static constexpr auto SMOOTHING_EXTRA_ARC_LENGTH_PARAM = + "dynamic_expansion.smoothing.extra_arc_length"; + static constexpr auto PRINT_RUNTIME_PARAM = "dynamic_expansion.print_runtime"; - double drivable_area_right_bound_offset; - double drivable_area_left_bound_offset; - std::vector drivable_area_types_to_skip; + // static expansion + double drivable_area_right_bound_offset{}; + double drivable_area_left_bound_offset{}; + std::vector drivable_area_types_to_skip{}; + // dynamic expansion bool enabled = false; - std::string expansion_method{}; double avoid_linestring_dist{}; - double ego_left_offset{}; - double ego_right_offset{}; - double ego_rear_offset{}; - double ego_front_offset{}; - double ego_extra_left_offset{}; - double ego_extra_right_offset{}; - double ego_extra_rear_offset{}; - double ego_extra_front_offset{}; + double extra_front_overhang{}; + double extra_wheelbase{}; + double extra_width{}; + int curvature_average_window{}; + double max_bound_rate{}; double dynamic_objects_extra_left_offset{}; double dynamic_objects_extra_right_offset{}; double dynamic_objects_extra_rear_offset{}; double dynamic_objects_extra_front_offset{}; double max_expansion_distance{}; double max_path_arc_length{}; + double resample_interval{}; double extra_arc_length{}; + double max_reuse_deviation{}; bool avoid_dynamic_objects{}; + bool print_runtime{}; std::vector avoid_linestring_types{}; - bool compensate_uncrossable_lines = false; - double compensate_extra_dist{}; + vehicle_info_util::VehicleInfo vehicle_info; DrivableAreaExpansionParameters() = default; explicit DrivableAreaExpansionParameters(rclcpp::Node & node) { init(node); } @@ -97,11 +98,15 @@ struct DrivableAreaExpansionParameters node.declare_parameter>(DRIVABLE_AREA_TYPES_TO_SKIP_PARAM); enabled = node.declare_parameter(ENABLED_PARAM); max_expansion_distance = node.declare_parameter(MAX_EXP_DIST_PARAM); + extra_front_overhang = node.declare_parameter(EGO_EXTRA_FRONT_OVERHANG); + extra_wheelbase = node.declare_parameter(EGO_EXTRA_WHEELBASE); + extra_width = node.declare_parameter(EGO_EXTRA_WIDTH); + curvature_average_window = node.declare_parameter(SMOOTHING_CURVATURE_WINDOW_PARAM); + max_bound_rate = node.declare_parameter(SMOOTHING_MAX_BOUND_RATE_PARAM); + extra_arc_length = node.declare_parameter(SMOOTHING_EXTRA_ARC_LENGTH_PARAM); max_path_arc_length = node.declare_parameter(MAX_PATH_ARC_LENGTH_PARAM); - ego_extra_front_offset = node.declare_parameter(EGO_EXTRA_OFFSET_FRONT); - ego_extra_rear_offset = node.declare_parameter(EGO_EXTRA_OFFSET_REAR); - ego_extra_left_offset = node.declare_parameter(EGO_EXTRA_OFFSET_LEFT); - ego_extra_right_offset = node.declare_parameter(EGO_EXTRA_OFFSET_RIGHT); + resample_interval = node.declare_parameter(RESAMPLE_INTERVAL_PARAM); + max_reuse_deviation = node.declare_parameter(MAX_REUSE_DEVIATION_PARAM); dynamic_objects_extra_front_offset = node.declare_parameter(DYN_OBJECTS_EXTRA_OFFSET_FRONT); dynamic_objects_extra_rear_offset = @@ -114,16 +119,9 @@ struct DrivableAreaExpansionParameters node.declare_parameter>(AVOID_LINESTRING_TYPES_PARAM); avoid_dynamic_objects = node.declare_parameter(AVOID_DYN_OBJECTS_PARAM); avoid_linestring_dist = node.declare_parameter(AVOID_LINESTRING_DIST_PARAM); - extra_arc_length = node.declare_parameter(EXTRA_ARC_LENGTH_PARAM); - compensate_uncrossable_lines = node.declare_parameter(COMPENSATE_PARAM); - compensate_extra_dist = node.declare_parameter(EXTRA_COMPENSATE_PARAM); - expansion_method = node.declare_parameter(EXPANSION_METHOD_PARAM); + print_runtime = node.declare_parameter(PRINT_RUNTIME_PARAM); - const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo(); - ego_left_offset = vehicle_info.max_lateral_offset_m; - ego_right_offset = vehicle_info.min_lateral_offset_m; - ego_rear_offset = vehicle_info.min_longitudinal_offset_m; - ego_front_offset = vehicle_info.max_longitudinal_offset_m; + vehicle_info = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo(); } }; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/path_projection.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/path_projection.hpp index 3e2b177f59167..3f5327acbca68 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/path_projection.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/path_projection.hpp @@ -21,7 +21,6 @@ #include -#include #include namespace drivable_area_expansion @@ -33,10 +32,10 @@ namespace drivable_area_expansion /// @param p2 second segment point /// @return projected point and corresponding distance inline PointDistance point_to_segment_projection( - const point_t & p, const point_t & p1, const point_t & p2) + const Point2d & p, const Point2d & p1, const Point2d & p2) { - const point_t p2_vec = {p2.x() - p1.x(), p2.y() - p1.y()}; - const point_t p_vec = {p.x() - p1.x(), p.y() - p1.y()}; + const Point2d p2_vec = {p2.x() - p1.x(), p2.y() - p1.y()}; + const Point2d p_vec = {p.x() - p1.x(), p.y() - p1.y()}; const auto cross = p2_vec.x() * p_vec.y() - p2_vec.y() * p_vec.x(); const auto dist_sign = cross < 0.0 ? -1.0 : 1.0; @@ -48,7 +47,7 @@ inline PointDistance point_to_segment_projection( if (c2 <= c1) return {p2, boost::geometry::distance(p, p2) * dist_sign}; const auto projection = p1 + (p2_vec * c1 / c2); - const auto projection_point = point_t{projection.x(), projection.y()}; + const auto projection_point = Point2d{projection.x(), projection.y()}; return {projection_point, boost::geometry::distance(p, projection_point) * dist_sign}; } @@ -59,10 +58,10 @@ inline PointDistance point_to_segment_projection( /// @param p2 second line point /// @return projected point and corresponding distance inline PointDistance point_to_line_projection( - const point_t & p, const point_t & p1, const point_t & p2) + const Point2d & p, const Point2d & p1, const Point2d & p2) { - const point_t p2_vec = {p2.x() - p1.x(), p2.y() - p1.y()}; - const point_t p_vec = {p.x() - p1.x(), p.y() - p1.y()}; + const Point2d p2_vec = {p2.x() - p1.x(), p2.y() - p1.y()}; + const Point2d p_vec = {p.x() - p1.x(), p.y() - p1.y()}; const auto cross = p2_vec.x() * p_vec.y() - p2_vec.y() * p_vec.x(); const auto dist_sign = cross < 0.0 ? -1.0 : 1.0; @@ -70,7 +69,7 @@ inline PointDistance point_to_line_projection( const auto c1 = boost::geometry::dot_product(p_vec, p2_vec); const auto c2 = boost::geometry::dot_product(p2_vec, p2_vec); const auto projection = p1 + (p2_vec * c1 / c2); - const auto projection_point = point_t{projection.x(), projection.y()}; + const auto projection_point = Point2d{projection.x(), projection.y()}; return {projection_point, boost::geometry::distance(p, projection_point) * dist_sign}; } @@ -78,7 +77,7 @@ inline PointDistance point_to_line_projection( /// @param p point to project /// @param ls linestring /// @return projected point, corresponding distance, and arc length along the linestring -inline Projection point_to_linestring_projection(const point_t & p, const linestring_t & ls) +inline Projection point_to_linestring_projection(const Point2d & p, const LineString2d & ls) { Projection closest; closest.distance = std::numeric_limits::max(); @@ -100,14 +99,14 @@ inline Projection point_to_linestring_projection(const point_t & p, const linest /// @param p2 second vector point /// @param dist distance /// @return point p such that (p1,p) is orthogonal to (p1,p2) at the given distance -inline point_t normal_at_distance(const point_t & p1, const point_t & p2, const double dist) +inline Point2d normal_at_distance(const Point2d & p1, const Point2d & p2, const double dist) { auto base = p1; auto normal_vector = p2; boost::geometry::subtract_point(normal_vector, base); boost::geometry::detail::vec_normalize(normal_vector); boost::geometry::multiply_value(normal_vector, dist); - return point_t{base.x() - normal_vector.y(), base.y() + normal_vector.x()}; + return Point2d{base.x() - normal_vector.y(), base.y() + normal_vector.x()}; } /// @brief interpolate between two points @@ -115,7 +114,7 @@ inline point_t normal_at_distance(const point_t & p1, const point_t & p2, const /// @param b second point /// @param ratio interpolation ratio such that 0 yields a, and 1 yields b /// @return point interpolated between a and b as per the given ratio -inline point_t lerp_point(const point_t & a, const point_t & b, const double ratio) +inline Point2d lerp_point(const Point2d & a, const Point2d & b, const double ratio) { return {interpolation::lerp(a.x(), b.x(), ratio), interpolation::lerp(a.y(), b.y(), ratio)}; } @@ -125,10 +124,10 @@ inline point_t lerp_point(const point_t & a, const point_t & b, const double rat /// @param arc_length arc length along the reference linestring of the resulting point /// @param distance distance from the reference linestring of the resulting point /// @return point at the distance and arc length relative to the reference linestring -inline segment_t linestring_to_point_projection( - const linestring_t & ls, const double arc_length, const double distance) +inline Segment2d linestring_to_point_projection( + const LineString2d & ls, const double arc_length, const double distance) { - if (ls.empty()) return segment_t{}; + if (ls.empty()) return Segment2d{}; if (ls.size() == 1lu) return {ls.front(), ls.front()}; auto ls_iterator = ls.begin(); auto prev_arc_length = 0.0; @@ -156,10 +155,10 @@ inline segment_t linestring_to_point_projection( /// @param from_arc_length arc length of the first point of the sub linestring /// @param to_arc_length arc length of the last point of the sub linestring /// @return sub linestring -inline linestring_t sub_linestring( - const linestring_t & ls, const double from_arc_length, const double to_arc_length) +inline LineString2d sub_linestring( + const LineString2d & ls, const double from_arc_length, const double to_arc_length) { - linestring_t sub_ls; + LineString2d sub_ls; if (from_arc_length >= to_arc_length || ls.empty()) throw(std::runtime_error("sub_linestring: bad inputs")); diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/types.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/types.hpp index daabfa97598fd..8da1521db6c28 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/types.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/types.hpp @@ -22,32 +22,39 @@ #include #include +#include + namespace drivable_area_expansion { using autoware_auto_perception_msgs::msg::PredictedObjects; using autoware_auto_planning_msgs::msg::PathPoint; +using autoware_auto_planning_msgs::msg::PathPointWithLaneId; using autoware_auto_planning_msgs::msg::PathWithLaneId; using geometry_msgs::msg::Point; +using geometry_msgs::msg::Pose; + +using tier4_autoware_utils::LineString2d; +using tier4_autoware_utils::MultiLineString2d; +using tier4_autoware_utils::MultiPoint2d; +using tier4_autoware_utils::MultiPolygon2d; +using tier4_autoware_utils::Point2d; +using tier4_autoware_utils::Polygon2d; +using tier4_autoware_utils::Segment2d; -using point_t = tier4_autoware_utils::Point2d; -using multi_point_t = tier4_autoware_utils::MultiPoint2d; -using polygon_t = tier4_autoware_utils::Polygon2d; -using ring_t = tier4_autoware_utils::LinearRing2d; -using multi_polygon_t = tier4_autoware_utils::MultiPolygon2d; -using segment_t = tier4_autoware_utils::Segment2d; -using linestring_t = tier4_autoware_utils::LineString2d; -using multi_linestring_t = tier4_autoware_utils::MultiLineString2d; +typedef boost::geometry::index::rtree> SegmentRtree; struct PointDistance { - point_t point; + Point2d point; double distance; }; struct Projection { - point_t projected_point; + Point2d projected_point; double distance; double arc_length; }; +enum Side { LEFT, RIGHT }; + } // namespace drivable_area_expansion #endif // BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__TYPES_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp index cb769a1f2884c..f4ab10db61b9c 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp @@ -28,12 +28,9 @@ #include #include -#include -#include +#include -#include #include -#include #include #include @@ -47,29 +44,28 @@ using geometry_msgs::msg::PoseArray; struct ParallelParkingParameters { - double th_arrived_distance; - double th_stopped_velocity; - double maximum_deceleration; + // common + double center_line_path_interval{0.0}; // forward parking - double after_forward_parking_straight_distance; - double forward_parking_velocity; - double forward_parking_lane_departure_margin; - double forward_parking_path_interval; - double forward_parking_max_steer_angle; + double after_forward_parking_straight_distance{0.0}; + double forward_parking_velocity{0.0}; + double forward_parking_lane_departure_margin{0.0}; + double forward_parking_path_interval{0.0}; + double forward_parking_max_steer_angle{0.0}; // backward parking - double after_backward_parking_straight_distance; - double backward_parking_velocity; - double backward_parking_lane_departure_margin; - double backward_parking_path_interval; - double backward_parking_max_steer_angle; + double after_backward_parking_straight_distance{0.0}; + double backward_parking_velocity{0.0}; + double backward_parking_lane_departure_margin{0.0}; + double backward_parking_path_interval{0.0}; + double backward_parking_max_steer_angle{0.0}; // pull_out - double pull_out_velocity; - double pull_out_lane_departure_margin; - double pull_out_path_interval; - double pull_out_max_steer_angle; + double pull_out_velocity{0.0}; + double pull_out_lane_departure_margin{0.0}; + double pull_out_path_interval{0.0}; + double pull_out_max_steer_angle{0.0}; }; class GeometricParallelParking @@ -78,10 +74,11 @@ class GeometricParallelParking bool isParking() const; bool planPullOver( const Pose & goal_pose, const lanelet::ConstLanelets & road_lanes, - const lanelet::ConstLanelets & shoulder_lanes, const bool is_forward); + const lanelet::ConstLanelets & shoulder_lanes, const bool is_forward, + const bool left_side_parking); bool planPullOut( const Pose & start_pose, const Pose & goal_pose, const lanelet::ConstLanelets & road_lanes, - const lanelet::ConstLanelets & shoulder_lanes); + const lanelet::ConstLanelets & shoulder_lanes, const bool left_side_start); void setParameters(const ParallelParkingParameters & parameters) { parameters_ = parameters; } void setPlannerData(const std::shared_ptr & planner_data) { @@ -94,6 +91,10 @@ class GeometricParallelParking std::vector getArcPaths() const { return arc_paths_; } std::vector getPaths() const { return paths_; } + std::vector> getPairsTerminalVelocityAndAccel() const + { + return pairs_terminal_velocity_and_accel_; + } PathWithLaneId getPathByIdx(size_t const idx) const; PathWithLaneId getCurrentPath() const; PathWithLaneId getFullPath() const; @@ -104,22 +105,23 @@ class GeometricParallelParking Pose getArcEndPose() const { return arc_end_pose_; } private: - std::shared_ptr planner_data_; - ParallelParkingParameters parameters_; + std::shared_ptr planner_data_{nullptr}; + ParallelParkingParameters parameters_{}; - double R_E_min_; // base_link - double R_Bl_min_; // front_left + double R_E_min_{0.0}; // base_link + double R_Bl_min_{0.0}; // front_left std::vector arc_paths_; std::vector paths_; + std::vector> pairs_terminal_velocity_and_accel_; size_t current_path_idx_ = 0; void clearPaths(); std::vector planOneTrial( - const Pose & start_pose, const Pose & goal_pose, const double R_E_r, + const Pose & start_pose, const Pose & goal_pose, const double R_E_far, const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes, - bool is_forward, const double end_pose_offset, const double lane_departure_margin, - const double arc_path_interval); + const bool is_forward, const bool left_side_parking, const double end_pose_offset, + const double lane_departure_margin, const double arc_path_interval); PathWithLaneId generateArcPath( const Pose & center, const double radius, const double start_yaw, double end_yaw, const double arc_path_interval, const bool is_left_turn, const bool is_forward); @@ -128,21 +130,23 @@ class GeometricParallelParking const bool is_forward); boost::optional calcStartPose( const Pose & goal_pose, const lanelet::ConstLanelets & road_lanes, - const double start_pose_offset, const double R_E_r, const bool is_forward); + const double start_pose_offset, const double R_E_far, const bool is_forward, + const bool left_side_parking); std::vector generatePullOverPaths( - const Pose & start_pose, const Pose & goal_pose, const double R_E_r, + const Pose & start_pose, const Pose & goal_pose, const double R_E_far, const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes, - const bool is_forward, const double end_pose_offset, const double velocity); + const bool is_forward, const bool left_side_parking, const double end_pose_offset, + const double velocity); PathWithLaneId generateStraightPath( const Pose & start_pose, const lanelet::ConstLanelets & road_lanes); void setVelocityToArcPaths( std::vector & arc_paths, const double velocity, const bool set_stop_end); // debug - Pose Cr_; - Pose Cl_; - Pose start_pose_; - Pose arc_end_pose_; + Pose Cr_{}; + Pose Cl_{}; + Pose start_pose_{}; + Pose arc_end_pose_{}; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/default_fixed_goal_planner.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/default_fixed_goal_planner.hpp index 40c339e507683..9cede67fff6e5 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/default_fixed_goal_planner.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/default_fixed_goal_planner.hpp @@ -20,7 +20,6 @@ #include #include -#include namespace behavior_path_planner { diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/fixed_goal_planner_base.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/fixed_goal_planner_base.hpp index 3bc0e960fe468..f587051442dcd 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/fixed_goal_planner_base.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/fixed_goal_planner_base.hpp @@ -16,13 +16,11 @@ #define BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__FIXED_GOAL_PLANNER_BASE_HPP_ #include "behavior_path_planner/data_manager.hpp" -#include "behavior_path_planner/parameters.hpp" #include #include #include -#include using autoware_auto_planning_msgs::msg::PathWithLaneId; using geometry_msgs::msg::Pose; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp index f87944641f59b..2b189d583b924 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp @@ -17,6 +17,7 @@ #define BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__GOAL_PLANNER_PARAMETERS_HPP_ #include "behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp" +#include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include #include @@ -40,67 +41,86 @@ enum class ParkingPolicy { struct GoalPlannerParameters { // general params - double minimum_request_length; - double th_arrived_distance; - double th_stopped_velocity; - double th_stopped_time; - double th_blinker_on_lateral_offset; + double th_arrived_distance{0.0}; + double th_stopped_velocity{0.0}; + double th_stopped_time{0.0}; + double th_blinker_on_lateral_offset{0.0}; + double center_line_path_interval{0.0}; // goal search - std::string search_priority; // "efficient_path" or "close_goal" + std::string goal_priority; // "minimum_weighted_distance" or "minimum_longitudinal_distance" + double minimum_weighted_distance_lateral_weight{0.0}; + bool prioritize_goals_before_objects{false}; ParkingPolicy parking_policy; // "left_side" or "right_side" - double forward_goal_search_length; - double backward_goal_search_length; - double goal_search_interval; - double longitudinal_margin; - double max_lateral_offset; - double lateral_offset_interval; - double ignore_distance_from_lane_start; - double margin_from_boundary; + double forward_goal_search_length{0.0}; + double backward_goal_search_length{0.0}; + double goal_search_interval{0.0}; + double longitudinal_margin{0.0}; + double max_lateral_offset{0.0}; + double lateral_offset_interval{0.0}; + double ignore_distance_from_lane_start{0.0}; + double margin_from_boundary{0.0}; // occupancy grid map - bool use_occupancy_grid; - bool use_occupancy_grid_for_longitudinal_margin; - double occupancy_grid_collision_check_margin; - int theta_size; - int obstacle_threshold; + bool use_occupancy_grid_for_goal_search{false}; + bool use_occupancy_grid_for_goal_longitudinal_margin{false}; + bool use_occupancy_grid_for_path_collision_check{false}; + double occupancy_grid_collision_check_margin{0.0}; + int theta_size{0}; + int obstacle_threshold{0}; // object recognition - bool use_object_recognition; - double object_recognition_collision_check_margin; - double object_recognition_collision_check_max_extra_stopping_margin; + bool use_object_recognition{false}; + double object_recognition_collision_check_margin{0.0}; + double object_recognition_collision_check_max_extra_stopping_margin{0.0}; + double th_moving_object_velocity{0.0}; // pull over general params - double pull_over_velocity; - double pull_over_minimum_velocity; - double decide_path_distance; - double maximum_deceleration; - double maximum_jerk; + double pull_over_minimum_request_length{0.0}; + double pull_over_velocity{0.0}; + double pull_over_minimum_velocity{0.0}; + double decide_path_distance{0.0}; + double maximum_deceleration{0.0}; + double maximum_jerk{0.0}; + std::string path_priority; // "efficient_path" or "close_goal" + std::vector efficient_path_order{}; // shift path - bool enable_shift_parking; - int shift_sampling_num; - double maximum_lateral_jerk; - double minimum_lateral_jerk; - double deceleration_interval; - double after_shift_straight_distance; + bool enable_shift_parking{false}; + int shift_sampling_num{0}; + double maximum_lateral_jerk{0.0}; + double minimum_lateral_jerk{0.0}; + double deceleration_interval{0.0}; + double after_shift_straight_distance{0.0}; // parallel parking - bool enable_arc_forward_parking; - bool enable_arc_backward_parking; + bool enable_arc_forward_parking{false}; + bool enable_arc_backward_parking{false}; ParallelParkingParameters parallel_parking_parameters; // freespace parking - bool enable_freespace_parking; + bool enable_freespace_parking{false}; std::string freespace_parking_algorithm; - double freespace_parking_velocity; - double vehicle_shape_margin; - PlannerCommonParam freespace_parking_common_parameters; - AstarParam astar_parameters; - RRTStarParam rrt_star_parameters; + double freespace_parking_velocity{0.0}; + double vehicle_shape_margin{0.0}; + PlannerCommonParam freespace_parking_common_parameters{}; + AstarParam astar_parameters{}; + RRTStarParam rrt_star_parameters{}; + + // stop condition + double maximum_deceleration_for_stop{0.0}; + double maximum_jerk_for_stop{0.0}; + + // hysteresis parameter + double hysteresis_factor_expand_rate{0.0}; + + // path safety checker + utils::path_safety_checker::EgoPredictedPathParams ego_predicted_path_params{}; + utils::path_safety_checker::ObjectsFilteringParams objects_filtering_params{}; + utils::path_safety_checker::SafetyCheckParams safety_check_params{}; // debug - bool print_debug_info; + bool print_debug_info{false}; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_searcher.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_searcher.hpp index d564dcaa19e29..2ed58b9678e70 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_searcher.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_searcher.hpp @@ -18,14 +18,11 @@ #include "behavior_path_planner/utils/goal_planner/goal_searcher_base.hpp" #include "behavior_path_planner/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp" -#include "autoware_auto_planning_msgs/msg/path_point_with_lane_id.hpp" - #include #include namespace behavior_path_planner { -using autoware_auto_planning_msgs::msg::PathPointWithLaneId; using tier4_autoware_utils::LinearRing2d; using BasicPolygons2d = std::vector; @@ -36,14 +33,16 @@ class GoalSearcher : public GoalSearcherBase const GoalPlannerParameters & parameters, const LinearRing2d & vehicle_footprint, const std::shared_ptr & occupancy_grid_map); - GoalCandidates search(const Pose & original_goal_pose) override; + GoalCandidates search() override; void update(GoalCandidates & goal_candidates) const override; private: + void countObjectsToAvoid( + GoalCandidates & goal_candidates, const PredictedObjects & objects) const; void createAreaPolygons(std::vector original_search_poses); - bool checkCollision(const Pose & pose) const; + bool checkCollision(const Pose & pose, const PredictedObjects & objects) const; bool checkCollisionWithLongitudinalDistance( - const Pose & ego_pose, const PredictedObjects & dynamic_objects) const; + const Pose & ego_pose, const PredictedObjects & objects) const; BasicPolygons2d getNoParkingAreaPolygons(const lanelet::ConstLanelets & lanes) const; BasicPolygons2d getNoStoppingAreaPolygons(const lanelet::ConstLanelets & lanes) const; bool isInAreas(const LinearRing2d & footprint, const BasicPolygons2d & areas) const; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_searcher_base.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_searcher_base.hpp index bab13a287960a..ab319111f6da6 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_searcher_base.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_searcher_base.hpp @@ -16,12 +16,10 @@ #define BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__GOAL_SEARCHER_BASE_HPP_ #include "behavior_path_planner/data_manager.hpp" -#include "behavior_path_planner/parameters.hpp" #include "behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp" #include -#include #include #include @@ -37,17 +35,7 @@ struct GoalCandidate double lateral_offset{0.0}; size_t id{0}; bool is_safe{true}; - - bool operator<(const GoalCandidate & other) const noexcept - { - const double diff = distance_from_original_goal - other.distance_from_original_goal; - constexpr double eps = 0.01; - if (std::abs(diff) < eps) { - return lateral_offset < other.lateral_offset; - } - - return distance_from_original_goal < other.distance_from_original_goal; - } + size_t num_objects_to_avoid{0}; }; using GoalCandidates = std::vector; @@ -57,19 +45,25 @@ class GoalSearcherBase explicit GoalSearcherBase(const GoalPlannerParameters & parameters) { parameters_ = parameters; } virtual ~GoalSearcherBase() = default; + void setReferenceGoal(const Pose & reference_goal_pose) + { + reference_goal_pose_ = reference_goal_pose; + } + void setPlannerData(const std::shared_ptr & planner_data) { planner_data_ = planner_data; } MultiPolygon2d getAreaPolygons() { return area_polygons_; } - virtual GoalCandidates search(const Pose & original_goal_pose) = 0; + virtual GoalCandidates search() = 0; virtual void update([[maybe_unused]] GoalCandidates & goal_candidates) const { return; } protected: - GoalPlannerParameters parameters_; - std::shared_ptr planner_data_; - MultiPolygon2d area_polygons_; + GoalPlannerParameters parameters_{}; + std::shared_ptr planner_data_{nullptr}; + Pose reference_goal_pose_{}; + MultiPolygon2d area_polygons_{}; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/pull_over_planner_base.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/pull_over_planner_base.hpp index 4b800917d4aec..54bba6aee2fc2 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/pull_over_planner_base.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/pull_over_planner_base.hpp @@ -16,7 +16,6 @@ #define BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__PULL_OVER_PLANNER_BASE_HPP_ #include "behavior_path_planner/data_manager.hpp" -#include "behavior_path_planner/parameters.hpp" #include "behavior_path_planner/utils/create_vehicle_footprint.hpp" #include "behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp" @@ -26,6 +25,7 @@ #include #include +#include #include using autoware_auto_planning_msgs::msg::PathWithLaneId; @@ -46,6 +46,8 @@ struct PullOverPath { PullOverPlannerType type{PullOverPlannerType::NONE}; std::vector partial_paths{}; + // accelerate with constant acceleration to the target velocity + std::vector> pairs_terminal_velocity_and_accel{}; Pose start_pose{}; Pose end_pose{}; std::vector debug_poses{}; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/shift_pull_over.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/shift_pull_over.hpp index 445161f631bb7..dccaed3184e77 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/shift_pull_over.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/shift_pull_over.hpp @@ -57,7 +57,6 @@ class ShiftPullOver : public PullOverPlannerBase LaneDepartureChecker lane_departure_checker_{}; std::shared_ptr occupancy_grid_map_{}; - static constexpr double resample_interval_{1.0}; bool left_side_parking_{true}; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/util.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/util.hpp index 3dde96e813aad..062a84bcd5aef 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/util.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/util.hpp @@ -26,7 +26,7 @@ #include #include -#include +#include #include #include @@ -44,17 +44,15 @@ using geometry_msgs::msg::Twist; using visualization_msgs::msg::Marker; using visualization_msgs::msg::MarkerArray; -// TODO(sugahara) move to util -PathWithLaneId combineReferencePath(const PathWithLaneId & path1, const PathWithLaneId & path2); -lanelet::ConstLanelets getPullOverLanes(const RouteHandler & route_handler, const bool left_side); +lanelet::ConstLanelets getPullOverLanes( + const RouteHandler & route_handler, const bool left_side, const double backward_distance, + const double forward_distance); PredictedObjects filterObjectsByLateralDistance( const Pose & ego_pose, const double vehicle_width, const PredictedObjects & objects, const double distance_thresh, const bool filter_inside); -bool isAllowedGoalModification( - const std::shared_ptr & route_handler, const bool left_side_parking); -bool checkOriginalGoalIsInShoulder( - const std::shared_ptr & route_handler, const bool left_side_parking); +bool isAllowedGoalModification(const std::shared_ptr & route_handler); +bool checkOriginalGoalIsInShoulder(const std::shared_ptr & route_handler); // debug MarkerArray createPullOverAreaMarkerArray( @@ -65,7 +63,10 @@ MarkerArray createPosesMarkerArray( MarkerArray createTextsMarkerArray( const std::vector & poses, std::string && ns, const std_msgs::msg::ColorRGBA & color); MarkerArray createGoalCandidatesMarkerArray( - GoalCandidates & goal_candidates, const std_msgs::msg::ColorRGBA & color); + const GoalCandidates & goal_candidates, const std_msgs::msg::ColorRGBA & color); +MarkerArray createNumObjectsToAvoidTextsMarkerArray( + const GoalCandidates & goal_candidates, std::string && ns, + const std_msgs::msg::ColorRGBA & color); } // namespace goal_planner_utils } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp index 0556a780467c0..c664ae3e15aef 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp @@ -16,15 +16,9 @@ #include "behavior_path_planner/utils/avoidance/avoidance_module_data.hpp" #include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" -#include "behavior_path_planner/utils/path_safety_checker/safety_check.hpp" -#include "lanelet2_core/geometry/Lanelet.h" -#include "autoware_auto_planning_msgs/msg/path_point_with_lane_id.hpp" +#include -#include -#include -#include -#include #include namespace behavior_path_planner @@ -65,23 +59,28 @@ struct LaneChangeParameters bool check_objects_on_current_lanes{true}; bool check_objects_on_other_lanes{true}; bool use_all_predicted_path{false}; + double lane_expansion_left_offset{0.0}; + double lane_expansion_right_offset{0.0}; - // true by default - bool check_car{true}; // check object car - bool check_truck{true}; // check object truck - bool check_bus{true}; // check object bus - bool check_trailer{true}; // check object trailer - bool check_unknown{true}; // check object unknown - bool check_bicycle{true}; // check object bicycle - bool check_motorcycle{true}; // check object motorbike - bool check_pedestrian{true}; // check object pedestrian + // regulatory elements + bool regulate_on_crosswalk{false}; + bool regulate_on_intersection{false}; + + // ego vehicle stuck detection + double stop_velocity_threshold{0.1}; + double stop_time_threshold{3.0}; + + // true by default for all objects + utils::path_safety_checker::ObjectTypesToCheck object_types_to_check; // safety check - utils::path_safety_checker::RSSparams rss_params; - utils::path_safety_checker::RSSparams rss_params_for_abort; + bool allow_loose_check_for_cancel{true}; + utils::path_safety_checker::RSSparams rss_params{}; + utils::path_safety_checker::RSSparams rss_params_for_abort{}; + utils::path_safety_checker::RSSparams rss_params_for_stuck{}; // abort - LaneChangeCancelParameters cancel; + LaneChangeCancelParameters cancel{}; double finish_judge_lateral_threshold{0.2}; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp index f59e0170bdf14..ac712fa2f0fa5 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp @@ -15,12 +15,10 @@ #ifndef BEHAVIOR_PATH_PLANNER__UTILS__LANE_CHANGE__UTILS_HPP_ #define BEHAVIOR_PATH_PLANNER__UTILS__LANE_CHANGE__UTILS_HPP_ -#include "behavior_path_planner/marker_utils/lane_change/debug.hpp" #include "behavior_path_planner/parameters.hpp" #include "behavior_path_planner/utils/lane_change/lane_change_module_data.hpp" #include "behavior_path_planner/utils/lane_change/lane_change_path.hpp" #include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" -#include "behavior_path_planner/utils/path_safety_checker/safety_check.hpp" #include "behavior_path_planner/utils/utils.hpp" #include @@ -30,12 +28,9 @@ #include #include -#include +#include -#include #include -#include -#include #include namespace behavior_path_planner::utils::lane_change @@ -52,6 +47,7 @@ using data::lane_change::PathSafetyStatus; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; +using path_safety_checker::CollisionCheckDebugMap; using route_handler::Direction; using tier4_autoware_utils::Polygon2d; @@ -88,8 +84,6 @@ std::vector> getSortedLaneIds( const RouteHandler & route_handler, const Pose & current_pose, const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes); -PathWithLaneId combineReferencePath(const PathWithLaneId & path1, const PathWithLaneId & path2); - lanelet::ConstLanelets getTargetPreferredLanes( const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, const Direction & direction, @@ -142,8 +136,6 @@ lanelet::ConstLanelets getBackwardLanelets( const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes, const Pose & current_pose, const double backward_length); -bool isTargetObjectType(const PredictedObject & object, const LaneChangeParameters & parameters); - double calcLateralBufferForFiltering(const double vehicle_width, const double lateral_buffer = 0.0); double calcLateralBufferForFiltering(const double vehicle_width, const double lateral_buffer); @@ -160,9 +152,6 @@ std::vector convertToPredictedPath( const LaneChangePath & lane_change_path, const Twist & vehicle_twist, const Pose & pose, const BehaviorPathPlannerParameters & common_parameters, const double resolution); -PredictedPath convertToPredictedPath( - const std::vector & path, const double time_resolution); - bool isParkedObject( const PathWithLaneId & path, const RouteHandler & route_handler, const ExtendedPredictedObject & object, const double object_check_min_road_shoulder_width, @@ -177,7 +166,8 @@ bool isParkedObject( bool passParkedObject( const RouteHandler & route_handler, const LaneChangePath & lane_change_path, const std::vector & objects, const double minimum_lane_change_length, - const bool is_goal_in_route, const LaneChangeParameters & lane_change_parameters); + const bool is_goal_in_route, const LaneChangeParameters & lane_change_parameters, + CollisionCheckDebugMap & object_debug); boost::optional getLeadingStaticObjectIdx( const RouteHandler & route_handler, const LaneChangePath & lane_change_path, @@ -190,5 +180,27 @@ std::optional createPolygon( ExtendedPredictedObject transform( const PredictedObject & object, const BehaviorPathPlannerParameters & common_parameters, const LaneChangeParameters & lane_change_parameters); + +bool isCollidedPolygonsInLanelet( + const std::vector & collided_polygons, const lanelet::ConstLanelets & lanes); + +/** + * @brief Generates expanded lanelets based on the given direction and offsets. + * + * Expands the provided lanelets in either the left or right direction based on + * the specified direction. If the direction is 'LEFT', the lanelets are expanded + * using the left_offset; if 'RIGHT', they are expanded using the right_offset. + * Otherwise, no expansion occurs. + * + * @param lanes The lanelets to be expanded. + * @param direction The direction of expansion: either LEFT or RIGHT. + * @param left_offset The offset value for left expansion. + * @param right_offset The offset value for right expansion. + * @return lanelet::ConstLanelets A collection of expanded lanelets. + */ +lanelet::ConstLanelets generateExpandedLanelets( + const lanelet::ConstLanelets & lanes, const Direction direction, const double left_offset, + const double right_offset); + } // namespace behavior_path_planner::utils::lane_change #endif // BEHAVIOR_PATH_PLANNER__UTILS__LANE_CHANGE__UTILS_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_following/module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_following/module_data.hpp index 118fb39606012..98d52b79e2edf 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_following/module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_following/module_data.hpp @@ -15,19 +15,16 @@ #ifndef BEHAVIOR_PATH_PLANNER__UTILS__LANE_FOLLOWING__MODULE_DATA_HPP_ #define BEHAVIOR_PATH_PLANNER__UTILS__LANE_FOLLOWING__MODULE_DATA_HPP_ -#include -#include - namespace behavior_path_planner { struct LaneFollowingParameters { - double lane_change_prepare_duration; + double lane_change_prepare_duration{0.0}; // finding closest lanelet - double distance_threshold; - double yaw_threshold; + double distance_threshold{0.0}; + double yaw_threshold{0.0}; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp index b4575eb4d3b7e..67588ed0a67b6 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp @@ -27,7 +27,6 @@ #include #include -#include #include #include @@ -38,6 +37,24 @@ using autoware_auto_perception_msgs::msg::PredictedObject; using autoware_auto_perception_msgs::msg::PredictedObjects; using autoware_auto_planning_msgs::msg::PathPointWithLaneId; +/** + * @brief Filters objects based on object centroid position. + * + * @param objects The predicted objects to filter. + * @param lanelet + * @return result. + */ +bool isCentroidWithinLanelet(const PredictedObject & object, const lanelet::ConstLanelet & lanelet); + +/** + * @brief Filters objects based on object polygon overlapping with lanelet. + * + * @param objects The predicted objects to filter. + * @param lanelet + * @return result. + */ +bool isPolygonOverlapLanelet(const PredictedObject & object, const lanelet::ConstLanelet & lanelet); + /** * @brief Filters objects based on various criteria. * @@ -55,24 +72,37 @@ PredictedObjects filterObjects( const std::shared_ptr & params); /** - * @brief Filter objects based on their velocity. + * @brief Filters objects based on their velocity. * - * @param objects The predicted objects to filter. - * @param lim_v Velocity limit for filtering. - * @return PredictedObjects The filtered objects. + * Depending on the remove_above_threshold parameter, this function either removes objects with + * velocities above the given threshold or only keeps those objects. It uses the helper function + * filterObjectsByVelocity() to do the actual filtering. + * + * @param objects The objects to be filtered. + * @param velocity_threshold The velocity threshold for the filtering. + * @param remove_above_threshold If true, objects with velocities above the threshold are removed. + * If false, only objects with velocities above the threshold are + * kept. + * @return A new collection of objects that have been filtered according to the rules. */ -PredictedObjects filterObjectsByVelocity(const PredictedObjects & objects, double lim_v); +PredictedObjects filterObjectsByVelocity( + const PredictedObjects & objects, const double velocity_threshold, + const bool remove_above_threshold = true); /** - * @brief Filter objects based on a velocity range. + * @brief Helper function to filter objects based on their velocity. * - * @param objects The predicted objects to filter. - * @param min_v Minimum velocity for filtering. - * @param max_v Maximum velocity for filtering. - * @return PredictedObjects The filtered objects. + * This function iterates over all objects and calculates their velocity norm. If the velocity norm + * is within the velocity_threshold and max_velocity range, the object is added to a new collection. + * This new collection is then returned. + * + * @param objects The objects to be filtered. + * @param velocity_threshold The minimum velocity for an object to be included in the output. + * @param max_velocity The maximum velocity for an object to be included in the output. + * @return A new collection of objects that have been filtered according to the rules. */ PredictedObjects filterObjectsByVelocity( - const PredictedObjects & objects, double min_v, double max_v); + const PredictedObjects & objects, double velocity_threshold, double max_velocity); /** * @brief Filter objects based on their position relative to a current_pose. @@ -87,6 +117,7 @@ void filterObjectsByPosition( PredictedObjects & objects, const std::vector & path_points, const geometry_msgs::msg::Point & current_pose, const double forward_distance, const double backward_distance); + /** * @brief Filters the provided objects based on their classification. * @@ -103,14 +134,16 @@ void filterObjectsByClass( * lanelet. */ std::pair, std::vector> separateObjectIndicesByLanelets( - const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets); + const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets, + const std::function & condition); /** * @brief Separate the objects into two part based on whether the object is within lanelet. * @return Objects pair. first objects are in the lanelet, and second others are out of lanelet. */ std::pair separateObjectsByLanelets( - const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets); + const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets, + const std::function & condition); /** * @brief Get the predicted path from an object. @@ -124,19 +157,31 @@ std::vector getPredictedPathFromObj( const ExtendedPredictedObject & obj, const bool & is_use_all_predicted_path); /** - * @brief Create a predicted path based on ego vehicle parameters. + * @brief Create a predicted path using the provided parameters. * - * @param ego_predicted_path_params Parameters for ego's predicted path. - * @param path_points Points on the path. - * @param vehicle_pose Current pose of the ego-vehicle. + * The function predicts the path based on the current vehicle pose, its current velocity, + * and certain parameters related to the vehicle's behavior and environment. The prediction + * considers acceleration, delay before departure, and maximum velocity constraints. + * + * During the delay before departure, the vehicle's velocity is assumed to be zero, and it does + * not move. After the delay, the vehicle starts to accelerate as per the provided parameters + * until it reaches the maximum allowable velocity or the specified time horizon. + * + * @param ego_predicted_path_params Parameters associated with the ego's predicted path behavior. + * @param path_points Path points to be followed by the vehicle. + * @param vehicle_pose Current pose of the vehicle. * @param current_velocity Current velocity of the vehicle. - * @param ego_seg_idx Index of the ego segment. - * @return std::vector The predicted path. + * @param ego_seg_idx Segment index where the ego vehicle is currently located on the path. + * @param is_object_front Flag indicating if there is an object in front of the ego vehicle. + * @param limit_to_max_velocity Flag indicating if the predicted path should consider the + * maximum allowable velocity. + * @return std::vector Predicted path based on the input parameters. */ std::vector createPredictedPath( const std::shared_ptr & ego_predicted_path_params, const std::vector & path_points, - const geometry_msgs::msg::Pose & vehicle_pose, const double current_velocity, size_t ego_seg_idx); + const geometry_msgs::msg::Pose & vehicle_pose, const double current_velocity, + const size_t ego_seg_idx, const bool is_object_front, const bool limit_to_max_velocity); /** * @brief Checks if the centroid of a given object is within the provided lanelets. @@ -174,6 +219,20 @@ TargetObjectsOnLane createTargetObjectsOnLane( const PredictedObjects & filtered_objects, const std::shared_ptr & params); +/** + * @brief Determines whether the predicted object type matches any of the target object types + * specified by the user. + * + * @param object The predicted object whose type is to be checked. + * @param target_object_types A structure containing boolean flags for each object type that the + * user is interested in checking. + * + * @return Returns true if the predicted object's highest probability label matches any of the + * specified target object types. + */ +bool isTargetObjectType( + const PredictedObject & object, const ObjectTypesToCheck & target_object_types); + } // namespace behavior_path_planner::utils::path_safety_checker #endif // BEHAVIOR_PATH_PLANNER__UTILS__PATH_SAFETY_CHECKER__OBJECTS_FILTERING_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp index c4a6a86861e6c..dc113b0b0be18 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp @@ -19,13 +19,18 @@ #include #include +#include +#include +#include +#include #include namespace behavior_path_planner::utils::path_safety_checker { using geometry_msgs::msg::Pose; +using geometry_msgs::msg::Twist; using tier4_autoware_utils::Polygon2d; struct PoseWithVelocity @@ -72,6 +77,7 @@ struct ExtendedPredictedObject autoware_auto_perception_msgs::msg::Shape shape; std::vector predicted_paths; }; +using ExtendedPredictedObjects = std::vector; /** * @brief Specifies which object class should be checked. @@ -123,8 +129,8 @@ struct RSSparams double longitudinal_distance_min_threshold{ 0.0}; ///< Minimum threshold for longitudinal distance. double longitudinal_velocity_delta_time{0.0}; ///< Delta time for longitudinal velocity. - double front_vehicle_deceleration; ///< brake parameter - double rear_vehicle_deceleration; ///< brake parameter + double front_vehicle_deceleration{0.0}; ///< brake parameter + double rear_vehicle_deceleration{0.0}; ///< brake parameter }; /** @@ -132,12 +138,13 @@ struct RSSparams */ struct EgoPredictedPathParams { - double acceleration; ///< Acceleration value. - double time_horizon; ///< Time horizon for prediction. - double time_resolution; ///< Time resolution for prediction. - double min_slow_speed; ///< Minimum slow speed. - double delay_until_departure; ///< Delay before departure. - double target_velocity; ///< Target velocity. + double min_velocity{0.0}; ///< Minimum velocity. + double acceleration{0.0}; ///< Acceleration value. + double max_velocity{0.0}; ///< Maximum velocity. + double time_horizon_for_front_object{0.0}; ///< Time horizon for front object. + double time_horizon_for_rear_object{0.0}; ///< Time horizon for rear object. + double time_resolution{0.0}; ///< Time resolution for prediction. + double delay_until_departure{0.0}; ///< Delay before departure. }; /** @@ -145,18 +152,18 @@ struct EgoPredictedPathParams */ struct ObjectsFilteringParams { - double safety_check_time_horizon; ///< Time horizon for object's prediction. - double safety_check_time_resolution; ///< Time resolution for object's prediction. - double object_check_forward_distance; ///< Forward distance for object checks. - double object_check_backward_distance; ///< Backward distance for object checks. - double ignore_object_velocity_threshold; ///< Velocity threshold for ignoring objects. - ObjectTypesToCheck object_types_to_check; ///< Specifies which object types to check. - ObjectLaneConfiguration object_lane_configuration; ///< Configuration for which lanes to check. - bool include_opposite_lane; ///< Include the opposite lane in checks. - bool invert_opposite_lane; ///< Invert the opposite lane in checks. - bool check_all_predicted_path; ///< Check all predicted paths. - bool use_all_predicted_path; ///< Use all predicted paths. - bool use_predicted_path_outside_lanelet; ///< Use predicted paths outside of lanelets. + double safety_check_time_horizon{0.0}; ///< Time horizon for object's prediction. + double safety_check_time_resolution{0.0}; ///< Time resolution for object's prediction. + double object_check_forward_distance{0.0}; ///< Forward distance for object checks. + double object_check_backward_distance{0.0}; ///< Backward distance for object checks. + double ignore_object_velocity_threshold{0.0}; ///< Velocity threshold for ignoring objects. + ObjectTypesToCheck object_types_to_check{}; ///< Specifies which object types to check. + ObjectLaneConfiguration object_lane_configuration{}; ///< Configuration for which lanes to check. + bool include_opposite_lane{false}; ///< Include the opposite lane in checks. + bool invert_opposite_lane{false}; ///< Invert the opposite lane in checks. + bool check_all_predicted_path{false}; ///< Check all predicted paths. + bool use_all_predicted_path{false}; ///< Use all predicted paths. + bool use_predicted_path_outside_lanelet{false}; ///< Use predicted paths outside of lanelets. }; /** @@ -164,13 +171,39 @@ struct ObjectsFilteringParams */ struct SafetyCheckParams { - bool enable_safety_check; ///< Enable safety checks. - double backward_lane_length; ///< Length of the backward lane for path generation. - double forward_path_length; ///< Length of the forward path lane for path generation. - RSSparams rss_params; ///< Parameters related to the RSS model. + bool enable_safety_check{false}; ///< Enable safety checks. + double hysteresis_factor_expand_rate{ + 0.0}; ///< Hysteresis factor to expand/shrink polygon with the value. + double backward_path_length{0.0}; ///< Length of the backward lane for path generation. + double forward_path_length{0.0}; ///< Length of the forward path lane for path generation. + RSSparams rss_params{}; ///< Parameters related to the RSS model. bool publish_debug_marker{false}; ///< Option to publish debug markers. }; +struct CollisionCheckDebug +{ + std::string unsafe_reason; ///< Reason indicating unsafe situation. + Twist current_twist{}; ///< Ego vehicle's current velocity and rotation. + Pose expected_ego_pose{}; ///< Predicted future pose of ego vehicle. + Pose current_obj_pose{}; ///< Detected object's current pose. + Twist object_twist{}; ///< Detected object's velocity and rotation. + Pose expected_obj_pose{}; ///< Predicted future pose of object. + double rss_longitudinal{0.0}; ///< Longitudinal RSS measure. + double inter_vehicle_distance{0.0}; ///< Distance between ego vehicle and object. + double forward_lon_offset{0.0}; ///< Forward longitudinal offset for extended polygon. + double backward_lon_offset{0.0}; ///< Backward longitudinal offset for extended polygon. + double lat_offset{0.0}; ///< Lateral offset for extended polygon. + bool is_front{false}; ///< True if object is in front of ego vehicle. + bool is_safe{false}; ///< True if situation is deemed safe. + std::vector ego_predicted_path; ///< ego vehicle's predicted path. + std::vector obj_predicted_path; ///< object's predicted path. + Polygon2d extended_ego_polygon{}; ///< Ego vehicle's extended collision polygon. + Polygon2d extended_obj_polygon{}; ///< Detected object's extended collision polygon. +}; +using CollisionCheckDebugPair = std::pair; +using CollisionCheckDebugMap = + std::unordered_map; + } // namespace behavior_path_planner::utils::path_safety_checker #endif // BEHAVIOR_PATH_PLANNER__UTILS__PATH_SAFETY_CHECKER__PATH_SAFETY_CHECKER_PARAMETERS_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp index 6d9df7677ead5..b8bd1629d5f3f 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp @@ -16,10 +16,10 @@ #define BEHAVIOR_PATH_PLANNER__UTILS__PATH_SAFETY_CHECKER__SAFETY_CHECK_HPP_ #include "behavior_path_planner/data_manager.hpp" -#include "behavior_path_planner/marker_utils/utils.hpp" #include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include +#include #include #include @@ -34,7 +34,6 @@ #include #endif -#include #include namespace behavior_path_planner::utils::path_safety_checker @@ -43,25 +42,34 @@ namespace behavior_path_planner::utils::path_safety_checker using autoware_auto_perception_msgs::msg::PredictedObject; using autoware_auto_perception_msgs::msg::PredictedPath; using autoware_auto_perception_msgs::msg::Shape; +using behavior_path_planner::utils::path_safety_checker::CollisionCheckDebug; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; -using marker_utils::CollisionCheckDebug; +using tier4_autoware_utils::calcYawDeviation; using tier4_autoware_utils::Point2d; using tier4_autoware_utils::Polygon2d; using vehicle_info_util::VehicleInfo; -namespace bg = boost::geometry; +bool isTargetObjectOncoming( + const geometry_msgs::msg::Pose & vehicle_pose, const geometry_msgs::msg::Pose & object_pose); +bool isTargetObjectFront( + const geometry_msgs::msg::Pose & ego_pose, const Polygon2d & obj_polygon, + const vehicle_info_util::VehicleInfo & vehicle_info); bool isTargetObjectFront( const PathWithLaneId & path, const geometry_msgs::msg::Pose & ego_pose, const vehicle_info_util::VehicleInfo & vehicle_info, const Polygon2d & obj_polygon); Polygon2d createExtendedPolygon( const Pose & base_link_pose, const vehicle_info_util::VehicleInfo & vehicle_info, - const double lon_length, const double lat_margin, CollisionCheckDebug & debug); + const double lon_length, const double lat_margin, const double is_stopped_obj, + CollisionCheckDebug & debug); Polygon2d createExtendedPolygon( const Pose & obj_pose, const Shape & shape, const double lon_length, const double lat_margin, - CollisionCheckDebug & debug); + const double is_stopped_obj, CollisionCheckDebug & debug); + +PredictedPath convertToPredictedPath( + const std::vector & path, const double time_resolution); double calcRssDistance( const double front_object_velocity, const double rear_object_velocity, @@ -69,7 +77,7 @@ double calcRssDistance( double calcMinimumLongitudinalLength( const double front_object_velocity, const double rear_object_velocity, - const BehaviorPathPlannerParameters & params); + const RSSparams & rss_params); boost::optional calcInterpolatedPoseWithVelocity( const std::vector & path, const double relative_time); @@ -77,7 +85,6 @@ boost::optional calcInterpolatedPoseWithVelocity( boost::optional getInterpolatedPoseWithVelocityAndPolygonStamped( const std::vector & pred_path, const double current_time, const VehicleInfo & ego_info); - /** * @brief Iterate the points in the ego and target's predicted path and * perform safety check for each of the iterated points. @@ -98,17 +105,33 @@ bool checkCollision( const ExtendedPredictedObject & target_object, const PredictedPathWithPolygon & target_object_path, const BehaviorPathPlannerParameters & common_parameters, const RSSparams & rss_parameters, - CollisionCheckDebug & debug); + const double hysteresis_factor, CollisionCheckDebug & debug); /** - * @brief Check collision between ego path footprints with extra longitudinal stopping margin and - * objects. - * @return Has collision or not + * @brief Iterate the points in the ego and target's predicted path and + * perform safety check for each of the iterated points. + * @param planned_path The predicted path of the ego vehicle. + * @param predicted_ego_path Ego vehicle's predicted path + * @param ego_current_velocity Current velocity of the ego vehicle. + * @param target_object The predicted object to check collision with. + * @param target_object_path The predicted path of the target object. + * @param common_parameters The common parameters used in behavior path planner. + * @param front_object_deceleration The deceleration of the object in the front.(used in RSS) + * @param rear_object_deceleration The deceleration of the object in the rear.(used in RSS) + * @param debug The debug information for collision checking. + * @return a list of polygon when collision is expected. */ -bool checkCollisionWithExtraStoppingMargin( - const PathWithLaneId & ego_path, const PredictedObjects & dynamic_objects, - const double base_to_front, const double base_to_rear, const double width, - const double maximum_deceleration, const double margin, const double max_stopping_margin); +std::vector getCollidedPolygons( + const PathWithLaneId & planned_path, + const std::vector & predicted_ego_path, + const ExtendedPredictedObject & target_object, + const PredictedPathWithPolygon & target_object_path, + const BehaviorPathPlannerParameters & common_parameters, const RSSparams & rss_parameters, + const double hysteresis_factor, CollisionCheckDebug & debug); + +bool checkPolygonsIntersects( + const std::vector & polys_1, const std::vector & polys_2); + } // namespace behavior_path_planner::utils::path_safety_checker #endif // BEHAVIOR_PATH_PLANNER__UTILS__PATH_SAFETY_CHECKER__SAFETY_CHECK_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_shifter/path_shifter.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_shifter/path_shifter.hpp index 4b1115a049ab0..91ed95b14b905 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_shifter/path_shifter.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_shifter/path_shifter.hpp @@ -15,8 +15,6 @@ #ifndef BEHAVIOR_PATH_PLANNER__UTILS__PATH_SHIFTER__PATH_SHIFTER_HPP_ #define BEHAVIOR_PATH_PLANNER__UTILS__PATH_SHIFTER__PATH_SHIFTER_HPP_ -#include "behavior_path_planner/parameters.hpp" - #include #include @@ -24,7 +22,6 @@ #include -#include #include #include #include diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_utils.hpp index 191cd3e10def5..b42502d9edec8 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_utils.hpp @@ -26,7 +26,7 @@ #include #include -#include +#include #include #include @@ -102,6 +102,8 @@ PathWithLaneId calcCenterLinePath( const std::shared_ptr & planner_data, const Pose & ref_pose, const double longest_dist_to_shift_line, const std::optional & prev_module_path = std::nullopt); + +PathWithLaneId combinePath(const PathWithLaneId & path1, const PathWithLaneId & path2); } // namespace behavior_path_planner::utils #endif // BEHAVIOR_PATH_PLANNER__UTILS__PATH_UTILS_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/side_shift/util.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/side_shift/util.hpp index 65d79a2a4977b..6cdfd84d79075 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/side_shift/util.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/side_shift/util.hpp @@ -20,9 +20,6 @@ #include #include -#include -#include - namespace behavior_path_planner { using autoware_auto_planning_msgs::msg::PathWithLaneId; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_goal_planner_common/common_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_goal_planner_common/common_module_data.hpp new file mode 100644 index 0000000000000..7a973a3bd699f --- /dev/null +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_goal_planner_common/common_module_data.hpp @@ -0,0 +1,47 @@ +// Copyright 2023 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__UTILS__START_GOAL_PLANNER_COMMON__COMMON_MODULE_DATA_HPP_ +#define BEHAVIOR_PATH_PLANNER__UTILS__START_GOAL_PLANNER_COMMON__COMMON_MODULE_DATA_HPP_ + +#include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" + +#include + +#include + +#include + +namespace behavior_path_planner +{ +using autoware_auto_perception_msgs::msg::PredictedObjects; +using behavior_path_planner::utils::path_safety_checker::CollisionCheckDebugMap; +using behavior_path_planner::utils::path_safety_checker::PoseWithVelocityStamped; +using behavior_path_planner::utils::path_safety_checker::TargetObjectsOnLane; +/* + * Common data for start/goal_planner module + */ +struct StartGoalPlannerData +{ + // filtered objects + PredictedObjects filtered_objects; + TargetObjectsOnLane target_objects_on_lane; + std::vector ego_predicted_path; + // collision check debug map + CollisionCheckDebugMap collision_check; +}; + +} // namespace behavior_path_planner + +#endif // BEHAVIOR_PATH_PLANNER__UTILS__START_GOAL_PLANNER_COMMON__COMMON_MODULE_DATA_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_goal_planner_common/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_goal_planner_common/utils.hpp new file mode 100644 index 0000000000000..37206c79542ff --- /dev/null +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_goal_planner_common/utils.hpp @@ -0,0 +1,108 @@ +// Copyright 2023 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__UTILS__START_GOAL_PLANNER_COMMON__UTILS_HPP_ +#define BEHAVIOR_PATH_PLANNER__UTILS__START_GOAL_PLANNER_COMMON__UTILS_HPP_ + +#include "behavior_path_planner/data_manager.hpp" +#include "behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp" +#include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" +#include "behavior_path_planner/utils/start_goal_planner_common/common_module_data.hpp" +#include "behavior_path_planner/utils/start_planner/pull_out_path.hpp" +#include "behavior_path_planner/utils/start_planner/start_planner_parameters.hpp" + +#include + +#include +#include +#include + +namespace behavior_path_planner::utils::start_goal_planner_common +{ + +using behavior_path_planner::StartPlannerParameters; +using behavior_path_planner::utils::path_safety_checker::CollisionCheckDebugMap; +using behavior_path_planner::utils::path_safety_checker::EgoPredictedPathParams; +using behavior_path_planner::utils::path_safety_checker::ObjectsFilteringParams; +using behavior_path_planner::utils::path_safety_checker::SafetyCheckParams; + +boost::optional calcFeasibleDecelDistance( + std::shared_ptr planner_data, const double acc_lim, const double jerk_lim, + const double target_velocity); + +/** + * @brief Update path velocities based on driving direction. + * + * This function updates the longitudinal velocity of each point in the provided paths, + * based on whether the vehicle is driving forward or backward. It also sets the terminal + * velocity and acceleration for each path. + * + * @param paths A vector of paths with lane IDs to be updated. + * @param terminal_vel_acc_pairs A vector of pairs, where each pair contains the terminal + * velocity and acceleration for a corresponding path. + * @param target_velocity The target velocity for ego vehicle predicted path. + * @param acceleration The acceleration for ego vehicle predicted path. + */ +void modifyVelocityByDirection( + std::vector & paths, + std::vector> & terminal_vel_acc_pairs, const double target_velocity, + const double acceleration); + +void updateEgoPredictedPathParams( + std::shared_ptr & ego_predicted_path_params, + const std::shared_ptr & start_planner_params); + +void updateEgoPredictedPathParams( + std::shared_ptr & ego_predicted_path_params, + const std::shared_ptr & goal_planner_params); + +void updateSafetyCheckParams( + std::shared_ptr & safety_check_params, + const std::shared_ptr & start_planner_params); + +void updateSafetyCheckParams( + std::shared_ptr & safety_check_params, + const std::shared_ptr & goal_planner_params); + +void updateObjectsFilteringParams( + std::shared_ptr & objects_filtering_params, + const std::shared_ptr & start_planner_params); + +void updateObjectsFilteringParams( + std::shared_ptr & objects_filtering_params, + const std::shared_ptr & goal_planner_params); + +void updatePathProperty( + std::shared_ptr & ego_predicted_path_params, + const std::pair & pairs_terminal_velocity_and_accel); + +void initializeCollisionCheckDebugMap(CollisionCheckDebugMap & collision_check_debug_map); + +void updateSafetyCheckTargetObjectsData( + StartGoalPlannerData & data, const PredictedObjects & filtered_objects, + const TargetObjectsOnLane & target_objects_on_lane, + const std::vector & ego_predicted_path); + +std::pair getPairsTerminalVelocityAndAccel( + const std::vector> & pairs_terminal_velocity_and_accel, + const size_t current_path_idx); + +std::optional generateFeasibleStopPath( + PathWithLaneId & current_path, std::shared_ptr planner_data, + geometry_msgs::msg::Pose & stop_pose, const double maximum_deceleration, + const double maximum_jerk); + +} // namespace behavior_path_planner::utils::start_goal_planner_common + +#endif // BEHAVIOR_PATH_PLANNER__UTILS__START_GOAL_PLANNER_COMMON__UTILS_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/freespace_pull_out.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/freespace_pull_out.hpp index abdf17c9c6cfe..d78df16f89a80 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/freespace_pull_out.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/freespace_pull_out.hpp @@ -24,7 +24,6 @@ #include #include -#include namespace behavior_path_planner { @@ -41,7 +40,7 @@ class FreespacePullOut : public PullOutPlannerBase PlannerType getPlannerType() override { return PlannerType::FREESPACE; } - boost::optional plan(Pose start_pose, Pose end_pose) override; + boost::optional plan(const Pose & start_pose, const Pose & end_pose) override; protected: std::unique_ptr planner_; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/geometric_pull_out.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/geometric_pull_out.hpp index 7ace770dc7ff1..a5ff52e74038f 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/geometric_pull_out.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/geometric_pull_out.hpp @@ -29,7 +29,7 @@ class GeometricPullOut : public PullOutPlannerBase explicit GeometricPullOut(rclcpp::Node & node, const StartPlannerParameters & parameters); PlannerType getPlannerType() override { return PlannerType::GEOMETRIC; }; - boost::optional plan(Pose start_pose, Pose goal_pose) override; + boost::optional plan(const Pose & start_pose, const Pose & goal_pose) override; GeometricParallelParking planner_; ParallelParkingParameters parallel_parking_parameters_; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/pull_out_planner_base.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/pull_out_planner_base.hpp index 6e6e5111e5dd9..4c9271d57127d 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/pull_out_planner_base.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/pull_out_planner_base.hpp @@ -16,7 +16,6 @@ #define BEHAVIOR_PATH_PLANNER__UTILS__START_PLANNER__PULL_OUT_PLANNER_BASE_HPP_ #include "behavior_path_planner/data_manager.hpp" -#include "behavior_path_planner/parameters.hpp" #include "behavior_path_planner/utils/create_vehicle_footprint.hpp" #include "behavior_path_planner/utils/start_planner/pull_out_path.hpp" #include "behavior_path_planner/utils/start_planner/start_planner_parameters.hpp" @@ -27,14 +26,13 @@ #include #include -#include +namespace behavior_path_planner +{ using autoware_auto_planning_msgs::msg::PathWithLaneId; using geometry_msgs::msg::Pose; using tier4_autoware_utils::LinearRing2d; -namespace behavior_path_planner -{ enum class PlannerType { NONE = 0, SHIFT = 1, @@ -60,7 +58,7 @@ class PullOutPlannerBase } virtual PlannerType getPlannerType() = 0; - virtual boost::optional plan(Pose start_pose, Pose goal_pose) = 0; + virtual boost::optional plan(const Pose & start_pose, const Pose & goal_pose) = 0; protected: std::shared_ptr planner_data_; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/shift_pull_out.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/shift_pull_out.hpp index 2042593064c51..78a64feb2c1ec 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/shift_pull_out.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/shift_pull_out.hpp @@ -37,13 +37,11 @@ class ShiftPullOut : public PullOutPlannerBase std::shared_ptr & lane_departure_checker); PlannerType getPlannerType() override { return PlannerType::SHIFT; }; - boost::optional plan(Pose start_pose, Pose goal_pose) override; + boost::optional plan(const Pose & start_pose, const Pose & goal_pose) override; std::vector calcPullOutPaths( const RouteHandler & route_handler, const lanelet::ConstLanelets & road_lanes, - const Pose & start_pose, const Pose & goal_pose, - const BehaviorPathPlannerParameters & common_parameter, - const behavior_path_planner::StartPlannerParameters & parameter); + const Pose & start_pose, const Pose & goal_pose); double calcBeforeShiftedArcLength( const PathWithLaneId & path, const double target_after_arc_length, const double dr); diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/start_planner_parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/start_planner_parameters.hpp index 1e26bef0c85be..6978a7d494f30 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/start_planner_parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/start_planner_parameters.hpp @@ -17,6 +17,7 @@ #define BEHAVIOR_PATH_PLANNER__UTILS__START_PLANNER__START_PLANNER_PARAMETERS_HPP_ #include "behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp" +#include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include #include @@ -34,48 +35,63 @@ using freespace_planning_algorithms::RRTStarParam; struct StartPlannerParameters { - double th_arrived_distance; - double th_stopped_velocity; - double th_stopped_time; - double th_turn_signal_on_lateral_offset; - double intersection_search_length; - double length_ratio_for_turn_signal_deactivation_near_intersection; - double collision_check_margin; - double collision_check_distance_from_end; - double th_moving_object_velocity; + double th_arrived_distance{0.0}; + double th_stopped_velocity{0.0}; + double th_stopped_time{0.0}; + double th_turn_signal_on_lateral_offset{0.0}; + double th_distance_to_middle_of_the_road{0.0}; + double intersection_search_length{0.0}; + double length_ratio_for_turn_signal_deactivation_near_intersection{0.0}; + double collision_check_margin{0.0}; + double collision_check_distance_from_end{0.0}; + double th_moving_object_velocity{0.0}; + double center_line_path_interval{0.0}; + // shift pull out - bool enable_shift_pull_out; - bool check_shift_path_lane_departure; - double minimum_shift_pull_out_distance; - int lateral_acceleration_sampling_num; - double lateral_jerk; - double maximum_lateral_acc; - double minimum_lateral_acc; - double maximum_curvature; // maximum curvature considered in the path generation - double deceleration_interval; + bool enable_shift_pull_out{false}; + bool check_shift_path_lane_departure{false}; + double minimum_shift_pull_out_distance{0.0}; + int lateral_acceleration_sampling_num{0}; + double lateral_jerk{0.0}; + double maximum_lateral_acc{0.0}; + double minimum_lateral_acc{0.0}; + double maximum_curvature{0.0}; // maximum curvature considered in the path generation + double deceleration_interval{0.0}; // geometric pull out - bool enable_geometric_pull_out; - bool divide_pull_out_path; - ParallelParkingParameters parallel_parking_parameters; + bool enable_geometric_pull_out{false}; + bool divide_pull_out_path{false}; + ParallelParkingParameters parallel_parking_parameters{}; // search start pose backward std::string search_priority; // "efficient_path" or "short_back_distance" - bool enable_back; - double backward_velocity; - double max_back_distance; - double backward_search_resolution; - double backward_path_update_duration; - double ignore_distance_from_lane_end; + bool enable_back{false}; + double backward_velocity{0.0}; + double max_back_distance{0.0}; + double backward_search_resolution{0.0}; + double backward_path_update_duration{0.0}; + double ignore_distance_from_lane_end{0.0}; // freespace planner - bool enable_freespace_planner; + bool enable_freespace_planner{false}; std::string freespace_planner_algorithm; - double end_pose_search_start_distance; - double end_pose_search_end_distance; - double end_pose_search_interval; - double freespace_planner_velocity; - double vehicle_shape_margin; + double end_pose_search_start_distance{0.0}; + double end_pose_search_end_distance{0.0}; + double end_pose_search_interval{0.0}; + double freespace_planner_velocity{0.0}; + double vehicle_shape_margin{0.0}; PlannerCommonParam freespace_planner_common_parameters; AstarParam astar_parameters; RRTStarParam rrt_star_parameters; + + // stop condition + double maximum_deceleration_for_stop{0.0}; + double maximum_jerk_for_stop{0.0}; + + // hysteresis parameter + double hysteresis_factor_expand_rate{0.0}; + + // path safety checker + utils::path_safety_checker::EgoPredictedPathParams ego_predicted_path_params{}; + utils::path_safety_checker::ObjectsFilteringParams objects_filtering_params{}; + utils::path_safety_checker::SafetyCheckParams safety_check_params{}; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/util.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/util.hpp index 63e374e074a5a..e7f51b2e86d5b 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/util.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/util.hpp @@ -16,6 +16,8 @@ #define BEHAVIOR_PATH_PLANNER__UTILS__START_PLANNER__UTIL_HPP_ #include "behavior_path_planner/data_manager.hpp" +#include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" +#include "behavior_path_planner/utils/path_safety_checker/safety_check.hpp" #include @@ -25,22 +27,21 @@ #include #include -#include +#include #include #include -#include namespace behavior_path_planner::start_planner_utils { using autoware_auto_perception_msgs::msg::PredictedObjects; using autoware_auto_perception_msgs::msg::PredictedPath; using autoware_auto_planning_msgs::msg::PathWithLaneId; +using behavior_path_planner::utils::path_safety_checker::EgoPredictedPathParams; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; using route_handler::RouteHandler; -PathWithLaneId combineReferencePath(const PathWithLaneId path1, const PathWithLaneId path2); PathWithLaneId getBackwardPath( const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes, const Pose & current_pose, const Pose & backed_pose, const double velocity); diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp index be227f4f7e93e..f418c9c7300f7 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp @@ -16,17 +16,11 @@ #define BEHAVIOR_PATH_PLANNER__UTILS__UTILS_HPP_ #include "behavior_path_planner/data_manager.hpp" -#include "behavior_path_planner/marker_utils/utils.hpp" -#include "behavior_path_planner/utils/lane_change/lane_change_module_data.hpp" -#include "behavior_path_planner/utils/lane_following/module_data.hpp" #include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" -#include "behavior_path_planner/utils/path_safety_checker/safety_check.hpp" -#include "behavior_path_planner/utils/start_planner/pull_out_path.hpp" -#include "motion_utils/motion_utils.hpp" -#include "object_recognition_utils/predicted_path_utils.hpp" +#include "motion_utils/trajectory/trajectory.hpp" #include -#include +#include #include #include @@ -39,8 +33,7 @@ #include #include -#include -#include +#include #include #ifdef ROS_DISTRO_GALACTIC @@ -52,8 +45,6 @@ #include #include #include -#include -#include #include namespace behavior_path_planner::utils @@ -232,6 +223,15 @@ std::vector calcBound( const bool enable_expanding_hatched_road_markings, const bool enable_expanding_intersection_areas, const bool is_left); +std::vector getBoundWithHatchedRoadMarkings( + const std::vector & original_bound, + const std::shared_ptr & route_handler); + +std::vector getBoundWithIntersectionAreas( + const std::vector & original_bound, + const std::shared_ptr & route_handler, + const std::vector & drivable_lanes, const bool is_left); + boost::optional getOverlappedLaneletId(const std::vector & lanes); std::vector cutOverlappedLanes( PathWithLaneId & path, const std::vector & lanes); @@ -323,11 +323,13 @@ PathPointWithLaneId insertStopPoint(const double length, PathWithLaneId & path); double getSignedDistanceFromBoundary( const lanelet::ConstLanelets & shoulder_lanelets, const Pose & pose, const bool left_side); std::optional getSignedDistanceFromBoundary( - const lanelet::ConstLanelets & shoulder_lanelets, const LinearRing2d & footprint, - const Pose & vehicle_pose, const bool left_side); + const lanelet::ConstLanelets & lanelets, const double vehicle_width, const double base_link2front, + const double base_link2rear, const Pose & vehicle_pose, const bool left_side); // misc +Polygon2d toPolygon2d(const lanelet::ConstLanelet & lanelet); + std::vector getTargetLaneletPolygons( const lanelet::ConstLanelets & lanelets, const Pose & pose, const double check_length, const std::string & target_type); 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 3023357bd0a96..0777822650dfb 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -15,18 +15,22 @@ #include "behavior_path_planner/behavior_path_planner_node.hpp" #include "behavior_path_planner/marker_utils/utils.hpp" +#include "behavior_path_planner/scene_module/avoidance/manager.hpp" +#include "behavior_path_planner/scene_module/dynamic_avoidance/manager.hpp" +#include "behavior_path_planner/scene_module/goal_planner/manager.hpp" #include "behavior_path_planner/scene_module/lane_change/interface.hpp" -#include "behavior_path_planner/utils/drivable_area_expansion/map_utils.hpp" +#include "behavior_path_planner/scene_module/lane_change/manager.hpp" +#include "behavior_path_planner/scene_module/side_shift/manager.hpp" +#include "behavior_path_planner/scene_module/start_planner/manager.hpp" #include "behavior_path_planner/utils/path_utils.hpp" -#include +#include #include #include #include #include -#include #include namespace @@ -66,7 +70,9 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod turn_signal_publisher_ = create_publisher("~/output/turn_indicators_cmd", 1); hazard_signal_publisher_ = create_publisher("~/output/hazard_lights_cmd", 1); - modified_goal_publisher_ = create_publisher("~/output/modified_goal", 1); + const auto durable_qos = rclcpp::QoS(1).transient_local(); + modified_goal_publisher_ = + create_publisher("~/output/modified_goal", durable_qos); stop_reason_publisher_ = create_publisher("~/output/stop_reasons", 1); reroute_availability_publisher_ = create_publisher("~/output/is_reroute_available", 1); @@ -130,45 +136,36 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod const std::lock_guard lock(mutex_manager_); // for planner_manager_ const auto & p = planner_data_->parameters; - planner_manager_ = std::make_shared(*this, p.verbose); - - const auto register_and_create_publisher = [&](const auto & manager) { - const auto & module_name = manager->name(); - planner_manager_->registerSceneModuleManager(manager); - path_candidate_publishers_.emplace( - module_name, create_publisher(path_candidate_name_space + module_name, 1)); - path_reference_publishers_.emplace( - module_name, create_publisher(path_reference_name_space + module_name, 1)); - }; + planner_manager_ = std::make_shared(*this, p.max_iteration_num, p.verbose); + + const auto register_and_create_publisher = + [&](const auto & manager, const bool create_publishers) { + const auto & module_name = manager->name(); + planner_manager_->registerSceneModuleManager(manager); + if (create_publishers) { + path_candidate_publishers_.emplace( + module_name, create_publisher(path_candidate_name_space + module_name, 1)); + path_reference_publishers_.emplace( + module_name, create_publisher(path_reference_name_space + module_name, 1)); + } + }; if (p.config_start_planner.enable_module) { auto manager = std::make_shared(this, "start_planner", p.config_start_planner); - planner_manager_->registerSceneModuleManager(manager); - path_candidate_publishers_.emplace( - "start_planner", create_publisher(path_candidate_name_space + "start_planner", 1)); - path_reference_publishers_.emplace( - "start_planner", create_publisher(path_reference_name_space + "start_planner", 1)); + register_and_create_publisher(manager, true); } if (p.config_goal_planner.enable_module) { auto manager = std::make_shared(this, "goal_planner", p.config_goal_planner); - planner_manager_->registerSceneModuleManager(manager); - path_candidate_publishers_.emplace( - "goal_planner", create_publisher(path_candidate_name_space + "goal_planner", 1)); - path_reference_publishers_.emplace( - "goal_planner", create_publisher(path_reference_name_space + "goal_planner", 1)); + register_and_create_publisher(manager, true); } if (p.config_side_shift.enable_module) { auto manager = std::make_shared(this, "side_shift", p.config_side_shift); - planner_manager_->registerSceneModuleManager(manager); - path_candidate_publishers_.emplace( - "side_shift", create_publisher(path_candidate_name_space + "side_shift", 1)); - path_reference_publishers_.emplace( - "side_shift", create_publisher(path_reference_name_space + "side_shift", 1)); + register_and_create_publisher(manager, true); } if (p.config_lane_change_left.enable_module) { @@ -176,7 +173,7 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod auto manager = std::make_shared( this, module_topic, p.config_lane_change_left, route_handler::Direction::LEFT, LaneChangeModuleType::NORMAL); - register_and_create_publisher(manager); + register_and_create_publisher(manager, true); } if (p.config_lane_change_right.enable_module) { @@ -184,7 +181,7 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod auto manager = std::make_shared( this, module_topic, p.config_lane_change_right, route_handler::Direction::RIGHT, LaneChangeModuleType::NORMAL); - register_and_create_publisher(manager); + register_and_create_publisher(manager, true); } if (p.config_ext_request_lane_change_right.enable_module) { @@ -192,7 +189,7 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod auto manager = std::make_shared( this, module_topic, p.config_ext_request_lane_change_right, route_handler::Direction::RIGHT, LaneChangeModuleType::EXTERNAL_REQUEST); - register_and_create_publisher(manager); + register_and_create_publisher(manager, true); } if (p.config_ext_request_lane_change_left.enable_module) { @@ -200,35 +197,25 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod auto manager = std::make_shared( this, module_topic, p.config_ext_request_lane_change_left, route_handler::Direction::LEFT, LaneChangeModuleType::EXTERNAL_REQUEST); - register_and_create_publisher(manager); + register_and_create_publisher(manager, true); } if (p.config_avoidance.enable_module) { auto manager = std::make_shared(this, "avoidance", p.config_avoidance); - planner_manager_->registerSceneModuleManager(manager); - path_candidate_publishers_.emplace( - "avoidance", create_publisher(path_candidate_name_space + "avoidance", 1)); - path_reference_publishers_.emplace( - "avoidance", create_publisher(path_reference_name_space + "avoidance", 1)); + register_and_create_publisher(manager, true); } if (p.config_avoidance_by_lc.enable_module) { auto manager = std::make_shared( this, "avoidance_by_lane_change", p.config_avoidance_by_lc); - planner_manager_->registerSceneModuleManager(manager); - path_candidate_publishers_.emplace( - "avoidance_by_lane_change", - create_publisher(path_candidate_name_space + "avoidance_by_lane_change", 1)); - path_reference_publishers_.emplace( - "avoidance_by_lane_change", - create_publisher(path_reference_name_space + "avoidance_by_lane_change", 1)); + register_and_create_publisher(manager, true); } if (p.config_dynamic_avoidance.enable_module) { auto manager = std::make_shared( this, "dynamic_avoidance", p.config_dynamic_avoidance); - planner_manager_->registerSceneModuleManager(manager); + register_and_create_publisher(manager, false); } } @@ -256,6 +243,8 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod timer_ = rclcpp::create_timer( this, get_clock(), period_ns, std::bind(&BehaviorPathPlannerNode::run, this)); } + + logger_configure_ = std::make_unique(this); } std::vector BehaviorPathPlannerNode::getWaitingApprovalModules() @@ -270,11 +259,24 @@ std::vector BehaviorPathPlannerNode::getWaitingApprovalModules() return waiting_approval_modules; } +std::vector BehaviorPathPlannerNode::getRunningModules() +{ + auto all_scene_module_ptr = planner_manager_->getSceneModuleStatus(); + std::vector running_modules; + for (const auto & module : all_scene_module_ptr) { + if (module->status == ModuleStatus::RUNNING) { + running_modules.push_back(module->module_name); + } + } + return running_modules; +} + BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam() { BehaviorPathPlannerParameters p{}; p.verbose = declare_parameter("verbose"); + p.max_iteration_num = declare_parameter("max_iteration_num"); const auto get_scene_module_manager_param = [&](std::string && ns) { ModuleConfigParameters config; @@ -284,6 +286,7 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam() declare_parameter(ns + "enable_simultaneous_execution_as_approved_module"); config.enable_simultaneous_execution_as_candidate_module = declare_parameter(ns + "enable_simultaneous_execution_as_candidate_module"); + config.keep_last = declare_parameter(ns + "keep_last"); config.priority = declare_parameter(ns + "priority"); config.max_module_size = declare_parameter(ns + "max_module_size"); return config; @@ -581,6 +584,7 @@ void BehaviorPathPlannerNode::run() lk_pd.unlock(); // release planner_data_ planner_manager_->print(); + planner_manager_->publishProcessingTime(); planner_manager_->publishMarker(); planner_manager_->publishVirtualWall(); lk_manager.unlock(); // release planner_manager_ @@ -1004,9 +1008,6 @@ SetParametersResult BehaviorPathPlannerNode::onSetParam( updateParam( parameters, DrivableAreaExpansionParameters::AVOID_DYN_OBJECTS_PARAM, planner_data_->drivable_area_expansion_parameters.avoid_dynamic_objects); - updateParam( - parameters, DrivableAreaExpansionParameters::EXPANSION_METHOD_PARAM, - planner_data_->drivable_area_expansion_parameters.expansion_method); updateParam( parameters, DrivableAreaExpansionParameters::AVOID_LINESTRING_TYPES_PARAM, planner_data_->drivable_area_expansion_parameters.avoid_linestring_types); @@ -1014,17 +1015,14 @@ SetParametersResult BehaviorPathPlannerNode::onSetParam( parameters, DrivableAreaExpansionParameters::AVOID_LINESTRING_DIST_PARAM, planner_data_->drivable_area_expansion_parameters.avoid_linestring_dist); updateParam( - parameters, DrivableAreaExpansionParameters::EGO_EXTRA_OFFSET_FRONT, - planner_data_->drivable_area_expansion_parameters.ego_extra_front_offset); + parameters, DrivableAreaExpansionParameters::EGO_EXTRA_FRONT_OVERHANG, + planner_data_->drivable_area_expansion_parameters.extra_front_overhang); updateParam( - parameters, DrivableAreaExpansionParameters::EGO_EXTRA_OFFSET_REAR, - planner_data_->drivable_area_expansion_parameters.ego_extra_rear_offset); + parameters, DrivableAreaExpansionParameters::EGO_EXTRA_WHEELBASE, + planner_data_->drivable_area_expansion_parameters.extra_wheelbase); updateParam( - parameters, DrivableAreaExpansionParameters::EGO_EXTRA_OFFSET_LEFT, - planner_data_->drivable_area_expansion_parameters.ego_extra_left_offset); - updateParam( - parameters, DrivableAreaExpansionParameters::EGO_EXTRA_OFFSET_RIGHT, - planner_data_->drivable_area_expansion_parameters.ego_extra_right_offset); + parameters, DrivableAreaExpansionParameters::EGO_EXTRA_WIDTH, + planner_data_->drivable_area_expansion_parameters.extra_width); updateParam( parameters, DrivableAreaExpansionParameters::DYN_OBJECTS_EXTRA_OFFSET_FRONT, planner_data_->drivable_area_expansion_parameters.dynamic_objects_extra_front_offset); @@ -1044,14 +1042,23 @@ SetParametersResult BehaviorPathPlannerNode::onSetParam( parameters, DrivableAreaExpansionParameters::MAX_PATH_ARC_LENGTH_PARAM, planner_data_->drivable_area_expansion_parameters.max_path_arc_length); updateParam( - parameters, DrivableAreaExpansionParameters::EXTRA_ARC_LENGTH_PARAM, - planner_data_->drivable_area_expansion_parameters.extra_arc_length); + parameters, DrivableAreaExpansionParameters::RESAMPLE_INTERVAL_PARAM, + planner_data_->drivable_area_expansion_parameters.resample_interval); updateParam( - parameters, DrivableAreaExpansionParameters::COMPENSATE_PARAM, - planner_data_->drivable_area_expansion_parameters.compensate_uncrossable_lines); + parameters, DrivableAreaExpansionParameters::MAX_REUSE_DEVIATION_PARAM, + planner_data_->drivable_area_expansion_parameters.max_reuse_deviation); + updateParam( + parameters, DrivableAreaExpansionParameters::SMOOTHING_CURVATURE_WINDOW_PARAM, + planner_data_->drivable_area_expansion_parameters.curvature_average_window); + updateParam( + parameters, DrivableAreaExpansionParameters::SMOOTHING_MAX_BOUND_RATE_PARAM, + planner_data_->drivable_area_expansion_parameters.max_bound_rate); + updateParam( + parameters, DrivableAreaExpansionParameters::SMOOTHING_EXTRA_ARC_LENGTH_PARAM, + planner_data_->drivable_area_expansion_parameters.extra_arc_length); updateParam( - parameters, DrivableAreaExpansionParameters::EXTRA_COMPENSATE_PARAM, - planner_data_->drivable_area_expansion_parameters.compensate_extra_dist); + parameters, DrivableAreaExpansionParameters::PRINT_RUNTIME_PARAM, + planner_data_->drivable_area_expansion_parameters.print_runtime); } catch (const rclcpp::exceptions::InvalidParameterTypeException & e) { result.successful = false; result.reason = e.what(); diff --git a/planning/behavior_path_planner/src/marker_utils/avoidance/debug.cpp b/planning/behavior_path_planner/src/marker_utils/avoidance/debug.cpp index 94073ad467326..ce9cedc816a19 100644 --- a/planning/behavior_path_planner/src/marker_utils/avoidance/debug.cpp +++ b/planning/behavior_path_planner/src/marker_utils/avoidance/debug.cpp @@ -14,11 +14,11 @@ #include "behavior_path_planner/marker_utils/avoidance/debug.hpp" -#include "behavior_path_planner/utils/path_utils.hpp" #include "behavior_path_planner/utils/utils.hpp" #include +#include #include #include @@ -27,7 +27,6 @@ namespace marker_utils::avoidance_marker { -using behavior_path_planner::AvoidLine; using tier4_autoware_utils::appendMarkerArray; using tier4_autoware_utils::calcDistance2d; using tier4_autoware_utils::calcOffsetPose; @@ -416,12 +415,6 @@ MarkerArray createOtherObjectsMarkerArray(const ObjectDataArray & objects, const return msg; } -MarkerArray createUnsafeObjectsMarkerArray(const ObjectDataArray & objects, std::string && ns) -{ - return createObjectsCubeMarkerArray( - objects, ns + "_cube", createMarkerScale(3.2, 1.7, 2.0), createMarkerColor(0.0, 0.0, 1.0, 0.8)); -} - MarkerArray makeOverhangToRoadShoulderMarkerArray( const ObjectDataArray & objects, std::string && ns) { diff --git a/planning/behavior_path_planner/src/marker_utils/lane_change/debug.cpp b/planning/behavior_path_planner/src/marker_utils/lane_change/debug.cpp index fca53930a0ec5..a7604124ebe43 100644 --- a/planning/behavior_path_planner/src/marker_utils/lane_change/debug.cpp +++ b/planning/behavior_path_planner/src/marker_utils/lane_change/debug.cpp @@ -12,6 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "behavior_path_planner/marker_utils/colors.hpp" #include "behavior_path_planner/marker_utils/utils.hpp" #include "behavior_path_planner/utils/path_shifter/path_shifter.hpp" @@ -24,8 +25,6 @@ #include #include -#include -#include #include #include @@ -33,43 +32,8 @@ namespace marker_utils::lane_change_markers { using geometry_msgs::msg::Point; using tier4_autoware_utils::createDefaultMarker; -using tier4_autoware_utils::createMarkerColor; using tier4_autoware_utils::createMarkerScale; -MarkerArray showObjectInfo( - const std::unordered_map & obj_debug_vec, std::string && ns) -{ - Marker obj_marker = createDefaultMarker( - "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, 0L, Marker::TEXT_VIEW_FACING, - createMarkerScale(0.5, 0.5, 0.5), createMarkerColor(0.0, 1.0, 1.0, 0.999)); - - MarkerArray marker_array; - int32_t id{0}; - - marker_array.markers.reserve(obj_debug_vec.size()); - - int idx{0}; - - for (const auto & [uuid, info] : obj_debug_vec) { - obj_marker.id = ++id; - obj_marker.pose = info.current_pose; - - std::ostringstream ss; - - ss << "Idx: " << ++idx << "\nReason: " << info.failed_reason - << "\nRSS dist: " << std::setprecision(4) << info.rss_longitudinal - << "\nEgo to obj: " << info.ego_to_obj_margin << "\nLateral offset: " << info.lateral_offset - << "\nLongitudinal offset: " << info.longitudinal_offset - << "\nPosition: " << (info.is_front ? "front" : "back") - << "\nSafe: " << (info.allow_lane_change ? "Yes" : "No"); - - obj_marker.text = ss.str(); - - marker_array.markers.push_back(obj_marker); - } - return marker_array; -} - MarkerArray showAllValidLaneChangePath(const std::vector & lanes, std::string && ns) { if (lanes.empty()) { @@ -80,7 +44,7 @@ MarkerArray showAllValidLaneChangePath(const std::vector & lanes int32_t id{0}; const auto current_time{rclcpp::Clock{RCL_ROS_TIME}.now()}; - constexpr auto colors = colorsList(); + const auto colors = colors::colors_list(); const auto loop_size = std::min(lanes.size(), colors.size()); marker_array.markers.reserve(loop_size); @@ -91,8 +55,7 @@ MarkerArray showAllValidLaneChangePath(const std::vector & lanes const auto & color = colors.at(idx); Marker marker = createDefaultMarker( - "map", current_time, ns, ++id, Marker::LINE_STRIP, createMarkerScale(0.1, 0.1, 0.0), - createMarkerColor(color[0], color[1], color[2], 0.9)); + "map", current_time, ns, ++id, Marker::LINE_STRIP, createMarkerScale(0.1, 0.1, 0.0), color); marker.points.reserve(lanes.at(idx).path.points.size()); for (const auto & point : lanes.at(idx).path.points) { @@ -104,165 +67,6 @@ MarkerArray showAllValidLaneChangePath(const std::vector & lanes return marker_array; } -MarkerArray showLerpedPose( - const std::unordered_map & obj_debug_vec, std::string && ns) -{ - MarkerArray marker_array; - int32_t id{0}; - const auto current_time{rclcpp::Clock{RCL_ROS_TIME}.now()}; - marker_array.markers.reserve(obj_debug_vec.size()); - - for (const auto & [uuid, info] : obj_debug_vec) { - Marker marker = createDefaultMarker( - "map", current_time, ns, ++id, Marker::POINTS, createMarkerScale(0.3, 0.3, 0.3), - createMarkerColor(1.0, 0.3, 1.0, 0.9)); - marker.points.reserve(info.lerped_path.size()); - - for (const auto & point : info.lerped_path) { - marker.points.push_back(point.position); - } - - marker_array.markers.push_back(marker); - } - return marker_array; -} - -MarkerArray showEgoPredictedPaths( - const std::unordered_map & obj_debug_vec, std::string && ns) -{ - if (obj_debug_vec.empty()) { - return MarkerArray{}; - } - - MarkerArray marker_array; - constexpr auto colors = colorsList(); - constexpr float scale_val = 0.2; - const auto current_time{rclcpp::Clock{RCL_ROS_TIME}.now()}; - marker_array.markers.reserve(obj_debug_vec.size()); - - int32_t id{0}; - for (const auto & [uuid, info] : obj_debug_vec) { - const auto loop_size = std::min(info.ego_predicted_path.size(), colors.size()); - - for (std::size_t idx = 0; idx < loop_size; ++idx) { - const auto & path = info.ego_predicted_path.at(idx).path; - const auto & color = colors.at(idx); - - Marker marker = createDefaultMarker( - "map", current_time, ns, ++id, Marker::LINE_STRIP, - createMarkerScale(scale_val, scale_val, scale_val), - createMarkerColor(color[0], color[1], color[2], 0.9)); - - marker.points.reserve(path.size()); - - for (const auto & point : path) { - marker.points.push_back(point.position); - } - - marker_array.markers.push_back(marker); - } - } - return marker_array; -} - -MarkerArray showPolygon( - const std::unordered_map & obj_debug_vec, std::string && ns) -{ - if (obj_debug_vec.empty()) { - return MarkerArray{}; - } - - constexpr float scale_val{0.2}; - int32_t id{0}; - const auto now = rclcpp::Clock{RCL_ROS_TIME}.now(); - Marker ego_marker = createDefaultMarker( - "map", now, ns, id, Marker::LINE_STRIP, createMarkerScale(scale_val, scale_val, scale_val), - createMarkerColor(1.0, 1.0, 1.0, 0.9)); - Marker obj_marker = ego_marker; - - auto text_marker = createDefaultMarker( - "map", now, ns + "_text", id, visualization_msgs::msg::Marker::TEXT_VIEW_FACING, - createMarkerScale(1.5, 1.5, 1.5), createMarkerColor(1.0, 1.0, 1.0, 1.0)); - - MarkerArray marker_array; - - constexpr auto red_color = colorsList().at(0); - constexpr auto green_color = colorsList().at(1); - const auto reserve_size = obj_debug_vec.size(); - - marker_array.markers.reserve(reserve_size * 4); - - int32_t idx = {0}; - - for (const auto & [uuid, info] : obj_debug_vec) { - const auto & color = info.allow_lane_change ? green_color : red_color; - const auto & ego_polygon = info.ego_polygon.outer(); - const auto poly_z = info.current_pose.position.z; // temporally - ego_marker.id = ++id; - ego_marker.color = createMarkerColor(color[0], color[1], color[2], 0.8); - ego_marker.points.reserve(ego_polygon.size()); - for (const auto & p : ego_polygon) { - ego_marker.points.push_back(tier4_autoware_utils::createPoint(p.x(), p.y(), poly_z)); - } - marker_array.markers.push_back(ego_marker); - - std::ostringstream ss; - text_marker.id = ego_marker.id; - ss << ++idx; - text_marker.text = ss.str(); - text_marker.pose = info.expected_ego_pose; - - marker_array.markers.push_back(text_marker); - - const auto & obj_polygon = info.obj_polygon.outer(); - obj_marker.id = ++id; - obj_marker.color = createMarkerColor(color[0], color[1], color[2], 0.8); - obj_marker.points.reserve(obj_polygon.size()); - for (const auto & p : obj_polygon) { - obj_marker.points.push_back(tier4_autoware_utils::createPoint(p.x(), p.y(), poly_z)); - } - marker_array.markers.push_back(obj_marker); - - text_marker.id = obj_marker.id; - text_marker.pose = info.expected_obj_pose; - marker_array.markers.push_back(text_marker); - - ego_marker.points.clear(); - obj_marker.points.clear(); - } - - return marker_array; -} - -MarkerArray showPolygonPose( - const std::unordered_map & obj_debug_vec, std::string && ns) -{ - constexpr auto colors = colorsList(); - const auto loop_size = std::min(colors.size(), obj_debug_vec.size()); - MarkerArray marker_array; - int32_t id{0}; - size_t idx{0}; - const auto current_time{rclcpp::Clock{RCL_ROS_TIME}.now()}; - marker_array.markers.reserve(obj_debug_vec.size()); - - for (const auto & [uuid, info] : obj_debug_vec) { - const auto & color = colors.at(idx); - Marker marker = createDefaultMarker( - "map", current_time, ns, ++id, Marker::POINTS, createMarkerScale(0.2, 0.2, 0.2), - createMarkerColor(color[0], color[1], color[2], 0.999)); - marker.points.reserve(2); - marker.points.push_back(info.expected_ego_pose.position); - marker.points.push_back(info.expected_obj_pose.position); - marker_array.markers.push_back(marker); - ++idx; - if (idx >= loop_size) { - break; - } - } - - return marker_array; -} - MarkerArray createLaneChangingVirtualWallMarker( const geometry_msgs::msg::Pose & lane_changing_pose, const std::string & module_name, const rclcpp::Time & now, const std::string & ns) @@ -273,7 +77,7 @@ MarkerArray createLaneChangingVirtualWallMarker( { auto wall_marker = createDefaultMarker( "map", now, ns + "virtual_wall", id, visualization_msgs::msg::Marker::CUBE, - createMarkerScale(0.1, 5.0, 2.0), createMarkerColor(0.0, 1.0, 0.0, 0.5)); + createMarkerScale(0.1, 5.0, 2.0), colors::green()); wall_marker.pose = lane_changing_pose; wall_marker.pose.position.z += 1.0; marker_array.markers.push_back(wall_marker); @@ -282,7 +86,7 @@ MarkerArray createLaneChangingVirtualWallMarker( { auto text_marker = createDefaultMarker( "map", now, ns + "_text", id, visualization_msgs::msg::Marker::TEXT_VIEW_FACING, - createMarkerScale(0.0, 0.0, 1.0), createMarkerColor(1.0, 1.0, 1.0, 1.0)); + createMarkerScale(0.0, 0.0, 1.0), colors::white()); text_marker.pose = lane_changing_pose; text_marker.pose.position.z += 2.0; text_marker.text = module_name; @@ -292,4 +96,28 @@ MarkerArray createLaneChangingVirtualWallMarker( return marker_array; } +MarkerArray showFilteredObjects( + const ExtendedPredictedObjects & current_lane_objects, + const ExtendedPredictedObjects & target_lane_objects, + const ExtendedPredictedObjects & other_lane_objects, const std::string & ns) +{ + int32_t update_id = 0; + auto current_marker = + marker_utils::showFilteredObjects(current_lane_objects, ns, colors::yellow(), update_id); + update_id += static_cast(current_marker.markers.size()); + auto target_marker = + marker_utils::showFilteredObjects(target_lane_objects, ns, colors::aqua(), update_id); + update_id += static_cast(target_marker.markers.size()); + auto other_marker = + marker_utils::showFilteredObjects(other_lane_objects, ns, colors::medium_orchid(), update_id); + + MarkerArray marker_array; + marker_array.markers.insert( + marker_array.markers.end(), current_marker.markers.begin(), current_marker.markers.end()); + marker_array.markers.insert( + marker_array.markers.end(), target_marker.markers.begin(), target_marker.markers.end()); + marker_array.markers.insert( + marker_array.markers.end(), other_marker.markers.begin(), other_marker.markers.end()); + return marker_array; +} } // namespace marker_utils::lane_change_markers diff --git a/planning/behavior_path_planner/src/marker_utils/utils.cpp b/planning/behavior_path_planner/src/marker_utils/utils.cpp index d8c87b6d1b291..031926f2d9d1d 100644 --- a/planning/behavior_path_planner/src/marker_utils/utils.cpp +++ b/planning/behavior_path_planner/src/marker_utils/utils.cpp @@ -14,15 +14,24 @@ #include "behavior_path_planner/marker_utils/utils.hpp" +#include "behavior_path_planner/marker_utils/colors.hpp" +#include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include "behavior_path_planner/utils/path_utils.hpp" -#include "behavior_path_planner/utils/utils.hpp" #include +#include + +#include +#include +#include + +#include namespace marker_utils { using behavior_path_planner::ShiftLine; using behavior_path_planner::utils::calcPathArcLengthArray; +using behavior_path_planner::utils::path_safety_checker::CollisionCheckDebug; using std_msgs::msg::ColorRGBA; using tier4_autoware_utils::calcOffsetPose; using tier4_autoware_utils::createDefaultMarker; @@ -32,6 +41,27 @@ using tier4_autoware_utils::createMarkerScale; using tier4_autoware_utils::createPoint; using visualization_msgs::msg::Marker; +CollisionCheckDebugPair createObjectDebug(const ExtendedPredictedObject & obj) +{ + CollisionCheckDebug debug; + debug.current_obj_pose = obj.initial_pose.pose; + debug.current_twist = obj.initial_twist.twist; + return {tier4_autoware_utils::toHexString(obj.uuid), debug}; +} + +void updateCollisionCheckDebugMap( + CollisionCheckDebugMap & debug_map, CollisionCheckDebugPair & object_debug, bool is_safe) +{ + auto & [key, element] = object_debug; + element.is_safe = is_safe; + if (debug_map.find(key) != debug_map.end()) { + debug_map[key] = element; + return; + } + + debug_map.insert(object_debug); +} + MarkerArray createPoseMarkerArray( const Pose & pose, std::string && ns, const int32_t & id, const float & r, const float & g, const float & b) @@ -408,4 +438,254 @@ MarkerArray createDrivableLanesMarkerArray( return msg; } +MarkerArray createPredictedPathMarkerArray( + const PredictedPath & predicted_path, const vehicle_info_util::VehicleInfo & vehicle_info, + std::string && ns, const int32_t & id, const float & r, const float & g, const float & b) +{ + if (predicted_path.path.empty()) { + return MarkerArray{}; + } + + const auto current_time = rclcpp::Clock{RCL_ROS_TIME}.now(); + MarkerArray msg; + + const auto & path = predicted_path.path; + + Marker marker = createDefaultMarker( + "map", current_time, ns, id, Marker::LINE_STRIP, createMarkerScale(0.1, 0.1, 0.1), + + createMarkerColor(r, g, b, 0.999)); + marker.lifetime = rclcpp::Duration::from_seconds(1.5); + + MarkerArray marker_array; + for (size_t i = 0; i < path.size(); ++i) { + marker.id = i + id; + marker.points.clear(); + + const auto & predicted_path_pose = path.at(i); + const double half_width = vehicle_info.vehicle_width_m / 2.0; + const double base_to_front = vehicle_info.vehicle_length_m - vehicle_info.rear_overhang_m; + const double base_to_rear = vehicle_info.rear_overhang_m; + + marker.points.push_back( + tier4_autoware_utils::calcOffsetPose(predicted_path_pose, base_to_front, -half_width, 0.0) + .position); + marker.points.push_back( + tier4_autoware_utils::calcOffsetPose(predicted_path_pose, base_to_front, half_width, 0.0) + .position); + marker.points.push_back( + tier4_autoware_utils::calcOffsetPose(predicted_path_pose, -base_to_rear, half_width, 0.0) + .position); + marker.points.push_back( + tier4_autoware_utils::calcOffsetPose(predicted_path_pose, -base_to_rear, -half_width, 0.0) + .position); + marker.points.push_back(marker.points.front()); + + marker_array.markers.push_back(marker); + } + return marker_array; + + marker.points.reserve(path.size()); + for (const auto & point : path) { + marker.points.push_back(point.position); + } + msg.markers.push_back(marker); + return msg; +} + +MarkerArray showPolygon(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns) +{ + if (obj_debug_vec.empty()) { + return MarkerArray{}; + } + + int32_t id{0}; + const auto now = rclcpp::Clock{RCL_ROS_TIME}.now(); + + constexpr float line_scale_val{0.2}; + const auto line_marker_scale = createMarkerScale(line_scale_val, line_scale_val, line_scale_val); + + auto default_line_marker = [&](const auto & color = colors::green()) { + return createDefaultMarker("map", now, ns, ++id, Marker::LINE_STRIP, line_marker_scale, color); + }; + + constexpr float text_scale_val{1.5}; + const auto text_marker_scale = createMarkerScale(text_scale_val, text_scale_val, text_scale_val); + + auto default_text_marker = [&]() { + return createDefaultMarker( + "map", now, ns + "_text", ++id, visualization_msgs::msg::Marker::TEXT_VIEW_FACING, + text_marker_scale, colors::white()); + }; + + auto default_cube_marker = + [&](const auto & width, const auto & depth, const auto & color = colors::green()) { + return createDefaultMarker( + "map", now, ns + "_cube", ++id, visualization_msgs::msg::Marker::CUBE, + createMarkerScale(width, depth, 1.0), color); + }; + + MarkerArray marker_array; + marker_array.markers.reserve( + obj_debug_vec.size() * 5); // poly ego, text ego, poly obj, text obj, cube obj + + int32_t idx = {0}; + for (const auto & [uuid, info] : obj_debug_vec) { + const auto color = info.is_safe ? colors::green() : colors::red(); + const auto poly_z = info.current_obj_pose.position.z; // temporally + + const auto insert_polygon_marker = [&](const auto & polygon) { + marker_array.markers.emplace_back(); + auto & polygon_marker = marker_array.markers.back(); + polygon_marker = default_line_marker(color); + polygon_marker.points.reserve(polygon.outer().size()); + for (const auto & p : polygon.outer()) { + polygon_marker.points.push_back(createPoint(p.x(), p.y(), poly_z)); + } + }; + + insert_polygon_marker(info.extended_ego_polygon); + insert_polygon_marker(info.extended_obj_polygon); + + const auto str_idx = std::to_string(++idx); + const auto insert_text_marker = [&](const auto & pose) { + marker_array.markers.emplace_back(); + auto & text_marker = marker_array.markers.back(); + text_marker = default_text_marker(); + text_marker.text = str_idx; + text_marker.pose = pose; + }; + + insert_text_marker(info.expected_ego_pose); + insert_text_marker(info.expected_obj_pose); + + const auto insert_cube_marker = [&](const auto & pose) { + marker_array.markers.emplace_back(); + auto & cube_marker = marker_array.markers.back(); + cube_marker = default_cube_marker(1.0, 1.0, color); + cube_marker.pose = pose; + }; + insert_cube_marker(info.current_obj_pose); + } + return marker_array; +} + +MarkerArray showPredictedPath(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns) +{ + int32_t id{0}; + const auto current_time{rclcpp::Clock{RCL_ROS_TIME}.now()}; + const auto arrow_marker_scale = createMarkerScale(1.0, 0.3, 0.3); + const auto default_arrow_marker = [&](const auto & color) { + return createDefaultMarker( + "map", current_time, ns, ++id, Marker::ARROW, arrow_marker_scale, color); + }; + + MarkerArray marker_array; + marker_array.markers.reserve(std::accumulate( + obj_debug_vec.cbegin(), obj_debug_vec.cend(), 0UL, + [&](const auto current_sum, const auto & obj_debug) { + const auto & [uuid, info] = obj_debug; + return current_sum + info.ego_predicted_path.size() + info.obj_predicted_path.size() + 2; + })); + + for (const auto & [uuid, info] : obj_debug_vec) { + const auto insert_marker = [&](const auto & path, const auto & color) { + for (const auto & pose : path) { + marker_array.markers.emplace_back(); + auto & marker = marker_array.markers.back(); + marker = default_arrow_marker(color); + marker.pose = pose.pose; + } + }; + + insert_marker(info.ego_predicted_path, colors::aqua()); + insert_marker(info.obj_predicted_path, colors::yellow()); + const auto insert_expected_pose_marker = [&](const auto & pose, const auto & color) { + // instead of checking for distance, inserting a new marker might be more efficient + marker_array.markers.emplace_back(); + auto & marker = marker_array.markers.back(); + marker = default_arrow_marker(color); + marker.pose = pose; + marker.pose.position.z += 0.05; + }; + + insert_expected_pose_marker(info.expected_ego_pose, colors::red()); + insert_expected_pose_marker(info.expected_obj_pose, colors::red()); + } + return marker_array; +} + +MarkerArray showSafetyCheckInfo(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns) +{ + int32_t id{0}; + auto default_text_marker = [&]() { + return createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, ++id, Marker::TEXT_VIEW_FACING, + createMarkerScale(0.5, 0.5, 0.5), colors::aqua()); + }; + + MarkerArray marker_array; + + marker_array.markers.reserve(obj_debug_vec.size()); + + int idx{0}; + + for (const auto & [uuid, info] : obj_debug_vec) { + auto safety_check_info_text = default_text_marker(); + safety_check_info_text.pose = info.current_obj_pose; + + std::ostringstream ss; + + ss << "Idx: " << ++idx << "\nUnsafe reason: " << info.unsafe_reason + << "\nRSS dist: " << std::setprecision(4) << info.rss_longitudinal + << "\nEgo to obj: " << info.inter_vehicle_distance + << "\nExtended polygon: " << (info.is_front ? "ego" : "object") + << "\nExtended polygon lateral offset: " << info.lat_offset + << "\nExtended polygon forward longitudinal offset: " << info.forward_lon_offset + << "\nExtended polygon backward longitudinal offset: " << info.backward_lon_offset + << "\nLast checked position: " << (info.is_front ? "obj in front ego" : "obj at back ego") + << "\nSafe: " << (info.is_safe ? "Yes" : "No"); + + safety_check_info_text.text = ss.str(); + marker_array.markers.push_back(safety_check_info_text); + } + return marker_array; +} + +MarkerArray showFilteredObjects( + const ExtendedPredictedObjects & predicted_objects, const std::string & ns, + const ColorRGBA & color, int32_t id) +{ + using behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObject; + if (predicted_objects.empty()) { + return MarkerArray{}; + } + + const auto now = rclcpp::Clock{RCL_ROS_TIME}.now(); + + auto default_cube_marker = + [&](const auto & width, const auto & depth, const auto & color = colors::green()) { + return createDefaultMarker( + "map", now, ns + "_cube", ++id, visualization_msgs::msg::Marker::CUBE, + createMarkerScale(width, depth, 1.0), color); + }; + + MarkerArray marker_array; + marker_array.markers.reserve( + predicted_objects.size()); // poly ego, text ego, poly obj, text obj, cube obj + + std::for_each( + predicted_objects.begin(), predicted_objects.end(), [&](const ExtendedPredictedObject & obj) { + const auto insert_cube_marker = [&](const auto & pose) { + marker_array.markers.emplace_back(); + auto & cube_marker = marker_array.markers.back(); + cube_marker = default_cube_marker(1.0, 1.0, color); + cube_marker.pose = pose; + }; + insert_cube_marker(obj.initial_pose.pose); + }); + + return marker_array; +} + } // namespace marker_utils diff --git a/planning/behavior_path_planner/src/planner_manager.cpp b/planning/behavior_path_planner/src/planner_manager.cpp index c09df4151ca35..79e6055d26a81 100644 --- a/planning/behavior_path_planner/src/planner_manager.cpp +++ b/planning/behavior_path_planner/src/planner_manager.cpp @@ -14,10 +14,11 @@ #include "behavior_path_planner/planner_manager.hpp" -#include "behavior_path_planner/utils/path_utils.hpp" #include "behavior_path_planner/utils/utils.hpp" +#include "tier4_autoware_utils/ros/debug_publisher.hpp" +#include "tier4_autoware_utils/system/stop_watch.hpp" -#include +#include #include #include @@ -27,12 +28,15 @@ namespace behavior_path_planner { -PlannerManager::PlannerManager(rclcpp::Node & node, const bool verbose) +PlannerManager::PlannerManager( + rclcpp::Node & node, const size_t max_iteration_num, const bool verbose) : logger_(node.get_logger().get_child("planner_manager")), clock_(*node.get_clock()), + max_iteration_num_{max_iteration_num}, verbose_{verbose} { processing_time_.emplace("total_time", 0.0); + debug_publisher_ptr_ = std::make_unique(&node, "~/debug"); } BehaviorModuleOutput PlannerManager::run(const std::shared_ptr & data) @@ -79,7 +83,7 @@ BehaviorModuleOutput PlannerManager::run(const std::shared_ptr & da return output; } - while (rclcpp::ok()) { + for (size_t itr_num = 1;; ++itr_num) { /** * STEP1: get approved modules' output */ @@ -125,8 +129,17 @@ BehaviorModuleOutput PlannerManager::run(const std::shared_ptr & da addApprovedModule(highest_priority_module); clearCandidateModules(); debug_info_.emplace_back(highest_priority_module, Action::ADD, "To Approval"); + + if (itr_num >= max_iteration_num_) { + RCLCPP_WARN_THROTTLE( + logger_, clock_, 1000, "Reach iteration limit (max: %ld). Output current result.", + max_iteration_num_); + processing_time_.at("total_time") = stop_watch_.toc("total_time", true); + return candidate_modules_output; + } } - return BehaviorModuleOutput{}; + + return BehaviorModuleOutput{}; // something wrong. }(); std::for_each( @@ -194,22 +207,6 @@ std::vector PlannerManager::getRequestModules( std::vector request_modules{}; - /** - * check whether it is possible to push back more modules to approved modules. - */ - { - const auto find_block_module = [this](const auto & m) { - return !getManager(m)->isSimultaneousExecutableAsApprovedModule(); - }; - - const auto itr = - std::find_if(approved_module_ptrs_.begin(), approved_module_ptrs_.end(), find_block_module); - - if (itr != approved_module_ptrs_.end()) { - return {}; - } - } - const auto toc = [this](const auto & name) { processing_time_.at(name) += stop_watch_.toc(name, true); }; @@ -218,14 +215,62 @@ std::vector PlannerManager::getRequestModules( stop_watch_.tic(manager_ptr->name()); /** - * don't launch candidate module if approved modules already exist. + * determine the execution capability of modules based on existing approved modules. */ - if (!approved_module_ptrs_.empty()) { - if (!manager_ptr->isSimultaneousExecutableAsApprovedModule()) { - toc(manager_ptr->name()); + // Condition 1: always executable module can be added regardless of the existence of other + // modules, so skip checking the existence of other modules. + // in other cases, need to check the existence of other modules and which module can be added. + const bool has_non_always_executable_module = std::any_of( + approved_module_ptrs_.begin(), approved_module_ptrs_.end(), + [this](const auto & m) { return !getManager(m)->isAlwaysExecutableModule(); }); + if (!manager_ptr->isAlwaysExecutableModule() && has_non_always_executable_module) { + // pairs of find_block_module and is_executable + std::vector, std::function>> + conditions; + + // Condition 2: do not add modules that are neither always nor simultaneous executable + // if there exists at least one approved module that is simultaneous but not always + // executable. (only modules that are either always executable or simultaneous executable can + // be added) + conditions.push_back( + {[&](const SceneModulePtr & m) { + return !getManager(m)->isAlwaysExecutableModule() && + getManager(m)->isSimultaneousExecutableAsApprovedModule(); + }, + [&]() { return manager_ptr->isSimultaneousExecutableAsApprovedModule(); }}); + + // Condition 3: do not add modules that are not always executable if there exists + // at least one approved module that is neither always nor simultaneous executable. + // (only modules that are always executable can be added) + conditions.push_back( + {[&](const SceneModulePtr & m) { + return !getManager(m)->isAlwaysExecutableModule() && + !getManager(m)->isSimultaneousExecutableAsApprovedModule(); + }, + [&]() { return false; }}); + + bool skip_module = false; + for (const auto & condition : conditions) { + const auto & find_block_module = condition.first; + const auto & is_executable = condition.second; + + const auto itr = std::find_if( + approved_module_ptrs_.begin(), approved_module_ptrs_.end(), find_block_module); + + if (itr != approved_module_ptrs_.end() && !is_executable()) { + toc(manager_ptr->name()); + skip_module = true; + continue; + } + } + if (skip_module) { continue; } } + // else{ + // Condition 4: if none of the above conditions are met, any module can be added. + // (when the approved modules are either empty or consist only of always executable modules.) + // } /** * launch new candidate module. @@ -309,6 +354,34 @@ std::vector PlannerManager::getRequestModules( return request_modules; } +BehaviorModuleOutput PlannerManager::getReferencePath( + const std::shared_ptr & data) const +{ + const auto & route_handler = data->route_handler; + const auto & pose = data->self_odometry->pose.pose; + const auto p = data->parameters; + + constexpr double extra_margin = 10.0; + const auto backward_length = + std::max(p.backward_path_length, p.backward_path_length + extra_margin); + + const auto lanelet_sequence = route_handler->getLaneletSequence( + root_lanelet_.get(), pose, backward_length, std::numeric_limits::max()); + + lanelet::ConstLanelet closest_lane{}; + if (lanelet::utils::query::getClosestLaneletWithConstrains( + lanelet_sequence, pose, &closest_lane, p.ego_nearest_dist_threshold, + p.ego_nearest_yaw_threshold)) { + return utils::getReferencePath(closest_lane, data); + } + + if (lanelet::utils::query::getClosestLanelet(lanelet_sequence, pose, &closest_lane)) { + return utils::getReferencePath(closest_lane, data); + } + + return {}; // something wrong. +} + SceneModulePtr PlannerManager::selectHighestPriorityModule( std::vector & request_modules) const { @@ -347,20 +420,58 @@ std::pair PlannerManager::runRequestModule * remove non-executable modules. */ for (const auto & module_ptr : sorted_request_modules) { - if (!getManager(module_ptr)->isSimultaneousExecutableAsCandidateModule()) { - if (executable_modules.empty()) { - executable_modules.push_back(module_ptr); - break; - } + // Condition 1: always executable module can be added regardless of the existence of other + // modules. + if (getManager(module_ptr)->isAlwaysExecutableModule()) { + executable_modules.push_back(module_ptr); + continue; } - const auto itr = - std::find_if(executable_modules.begin(), executable_modules.end(), [this](const auto & m) { - return !getManager(m)->isSimultaneousExecutableAsCandidateModule(); - }); - - if (itr == executable_modules.end()) { + // Condition 4: If the executable modules are either empty or consist only of always executable + // modules, any module can be added. + const bool has_non_always_executable_module = std::any_of( + executable_modules.begin(), executable_modules.end(), + [this](const auto & m) { return !getManager(m)->isAlwaysExecutableModule(); }); + if (!has_non_always_executable_module) { executable_modules.push_back(module_ptr); + continue; + } + + // pairs of find_block_module and is_executable + std::vector, std::function>> + conditions; + + // Condition 3: Only modules that are always executable can be added + // if there exists at least one executable module that is neither always nor simultaneous + // executable. + conditions.push_back( + {[this](const SceneModulePtr & m) { + return !getManager(m)->isAlwaysExecutableModule() && + !getManager(m)->isSimultaneousExecutableAsCandidateModule(); + }, + [&]() { return false; }}); + + // Condition 2: Only modules that are either always executable or simultaneous executable can be + // added if there exists at least one executable module that is simultaneous but not always + // executable. + conditions.push_back( + {[this](const SceneModulePtr & m) { + return !getManager(m)->isAlwaysExecutableModule() && + getManager(m)->isSimultaneousExecutableAsCandidateModule(); + }, + [&]() { return getManager(module_ptr)->isSimultaneousExecutableAsCandidateModule(); }}); + + for (const auto & condition : conditions) { + const auto & find_block_module = condition.first; + const auto & is_executable = condition.second; + + const auto itr = + std::find_if(executable_modules.begin(), executable_modules.end(), find_block_module); + + if (itr != executable_modules.end() && is_executable()) { + executable_modules.push_back(module_ptr); + break; + } } } @@ -456,11 +567,49 @@ BehaviorModuleOutput PlannerManager::runApprovedModules(const std::shared_ptrisKeepLast(); + }; + move_to_end(approved_module_ptrs_, keep_last_module_cond); + } + // lock approved modules besides last one std::for_each(approved_module_ptrs_.begin(), approved_module_ptrs_.end(), [&](const auto & m) { m->lockOutputPath(); }); - approved_module_ptrs_.back()->unlockOutputPath(); + + // unlock only last approved module except keep last module. + { + const auto not_keep_last_modules = std::find_if( + approved_module_ptrs_.rbegin(), approved_module_ptrs_.rend(), + [this](const auto & m) { return !getManager(m)->isKeepLast(); }); + + if (not_keep_last_modules != approved_module_ptrs_.rend()) { + (*not_keep_last_modules)->unlockOutputPath(); + } + } /** * execute all approved modules. @@ -478,21 +627,31 @@ BehaviorModuleOutput PlannerManager::runApprovedModules(const std::shared_ptrisWaitingApproval(); }; + const auto not_keep_last_module = std::find_if( + approved_module_ptrs_.rbegin(), approved_module_ptrs_.rend(), + [this](const auto & m) { return !getManager(m)->isKeepLast(); }); - const auto itr = std::find_if( - approved_module_ptrs_.rbegin(), approved_module_ptrs_.rend(), waiting_approval_modules); + // convert reverse iterator -> iterator + const auto begin_itr = not_keep_last_module != approved_module_ptrs_.rend() + ? std::next(not_keep_last_module).base() + : approved_module_ptrs_.begin(); - if (itr != approved_module_ptrs_.rend()) { - const auto is_last_module = std::distance(approved_module_ptrs_.rbegin(), itr) == 0; - if (is_last_module) { - clearCandidateModules(); - candidate_module_ptrs_.push_back(*itr); + const auto waiting_approval_modules_itr = std::find_if( + begin_itr, approved_module_ptrs_.end(), + [](const auto & m) { return m->isWaitingApproval(); }); - debug_info_.emplace_back(*itr, Action::MOVE, "Back To Waiting Approval"); + if (waiting_approval_modules_itr != approved_module_ptrs_.end()) { + clearCandidateModules(); + candidate_module_ptrs_.push_back(*waiting_approval_modules_itr); - approved_module_ptrs_.pop_back(); - } + debug_info_.emplace_back( + *waiting_approval_modules_itr, Action::MOVE, "Back To Waiting Approval"); + + std::for_each( + waiting_approval_modules_itr, approved_module_ptrs_.end(), + [&results](const auto & m) { results.erase(m->name()); }); + + approved_module_ptrs_.erase(waiting_approval_modules_itr); std::for_each( manager_ptrs_.begin(), manager_ptrs_.end(), [](const auto & m) { m->updateObserver(); }); @@ -529,36 +688,43 @@ BehaviorModuleOutput PlannerManager::runApprovedModules(const std::shared_ptrname(); - const auto approved_modules_output = [&output_module_name, &results]() { - if (results.count(output_module_name) == 0) { - return results.at("root"); + const auto approved_modules_output = [&results, this]() { + const auto itr = std::find_if( + approved_module_ptrs_.rbegin(), approved_module_ptrs_.rend(), + [&results](const auto & m) { return results.count(m->name()) != 0; }); + + if (itr != approved_module_ptrs_.rend()) { + return results.at((*itr)->name()); } - return results.at(output_module_name); + return results.at("root"); }(); /** * remove success module immediately. if lane change module has succeeded, update root lanelet. */ { - const auto success_module_itr = std::partition( - approved_module_ptrs_.begin(), approved_module_ptrs_.end(), - [](const auto & m) { return m->getCurrentStatus() != ModuleStatus::SUCCESS; }); + const auto success_module_cond = [](const auto & m) { + return m->getCurrentStatus() == ModuleStatus::SUCCESS; + }; + move_to_end(approved_module_ptrs_, success_module_cond); + + const auto itr = + std::find_if(approved_module_ptrs_.begin(), approved_module_ptrs_.end(), success_module_cond); const auto success_lane_change = std::any_of( - success_module_itr, approved_module_ptrs_.end(), + itr, approved_module_ptrs_.end(), [](const auto & m) { return m->name().find("lane_change") != std::string::npos; }); if (success_lane_change) { - root_lanelet_ = updateRootLanelet(data, true); + root_lanelet_ = updateRootLanelet(data); } - std::for_each(success_module_itr, approved_module_ptrs_.end(), [&](auto & m) { + std::for_each(itr, approved_module_ptrs_.end(), [&](auto & m) { debug_info_.emplace_back(m, Action::DELETE, "From Approved"); deleteExpiredModules(m); }); - approved_module_ptrs_.erase(success_module_itr, approved_module_ptrs_.end()); + approved_module_ptrs_.erase(itr, approved_module_ptrs_.end()); std::for_each( manager_ptrs_.begin(), manager_ptrs_.end(), [](const auto & m) { m->updateObserver(); }); @@ -735,6 +901,14 @@ void PlannerManager::print() const RCLCPP_INFO_STREAM(logger_, string_stream.str()); } +void PlannerManager::publishProcessingTime() const +{ + for (const auto & t : processing_time_) { + std::string name = t.first + std::string("/processing_time_ms"); + debug_publisher_ptr_->publish(name, t.second); + } +} + std::shared_ptr PlannerManager::getDebugMsg() { debug_msg_ptr_ = std::make_shared(); 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 ff5706c34bdc7..84064a1c38bcb 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 @@ -17,18 +17,25 @@ #include "behavior_path_planner/marker_utils/avoidance/debug.hpp" #include "behavior_path_planner/scene_module/scene_module_visitor.hpp" #include "behavior_path_planner/utils/avoidance/utils.hpp" +#include "behavior_path_planner/utils/create_vehicle_footprint.hpp" #include "behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp" #include "behavior_path_planner/utils/path_utils.hpp" #include "behavior_path_planner/utils/utils.hpp" #include #include -#include +#include +#include +#include +#include #include #include #include +#include +#include + #include #include #include @@ -45,16 +52,12 @@ namespace behavior_path_planner { -using marker_utils::CollisionCheckDebug; using motion_utils::calcLongitudinalOffsetPose; using motion_utils::calcSignedArcLength; using motion_utils::findNearestIndex; -using motion_utils::findNearestSegmentIndex; +using tier4_autoware_utils::appendMarkerArray; using tier4_autoware_utils::calcDistance2d; -using tier4_autoware_utils::calcInterpolatedPose; using tier4_autoware_utils::calcLateralDeviation; -using tier4_autoware_utils::calcLongitudinalDeviation; -using tier4_autoware_utils::calcYawDeviation; using tier4_autoware_utils::getPoint; using tier4_autoware_utils::getPose; using tier4_autoware_utils::toHexString; @@ -62,22 +65,6 @@ using tier4_planning_msgs::msg::AvoidanceDebugFactor; namespace { -bool isEndPointsConnected( - const lanelet::ConstLanelet & left_lane, const lanelet::ConstLanelet & right_lane) -{ - const auto & left_back_point_2d = right_lane.leftBound2d().back().basicPoint(); - const auto & right_back_point_2d = left_lane.rightBound2d().back().basicPoint(); - - constexpr double epsilon = 1e-5; - return (right_back_point_2d - left_back_point_2d).norm() < epsilon; -} - -template -void pushUniqueVector(T & base_vector, const T & additional_vector) -{ - base_vector.insert(base_vector.end(), additional_vector.begin(), additional_vector.end()); -} - bool isDrivingSameLane( const lanelet::ConstLanelets & previous_lanes, const lanelet::ConstLanelets & current_lanes) { @@ -101,6 +88,64 @@ bool isBestEffort(const std::string & policy) { return policy == "best_effort"; } + +AvoidLine merge(const AvoidLine & line1, const AvoidLine & line2, const uint64_t id) +{ + AvoidLine ret{}; + + ret.start_idx = line1.start_idx; + ret.start_shift_length = line1.start_shift_length; + ret.start_longitudinal = line1.start_longitudinal; + + ret.end_idx = line2.end_idx; + ret.end_shift_length = line2.end_shift_length; + ret.end_longitudinal = line2.end_longitudinal; + + ret.id = id; + ret.object = line1.object; + + return ret; +} + +AvoidLine fill(const AvoidLine & line1, const AvoidLine & line2, const uint64_t id) +{ + AvoidLine ret{}; + + ret.start_idx = line1.end_idx; + ret.start_shift_length = line1.end_shift_length; + ret.start_longitudinal = line1.end_longitudinal; + + ret.end_idx = line2.start_idx; + ret.end_shift_length = line2.start_shift_length; + ret.end_longitudinal = line2.start_longitudinal; + + ret.id = id; + ret.object = line1.object; + + return ret; +} + +AvoidLineArray toArray(const AvoidOutlines & outlines) +{ + AvoidLineArray ret{}; + for (const auto & outline : outlines) { + ret.push_back(outline.avoid_line); + ret.push_back(outline.return_line); + + std::for_each( + outline.middle_lines.begin(), outline.middle_lines.end(), + [&ret](const auto & line) { ret.push_back(line); }); + } + return ret; +} + +lanelet::BasicLineString3d toLineString3d(const std::vector & bound) +{ + lanelet::BasicLineString3d ret{}; + std::for_each( + bound.begin(), bound.end(), [&](const auto & p) { ret.emplace_back(p.x, p.y, p.z); }); + return ret; +} } // namespace AvoidanceModule::AvoidanceModule( @@ -125,11 +170,14 @@ bool AvoidanceModule::isExecutionRequested() const return true; } - if (avoid_data_.unapproved_new_sl.empty()) { + if (avoid_data_.new_shift_line.empty()) { return false; } - return !avoid_data_.target_objects.empty(); + return std::any_of( + avoid_data_.target_objects.begin(), avoid_data_.target_objects.end(), [](const auto & o) { + return o.is_avoidable || o.reason == AvoidanceDebugFactor::TOO_LARGE_JERK; + }); } bool AvoidanceModule::isExecutionReady() const @@ -166,7 +214,10 @@ bool AvoidanceModule::canTransitSuccessState() } } - const bool has_avoidance_target = !data.target_objects.empty(); + const bool has_avoidance_target = + std::any_of(data.target_objects.begin(), data.target_objects.end(), [](const auto & o) { + return o.is_avoidable || o.reason == AvoidanceDebugFactor::TOO_LARGE_JERK; + }); const bool has_shift_point = !path_shifter_.getShiftLines().empty(); const bool has_base_offset = std::abs(path_shifter_.getBaseOffset()) > parameters_->lateral_avoid_check_threshold; @@ -200,6 +251,23 @@ void AvoidanceModule::fillFundamentalData(AvoidancePlanningData & data, DebugDat data.current_lanelets = utils::avoidance::getCurrentLanesFromPath( *getPreviousModuleOutput().reference_path, planner_data_); + // expand drivable lanes + std::for_each( + data.current_lanelets.begin(), data.current_lanelets.end(), [&](const auto & lanelet) { + data.drivable_lanes.push_back( + utils::avoidance::generateExpandDrivableLanes(lanelet, planner_data_, parameters_)); + }); + + // calc drivable bound + const auto shorten_lanes = + utils::cutOverlappedLanes(*getPreviousModuleOutput().path, data.drivable_lanes); + data.left_bound = toLineString3d(utils::calcBound( + planner_data_->route_handler, shorten_lanes, parameters_->use_hatched_road_markings, + parameters_->use_intersection_areas, true)); + data.right_bound = toLineString3d(utils::calcBound( + planner_data_->route_handler, shorten_lanes, parameters_->use_hatched_road_markings, + parameters_->use_intersection_areas, false)); + // reference path if (isDrivingSameLane(helper_.getPreviousDrivingLanes(), data.current_lanelets)) { data.reference_path_rough = extendBackwardLength(*getPreviousModuleOutput().path); @@ -244,10 +312,17 @@ void AvoidanceModule::fillAvoidanceTargetObjects( using utils::avoidance::filterTargetObjects; using utils::avoidance::getTargetLanelets; + // Add margin in order to prevent avoidance request chattering only when the module is running. + const auto is_running = getCurrentStatus() == ModuleStatus::RUNNING || + getCurrentStatus() == ModuleStatus::WAITING_APPROVAL; + // Separate dynamic objects based on whether they are inside or outside of the expanded lanelets. + const auto sparse_resample_path = utils::resamplePathWithSpline( + helper_.getPreviousSplineShiftPath().path, parameters_->resample_interval_for_output); const auto [object_within_target_lane, object_outside_target_lane] = utils::avoidance::separateObjectsByPath( - helper_.getPreviousSplineShiftPath().path, planner_data_, data, parameters_, debug); + sparse_resample_path, planner_data_, data, parameters_, helper_.getForwardDetectionRange(), + is_running, debug); for (const auto & object : object_outside_target_lane.objects) { ObjectData other_object; @@ -329,7 +404,7 @@ ObjectData AvoidanceModule::createObjectData( // Find the footprint point closest to the path, set to object_data.overhang_distance. object_data.overhang_dist = utils::avoidance::calcEnvelopeOverhangDistance( - object_data, object_closest_pose, object_data.overhang_pose.position); + object_data, data.reference_path, object_data.overhang_pose.position); // Check whether the the ego should avoid the object. const auto & vehicle_width = planner_data_->parameters.vehicle_width; @@ -379,42 +454,44 @@ void AvoidanceModule::fillShiftLine(AvoidancePlanningData & data, DebugData & de auto path_shifter = path_shifter_; /** - * STEP 1 - * Create raw shift points from target object. The lateral margin between the ego and the target - * object varies depending on the relative speed between the ego and the target object. + * STEP1: Generate avoid outlines. + * Basically, avoid outlines are generated per target objects. */ - data.unapproved_raw_sl = calcRawShiftLinesFromObjects(data, debug); + const auto outlines = generateAvoidOutline(data, debug); /** - * STEP 2 - * Modify the raw shift points. (Merging, Trimming) + * STEP2: Create rough shift lines. */ - const auto processed_raw_sp = applyPreProcessToRawShiftLines(data.unapproved_raw_sl, debug); + data.raw_shift_line = applyPreProcess(outlines, debug); /** - * STEP 3 - * Find new shift point + * STEP3: Create candidate shift lines. + * Merge rough shift lines, and extract new shift lines. */ - const auto new_sp = findNewShiftLine(processed_raw_sp); - if (isValidShiftLine(new_sp, path_shifter)) { - data.unapproved_new_sl = new_sp; - } + const auto processed_shift_lines = generateCandidateShiftLine(data.raw_shift_line, debug); - const auto found_new_sl = data.unapproved_new_sl.size() > 0; + /** + * Step4: Validate new shift lines. + * Output new shift lines only when the avoidance path which is generated from them doesn't have + * huge offset from ego. + */ + data.valid = isValidShiftLine(processed_shift_lines, path_shifter); + data.new_shift_line = data.valid ? processed_shift_lines : AvoidLineArray{}; + const auto found_new_sl = data.new_shift_line.size() > 0; const auto registered = path_shifter.getShiftLines().size() > 0; data.found_avoidance_path = found_new_sl || registered; /** - * STEP 4 - * If there are new shift points, these shift points are registered in path_shifter. + * STEP5: Set new shift lines. + * If there are new shift points, these shift points are registered in path_shifter in order to + * generate candidate avoidance path. */ - if (!data.unapproved_new_sl.empty()) { - addNewShiftLines(path_shifter, data.unapproved_new_sl); + if (!data.new_shift_line.empty()) { + addNewShiftLines(path_shifter, data.new_shift_line); } /** - * STEP 5 - * Generate avoidance path. + * STEP6: Generate avoidance path. */ ShiftedPath spline_shift_path = utils::avoidance::toShiftedPath(data.reference_path); const auto success_spline_path_generation = @@ -424,17 +501,28 @@ void AvoidanceModule::fillShiftLine(AvoidancePlanningData & data, DebugData & de : utils::avoidance::toShiftedPath(data.reference_path); /** - * STEP 6 - * Check avoidance path safety. For each target objects and the objects in adjacent lanes, + * STEP7: Check avoidance path safety. + * For each target objects and the objects in adjacent lanes, * check that there is a certain amount of margin in the lateral and longitudinal direction. */ - data.comfortable = isComfortable(data.unapproved_new_sl); + data.comfortable = isComfortable(data.new_shift_line); data.safe = isSafePath(data.candidate_path, debug); } void AvoidanceModule::fillEgoStatus( AvoidancePlanningData & data, [[maybe_unused]] DebugData & debug) const { + /** + * TODO(someone): prevent meaningless stop point insertion in other way. + * If the candidate shift line is invalid, manage all objects as unavoidable. + */ + if (!data.valid) { + std::for_each(data.target_objects.begin(), data.target_objects.end(), [](auto & o) { + o.is_avoidable = false; + o.reason = "InvalidShiftLine"; + }); + } + /** * Find the nearest object that should be avoid. When the ego follows reference path, * if the both of following two conditions are satisfied, the module surely avoid the object. @@ -459,7 +547,7 @@ void AvoidanceModule::fillEgoStatus( * If the output path is locked by outside of this module, don't update output path. */ if (isOutputPathLocked()) { - data.safe_new_sl.clear(); + data.safe_shift_line.clear(); data.candidate_path = helper_.getPreviousSplineShiftPath(); RCLCPP_DEBUG_THROTTLE( getLogger(), *clock_, 500, "this module is locked now. keep current path."); @@ -471,7 +559,7 @@ void AvoidanceModule::fillEgoStatus( */ if (data.safe) { data.yield_required = false; - data.safe_new_sl = data.unapproved_new_sl; + data.safe_shift_line = data.new_shift_line; return; } @@ -481,7 +569,7 @@ void AvoidanceModule::fillEgoStatus( */ if (!parameters_->enable_yield_maneuver) { data.yield_required = false; - data.safe_new_sl = data.unapproved_new_sl; + data.safe_shift_line = data.new_shift_line; return; } @@ -493,7 +581,7 @@ void AvoidanceModule::fillEgoStatus( if (!can_yield_maneuver) { data.safe = true; // overwrite safety judge. data.yield_required = false; - data.safe_new_sl = data.unapproved_new_sl; + data.safe_shift_line = data.new_shift_line; RCLCPP_WARN_THROTTLE(getLogger(), *clock_, 500, "unsafe. but could not transit yield status."); return; } @@ -503,7 +591,7 @@ void AvoidanceModule::fillEgoStatus( */ { data.yield_required = true; - data.safe_new_sl = data.unapproved_new_sl; + data.safe_shift_line = data.new_shift_line; } /** @@ -525,12 +613,9 @@ void AvoidanceModule::fillEgoStatus( } } -void AvoidanceModule::fillDebugData(const AvoidancePlanningData & data, DebugData & debug) const +void AvoidanceModule::fillDebugData( + const AvoidancePlanningData & data, [[maybe_unused]] DebugData & debug) const { - debug.output_shift = data.candidate_path.shift_length; - debug.current_raw_shift = data.unapproved_raw_sl; - debug.new_shift_lines = data.unapproved_new_sl; - if (!data.stop_target_object) { return; } @@ -539,7 +624,7 @@ void AvoidanceModule::fillDebugData(const AvoidancePlanningData & data, DebugDat return; } - if (data.unapproved_new_sl.empty()) { + if (data.new_shift_line.empty()) { return; } @@ -620,6 +705,8 @@ void AvoidanceModule::updateEgoBehavior(const AvoidancePlanningData & data, Shif throw std::domain_error("invalid behavior"); } + insertReturnDeadLine(isBestEffort(parameters_->policy_deceleration), path); + setStopReason(StopReason::AVOIDANCE, path.path); } @@ -672,70 +759,77 @@ void AvoidanceModule::updateRegisteredRawShiftLines() printShiftLines(avoid_lines, "registered_raw_shift_lines_ (after)"); registered_raw_shift_lines_ = avoid_lines; - debug_data_.registered_raw_shift = registered_raw_shift_lines_; + debug_data_.step1_registered_shift_line = registered_raw_shift_lines_; } -AvoidLineArray AvoidanceModule::applyPreProcessToRawShiftLines( - AvoidLineArray & raw_shift_lines, DebugData & debug) const +AvoidLineArray AvoidanceModule::applyPreProcess( + const AvoidOutlines & outlines, DebugData & debug) const { - const auto fill_gap_shift_lines = getFillGapShiftLines(raw_shift_lines); + AvoidOutlines processed_outlines = outlines; + + /** + * Step1: Rough merge process. + * Merge multiple avoid outlines. If an avoid outlines' return shift line conflicts other + * outline's avoid shift line, those avoid outlines are merged. + */ + processed_outlines = applyMergeProcess(processed_outlines, debug); + + /** + * Step2: Fill gap process. + * Create and add new shift line to avoid outline in order to fill gaps between avoid shift line + * and middle shift lines, return shift line and middle shift lines. + */ + processed_outlines = applyFillGapProcess(processed_outlines, debug); /** + * Step3: Convert to AvoidLineArray from AvoidOutlines. + */ + AvoidLineArray processed_raw_lines = toArray(processed_outlines); + + /** + * Step4: Combine process. * Use all registered points. For the current points, if the similar one of the current * points are already registered, will not use it. - * TODO(Horibe): enrich this logic to be able to consider the removal of the registered - * shift, because it cannot handle the case like "we don't have to avoid - * the object anymore". */ - raw_shift_lines = utils::avoidance::combineRawShiftLinesWithUniqueCheck( - registered_raw_shift_lines_, raw_shift_lines); - - printShiftLines(raw_shift_lines, "raw_shift_lines"); - printShiftLines(registered_raw_shift_lines_, "registered_raw_shift_lines"); + processed_raw_lines = + applyCombineProcess(processed_raw_lines, registered_raw_shift_lines_, debug); /* + * Step5: Add return shift line. * Add return-to-center shift point from the last shift point, if needed. * If there is no shift points, set return-to center shift from ego. */ - // TODO(Horibe) Here, the return point is calculated considering the prepare distance, - // but there is an issue that sometimes this prepare distance is erased by the trimSimilarGrad, - // and it suddenly tries to return from ego. Then steer rotates aggressively. - // It is temporally solved by changing the threshold of trimSimilarGrad, but it needs to be - // fixed in a proper way. - // Maybe after merge, all shift points before the prepare distance can be deleted. - addReturnShiftLineFromEgo(raw_shift_lines); + processed_raw_lines = addReturnShiftLine(processed_raw_lines, debug); /* - * Add gap filled shift lines so that merged shift lines connect smoothly. + * Step6: Fill gap process. + * Create and add new shift line to avoid lines. */ - fillShiftLineGap(raw_shift_lines); - raw_shift_lines.insert( - raw_shift_lines.end(), fill_gap_shift_lines.begin(), fill_gap_shift_lines.end()); - debug.gap_filled = raw_shift_lines; + return applyFillGapProcess(processed_raw_lines, debug); +} + +AvoidLineArray AvoidanceModule::generateCandidateShiftLine( + const AvoidLineArray & shift_lines, DebugData & debug) const +{ + AvoidLineArray processed_shift_lines = shift_lines; /** - * On each path point, compute shift length with considering the raw shift points. - * Then create a merged shift points by finding the change point of the gradient of shifting. - * - take maximum shift length if there is duplicate shift point - * - take sum if there are shifts for opposite direction (right and left) - * - shift length is interpolated linearly. - * Note: Because this function just foolishly extracts points, it includes - * insignificant small (useless) shift points, which should be removed in post-process. + * Step1: Merge process. + * Merge positive shift avoid lines and negative shift avoid lines. */ - auto merged_shift_lines = mergeShiftLines(raw_shift_lines, debug); - debug.merged = merged_shift_lines; + processed_shift_lines = applyMergeProcess(processed_shift_lines, debug); - /* - * Remove unnecessary shift points - * - Quantize the shift length to reduce the shift point noise - * - Change the shift length to the previous one if the deviation is small. - * - Combine shift points that have almost same gradient - * - Remove unnecessary return shift (back to the center line). + /** + * Step2: Clean up process. + * Remove noisy shift line and concat same gradient shift lines. */ - auto shift_lines = trimShiftLine(merged_shift_lines, debug); - DEBUG_PRINT("final shift point size = %lu", shift_lines.size()); + processed_shift_lines = applyTrimProcess(processed_shift_lines, debug); - return shift_lines; + /** + * Step3: Extract new shift lines. + * Compare processed shift lines and registered shift lines in order to find new shift lines. + */ + return findNewShiftLine(processed_shift_lines, debug); } void AvoidanceModule::registerRawShiftLines(const AvoidLineArray & future) @@ -795,7 +889,7 @@ void AvoidanceModule::registerRawShiftLines(const AvoidLineArray & future) DEBUG_PRINT("registered object size: %lu -> %lu", old_size, registered_raw_shift_lines_.size()); } -AvoidLineArray AvoidanceModule::calcRawShiftLinesFromObjects( +AvoidOutlines AvoidanceModule::generateAvoidOutline( AvoidancePlanningData & data, [[maybe_unused]] DebugData & debug) const { // To be consistent with changes in the ego position, the current shift length is considered. @@ -878,17 +972,35 @@ AvoidLineArray AvoidanceModule::calcRawShiftLinesFromObjects( } // output avoidance path under lateral jerk constraints. - const auto feasible_shift_length = PathShifter::calcLateralDistFromJerk( + const auto feasible_relative_shift_length = PathShifter::calcLateralDistFromJerk( remaining_distance, helper_.getLateralMaxJerkLimit(), helper_.getAvoidanceEgoSpeed()); - RCLCPP_WARN_THROTTLE( - getLogger(), *clock_, 1000, - "original shift length is not feasible. generate avoidance path under the constraints. " - "[original: (%.2f) actual: (%.2f)]", - std::abs(avoiding_shift), feasible_shift_length); + if (std::abs(feasible_relative_shift_length) < parameters_->lateral_execution_threshold) { + object.reason = "LessThanExecutionThreshold"; + return boost::none; + } + + const auto feasible_shift_length = + desire_shift_length > 0.0 ? feasible_relative_shift_length + current_ego_shift + : -1.0 * feasible_relative_shift_length + current_ego_shift; + + const auto feasible = + std::abs(feasible_shift_length - object.overhang_dist) < + 0.5 * planner_data_->parameters.vehicle_width + object_parameter.safety_buffer_lateral; + if (feasible) { + RCLCPP_WARN_THROTTLE( + getLogger(), *clock_, 1000, "feasible shift length is not enough to avoid. "); + object.reason = AvoidanceDebugFactor::INSUFFICIENT_LATERAL_MARGIN; + return boost::none; + } - return desire_shift_length > 0.0 ? feasible_shift_length + current_ego_shift - : -1.0 * feasible_shift_length + current_ego_shift; + { + RCLCPP_WARN_THROTTLE( + getLogger(), *clock_, 1000, "use feasible shift length. [original: (%.2f) actual: (%.2f)]", + std::abs(avoiding_shift), feasible_relative_shift_length); + } + + return feasible_shift_length; }; const auto is_forward_object = [](const auto & object) { return object.longitudinal > 0.0; }; @@ -897,7 +1009,7 @@ AvoidLineArray AvoidanceModule::calcRawShiftLinesFromObjects( return s.start_longitudinal > 0.0 && s.start_longitudinal < s.end_longitudinal; }; - AvoidLineArray avoid_lines; + AvoidOutlines outlines; for (auto & o : data.target_objects) { if (!o.avoid_margin) { o.reason = AvoidanceDebugFactor::INSUFFICIENT_LATERAL_MARGIN; @@ -956,6 +1068,7 @@ AvoidLineArray AvoidanceModule::calcRawShiftLinesFromObjects( al_avoid.end_longitudinal = o.longitudinal - offset; al_avoid.id = getOriginalShiftLineUniqueId(); al_avoid.object = o; + al_avoid.object_on_right = utils::avoidance::isOnRight(o); } AvoidLine al_return; @@ -975,11 +1088,14 @@ AvoidLineArray AvoidanceModule::calcRawShiftLinesFromObjects( o.longitudinal + offset + std::min(feasible_return_distance, return_remaining_distance); al_return.id = getOriginalShiftLineUniqueId(); al_return.object = o; + al_return.object_on_right = utils::avoidance::isOnRight(o); } if (is_valid_shift_line(al_avoid) && is_valid_shift_line(al_return)) { - avoid_lines.push_back(al_avoid); - avoid_lines.push_back(al_return); + outlines.emplace_back(al_avoid, al_return); + } else { + o.reason = "InvalidShiftLine"; + continue; } o.is_avoidable = true; @@ -1007,9 +1123,11 @@ AvoidLineArray AvoidanceModule::calcRawShiftLinesFromObjects( debug_avoidance_initializer_for_shift_line_time_ = clock_->now(); } - utils::avoidance::fillAdditionalInfoFromLongitudinal(data, avoid_lines); + utils::avoidance::fillAdditionalInfoFromLongitudinal(data, outlines); + + debug.step1_current_shift_line = toArray(outlines); - return avoid_lines; + return outlines; } void AvoidanceModule::generateTotalShiftLine( @@ -1195,98 +1313,184 @@ AvoidLineArray AvoidanceModule::extractShiftLinesFromLine(ShiftLineData & shift_ return merged_avoid_lines; } -AvoidLineArray AvoidanceModule::getFillGapShiftLines(const AvoidLineArray & shift_lines) const +AvoidOutlines AvoidanceModule::applyMergeProcess( + const AvoidOutlines & outlines, DebugData & debug) const { - AvoidLineArray ret{}; + AvoidOutlines ret{}; - if (shift_lines.empty()) { - return ret; + if (outlines.size() < 2) { + return outlines; } - const auto calc_gap_shift_line = [&](const auto & line1, const auto & line2) { - AvoidLine gap_filled_line{}; - gap_filled_line.start_shift_length = line1.end_shift_length; - gap_filled_line.start_longitudinal = line1.end_longitudinal; - gap_filled_line.end_shift_length = line2.start_shift_length; - gap_filled_line.end_longitudinal = line2.start_longitudinal; - gap_filled_line.id = getOriginalShiftLineUniqueId(); + const auto no_conflict = [](const auto & line1, const auto & line2) { + return line1.end_idx < line2.start_idx || line2.end_idx < line1.start_idx; + }; + + const auto same_side_shift = [](const auto & line1, const auto & line2) { + return line1.object_on_right == line2.object_on_right; + }; - return gap_filled_line; + const auto within = [](const auto & line, const size_t idx) { + return line.start_idx < idx && idx < line.end_idx; }; - // fill gap among shift lines. - for (size_t i = 0; i < shift_lines.size() - 1; i += 2) { - if (shift_lines.at(i).end_longitudinal > shift_lines.at(i + 1).start_longitudinal) { + ret.push_back(outlines.front()); + + for (size_t i = 1; i < outlines.size(); i++) { + auto & last_outline = ret.back(); + auto & next_outline = outlines.at(i); + + const auto & return_line = last_outline.return_line; + const auto & avoid_line = next_outline.avoid_line; + + if (no_conflict(return_line, avoid_line)) { + ret.push_back(outlines.at(i)); + continue; + } + + const auto merged_shift_line = merge(return_line, avoid_line, getOriginalShiftLineUniqueId()); + + if (!isComfortable(AvoidLineArray{merged_shift_line})) { + ret.push_back(outlines.at(i)); + continue; + } + + if (same_side_shift(return_line, avoid_line)) { + last_outline.middle_lines.push_back(merged_shift_line); + last_outline.return_line = next_outline.return_line; + debug.step1_merged_shift_line.push_back(merged_shift_line); + continue; + } + + if (within(return_line, avoid_line.end_idx) && within(avoid_line, return_line.start_idx)) { + last_outline.middle_lines.push_back(merged_shift_line); + last_outline.return_line = next_outline.return_line; + debug.step1_merged_shift_line.push_back(merged_shift_line); + continue; + } + + if (within(return_line, avoid_line.start_idx) && within(avoid_line, return_line.end_idx)) { + last_outline.middle_lines.push_back(merged_shift_line); + last_outline.return_line = next_outline.return_line; + debug.step1_merged_shift_line.push_back(merged_shift_line); continue; } - ret.push_back(calc_gap_shift_line(shift_lines.at(i), shift_lines.at(i + 1))); } utils::avoidance::fillAdditionalInfoFromLongitudinal(avoid_data_, ret); + utils::avoidance::fillAdditionalInfoFromLongitudinal(avoid_data_, debug.step1_merged_shift_line); return ret; } -void AvoidanceModule::fillShiftLineGap(AvoidLineArray & shift_lines) const +AvoidOutlines AvoidanceModule::applyFillGapProcess( + const AvoidOutlines & outlines, DebugData & debug) const { - using utils::avoidance::setEndData; + AvoidOutlines ret = outlines; - if (shift_lines.empty()) { - return; - } + for (auto & outline : ret) { + if (outline.middle_lines.empty()) { + const auto new_line = + fill(outline.avoid_line, outline.return_line, getOriginalShiftLineUniqueId()); + outline.middle_lines.push_back(new_line); + debug.step1_filled_shift_line.push_back(new_line); + } - const auto & data = avoid_data_; + helper_.alignShiftLinesOrder(outline.middle_lines, false); - helper_.alignShiftLinesOrder(shift_lines, false); + if (outline.avoid_line.end_longitudinal < outline.middle_lines.front().start_longitudinal) { + const auto new_line = + fill(outline.avoid_line, outline.middle_lines.front(), getOriginalShiftLineUniqueId()); + outline.middle_lines.push_back(new_line); + debug.step1_filled_shift_line.push_back(new_line); + } + + helper_.alignShiftLinesOrder(outline.middle_lines, false); - const auto fill_gap = [&shift_lines, this](const auto & front_line, const auto & back_line) { - const auto has_gap = back_line.start_longitudinal - front_line.end_longitudinal > 0.0; - if (!has_gap) { - return; + if (outline.middle_lines.back().end_longitudinal < outline.return_line.start_longitudinal) { + const auto new_line = + fill(outline.middle_lines.back(), outline.return_line, getOriginalShiftLineUniqueId()); + outline.middle_lines.push_back(new_line); + debug.step1_filled_shift_line.push_back(new_line); } - AvoidLine new_line{}; - new_line.start_shift_length = front_line.end_shift_length; - new_line.start_longitudinal = front_line.end_longitudinal; - new_line.end_shift_length = back_line.start_shift_length; - new_line.end_longitudinal = back_line.start_longitudinal; - new_line.id = getOriginalShiftLineUniqueId(); + helper_.alignShiftLinesOrder(outline.middle_lines, false); + } + + utils::avoidance::fillAdditionalInfoFromLongitudinal(avoid_data_, ret); + utils::avoidance::fillAdditionalInfoFromLongitudinal(avoid_data_, debug.step1_filled_shift_line); - shift_lines.push_back(new_line); - }; + return ret; +} + +AvoidLineArray AvoidanceModule::applyFillGapProcess( + const AvoidLineArray & shift_lines, DebugData & debug) const +{ + AvoidLineArray sorted = shift_lines; + + helper_.alignShiftLinesOrder(sorted, false); + + AvoidLineArray ret = sorted; + + if (shift_lines.empty()) { + return ret; + } + + const auto & data = avoid_data_; // fill gap between ego and nearest shift line. - { + if (sorted.front().start_longitudinal > 0.0) { AvoidLine ego_line{}; - setEndData( + utils::avoidance::setEndData( ego_line, helper_.getEgoLinearShift(), data.reference_pose, data.ego_closest_path_index, 0.0); - fill_gap(ego_line, shift_lines.front()); + const auto new_line = fill(ego_line, sorted.front(), getOriginalShiftLineUniqueId()); + ret.push_back(new_line); + debug.step1_front_shift_line.push_back(new_line); } + helper_.alignShiftLinesOrder(sorted, false); + // fill gap among shift lines. - for (size_t i = 0; i < shift_lines.size() - 1; ++i) { - fill_gap(shift_lines.at(i), shift_lines.at(i + 1)); + for (size_t i = 0; i < sorted.size() - 1; ++i) { + if (sorted.at(i + 1).start_longitudinal < sorted.at(i).end_longitudinal) { + continue; + } + + const auto new_line = fill(sorted.at(i), sorted.at(i + 1), getOriginalShiftLineUniqueId()); + ret.push_back(new_line); + debug.step1_front_shift_line.push_back(new_line); } - utils::avoidance::fillAdditionalInfoFromLongitudinal(data, shift_lines); + helper_.alignShiftLinesOrder(ret, false); + + utils::avoidance::fillAdditionalInfoFromLongitudinal(avoid_data_, ret); + utils::avoidance::fillAdditionalInfoFromLongitudinal(avoid_data_, debug.step1_front_shift_line); + + return ret; +} - helper_.alignShiftLinesOrder(shift_lines, false); +AvoidLineArray AvoidanceModule::applyCombineProcess( + const AvoidLineArray & shift_lines, const AvoidLineArray & registered_lines, + DebugData & debug) const +{ + debug.step1_registered_shift_line = registered_lines; + return utils::avoidance::combineRawShiftLinesWithUniqueCheck(registered_lines, shift_lines); } -AvoidLineArray AvoidanceModule::mergeShiftLines( - const AvoidLineArray & raw_shift_lines, DebugData & debug) const +AvoidLineArray AvoidanceModule::applyMergeProcess( + const AvoidLineArray & shift_lines, DebugData & debug) const { - // Generate shift line by merging raw_shift_lines. + // Generate shift line by merging shift_lines. ShiftLineData shift_line_data; - generateTotalShiftLine(raw_shift_lines, shift_line_data); + generateTotalShiftLine(shift_lines, shift_line_data); // Re-generate shift points by detecting gradient-change point of the shift line. auto merged_shift_lines = extractShiftLinesFromLine(shift_line_data); // set parent id for (auto & al : merged_shift_lines) { - al.parent_ids = utils::avoidance::calcParentIds(raw_shift_lines, al); + al.parent_ids = utils::avoidance::calcParentIds(shift_lines, al); } // sort by distance from ego. @@ -1301,39 +1505,13 @@ AvoidLineArray AvoidanceModule::mergeShiftLines( debug.neg_shift_grad = shift_line_data.neg_shift_line_grad; debug.total_forward_grad = shift_line_data.forward_grad; debug.total_backward_grad = shift_line_data.backward_grad; + debug.step2_merged_shift_line = merged_shift_lines; } - // debug print - { - const auto & arc = avoid_data_.arclength_from_ego; - const auto & closest = avoid_data_.ego_closest_path_index; - const auto & sl = shift_line_data.shift_line; - const auto & sg = shift_line_data.shift_line_grad; - const auto & fg = shift_line_data.forward_grad; - const auto & bg = shift_line_data.backward_grad; - using std::setw; - std::stringstream ss; - ss << std::fixed << std::setprecision(3); - ss << "\n[idx, arc, shift (for each shift points, filtered | total), grad (ideal, bwd, fwd)]: " - "closest = " - << closest << ", raw_shift_lines size = " << raw_shift_lines.size() << std::endl; - for (size_t i = 0; i < arc.size(); ++i) { - ss << "i = " << i << " | arc: " << arc.at(i) << " | shift: ("; - for (const auto & p : shift_line_data.shift_line_history) { - ss << setw(5) << p.at(i) << ", "; - } - ss << "| total: " << setw(5) << sl.at(i) << ") | grad: (" << sg.at(i) << ", " << fg.at(i) - << ", " << bg.at(i) << ")" << std::endl; - } - DEBUG_PRINT("%s", ss.str().c_str()); - } - - printShiftLines(merged_shift_lines, "merged_shift_lines"); - return merged_shift_lines; } -AvoidLineArray AvoidanceModule::trimShiftLine( +AvoidLineArray AvoidanceModule::applyTrimProcess( const AvoidLineArray & shift_lines, DebugData & debug) const { if (shift_lines.empty()) { @@ -1348,65 +1526,51 @@ AvoidLineArray AvoidanceModule::trimShiftLine( // - Change the shift length to the previous one if the deviation is small. { constexpr double SHIFT_DIFF_THRES = 1.0; - trimSmallShiftLine(sl_array_trimmed, SHIFT_DIFF_THRES); + applySmallShiftFilter(sl_array_trimmed, SHIFT_DIFF_THRES); } // - Combine avoid points that have almost same gradient. // this is to remove the noise. { const auto THRESHOLD = parameters_->same_grad_filter_1_threshold; - trimSimilarGradShiftLine(sl_array_trimmed, THRESHOLD); - debug.trim_similar_grad_shift = sl_array_trimmed; - printShiftLines(sl_array_trimmed, "after trim_similar_grad_shift"); + applySimilarGradFilter(sl_array_trimmed, THRESHOLD); + debug.step3_grad_filtered_1st = sl_array_trimmed; } // - Quantize the shift length to reduce the shift point noise // This is to remove the noise coming from detection accuracy, interpolation, resampling, etc. { const auto THRESHOLD = parameters_->quantize_filter_threshold; - quantizeShiftLine(sl_array_trimmed, THRESHOLD); - printShiftLines(sl_array_trimmed, "after sl_array_trimmed"); - debug.quantized = sl_array_trimmed; + applyQuantizeProcess(sl_array_trimmed, THRESHOLD); + debug.step3_quantize_filtered = sl_array_trimmed; } // - Change the shift length to the previous one if the deviation is small. { constexpr double SHIFT_DIFF_THRES = 1.0; - trimSmallShiftLine(sl_array_trimmed, SHIFT_DIFF_THRES); - debug.trim_small_shift = sl_array_trimmed; - printShiftLines(sl_array_trimmed, "after trim_small_shift"); + applySmallShiftFilter(sl_array_trimmed, SHIFT_DIFF_THRES); + debug.step3_noise_filtered = sl_array_trimmed; } // - Combine avoid points that have almost same gradient (again) { const auto THRESHOLD = parameters_->same_grad_filter_2_threshold; - trimSimilarGradShiftLine(sl_array_trimmed, THRESHOLD); - debug.trim_similar_grad_shift_second = sl_array_trimmed; - printShiftLines(sl_array_trimmed, "after trim_similar_grad_shift_second"); - } - - // - trimTooSharpShift - // Check if it is not too sharp for the return-to-center shift point. - // If the shift is sharp, it is combined with the next shift point until it gets non-sharp. - { - const auto THRESHOLD = parameters_->sharp_shift_filter_threshold; - trimSharpReturn(sl_array_trimmed, THRESHOLD); - debug.trim_too_sharp_shift = sl_array_trimmed; - printShiftLines(sl_array_trimmed, "after trimSharpReturn"); + applySimilarGradFilter(sl_array_trimmed, THRESHOLD); + debug.step3_grad_filtered_2nd = sl_array_trimmed; } // - Combine avoid points that have almost same gradient (again) { const auto THRESHOLD = parameters_->same_grad_filter_3_threshold; - trimSimilarGradShiftLine(sl_array_trimmed, THRESHOLD); - debug.trim_similar_grad_shift_third = sl_array_trimmed; - printShiftLines(sl_array_trimmed, "after trim_similar_grad_shift_second"); + applySimilarGradFilter(sl_array_trimmed, THRESHOLD); + debug.step3_grad_filtered_3rd = sl_array_trimmed; } return sl_array_trimmed; } -void AvoidanceModule::quantizeShiftLine(AvoidLineArray & shift_lines, const double threshold) const +void AvoidanceModule::applyQuantizeProcess( + AvoidLineArray & shift_lines, const double threshold) const { if (threshold < 1.0e-5) { return; // no need to process @@ -1419,7 +1583,8 @@ void AvoidanceModule::quantizeShiftLine(AvoidLineArray & shift_lines, const doub helper_.alignShiftLinesOrder(shift_lines); } -void AvoidanceModule::trimSmallShiftLine(AvoidLineArray & shift_lines, const double threshold) const +void AvoidanceModule::applySmallShiftFilter( + AvoidLineArray & shift_lines, const double threshold) const { if (shift_lines.empty()) { return; @@ -1437,7 +1602,7 @@ void AvoidanceModule::trimSmallShiftLine(AvoidLineArray & shift_lines, const dou } } -void AvoidanceModule::trimSimilarGradShiftLine( +void AvoidanceModule::applySimilarGradFilter( AvoidLineArray & avoid_lines, const double threshold) const { if (avoid_lines.empty()) { @@ -1501,165 +1666,35 @@ void AvoidanceModule::trimSimilarGradShiftLine( DEBUG_PRINT("size %lu -> %lu", input.size(), avoid_lines.size()); } -void AvoidanceModule::trimSharpReturn(AvoidLineArray & shift_lines, const double threshold) const +AvoidLineArray AvoidanceModule::addReturnShiftLine( + const AvoidLineArray & shift_lines, DebugData & debug) const { - AvoidLineArray shift_lines_orig = shift_lines; - shift_lines.clear(); - - const auto isZero = [](double v) { return std::abs(v) < 0.01; }; - - // check if the shift point is positive (avoiding) shift - const auto isPositive = [&](const auto & sl) { - constexpr auto POSITIVE_SHIFT_THR = 0.1; - return std::abs(sl.end_shift_length) - std::abs(sl.start_shift_length) > POSITIVE_SHIFT_THR; - }; - - // check if the shift point is negative (returning) shift - const auto isNegative = [&](const auto & sl) { - constexpr auto NEGATIVE_SHIFT_THR = -0.1; - return std::abs(sl.end_shift_length) - std::abs(sl.start_shift_length) < NEGATIVE_SHIFT_THR; - }; - - // combine two shift points. Be careful the order of "now" and "next". - const auto combineShiftLine = [this](const auto & sl_next, const auto & sl_now) { - auto sl_modified = sl_now; - utils::avoidance::setEndData( - sl_modified, sl_next.end_shift_length, sl_next.end, sl_next.end_idx, - sl_next.end_longitudinal); - sl_modified.parent_ids = - utils::avoidance::concatParentIds(sl_modified.parent_ids, sl_now.parent_ids); - return sl_modified; - }; - - // Check if the merged shift has a conflict with the original shifts. - const auto hasViolation = [&threshold](const auto & combined, const auto & combined_src) { - for (const auto & sl : combined_src) { - const auto combined_shift = - utils::avoidance::lerpShiftLengthOnArc(sl.end_longitudinal, combined); - if (sl.end_shift_length < -0.01 && combined_shift > sl.end_shift_length + threshold) { - return true; - } - if (sl.end_shift_length > 0.01 && combined_shift < sl.end_shift_length - threshold) { - return true; - } - } - return false; - }; + AvoidLineArray ret = shift_lines; - // check for all shift points - for (size_t i = 0; i < shift_lines_orig.size(); ++i) { - auto sl_now = shift_lines_orig.at(i); - sl_now.start_shift_length = - shift_lines.empty() ? helper_.getEgoLinearShift() : shift_lines.back().end_shift_length; - - if (sl_now.end_shift_length * sl_now.start_shift_length < -0.01) { - DEBUG_PRINT("i = %lu, This is avoid shift for opposite direction. take this one", i); - continue; - } - - // Do nothing for non-reduce shift point - if (!isNegative(sl_now)) { - shift_lines.push_back(sl_now); - DEBUG_PRINT( - "i = %lu, positive shift. take this one. sl_now.length * sl_now.start_length = %f", i, - sl_now.end_shift_length * sl_now.start_shift_length); - continue; - } - - // The last point is out of target of this function. - if (i == shift_lines_orig.size() - 1) { - shift_lines.push_back(sl_now); - DEBUG_PRINT("i = %lu, last shift. take this one.", i); - continue; - } - - // ----------------------------------------------------------------------- - // ------------ From here, the shift point is "negative" ----------------- - // ----------------------------------------------------------------------- - - // if next shift is negative, combine them. loop until combined shift line - // exceeds merged shift point. - DEBUG_PRINT("i = %lu, found negative dist. search.", i); - { - auto sl_combined = sl_now; - auto sl_combined_prev = sl_combined; - AvoidLineArray sl_combined_array{sl_now}; - size_t j = i + 1; - for (; i < shift_lines_orig.size(); ++j) { - const auto sl_combined = combineShiftLine(shift_lines_orig.at(j), sl_now); - - { - std::stringstream ss; - ss << "i = " << i << ", j = " << j << ": sl_combined = " << toStrInfo(sl_combined); - DEBUG_PRINT("%s", ss.str().c_str()); - } - - // it gets positive. Finish merging. - if (isPositive(sl_combined)) { - shift_lines.push_back(sl_combined); - DEBUG_PRINT("reach positive."); - break; - } - - // Still negative, but it violates the original shift points. - // Finish with the previous merge result. - if (hasViolation(sl_combined, sl_combined_array)) { - shift_lines.push_back(sl_combined_prev); - DEBUG_PRINT("violation found."); - --j; - break; - } - - // Still negative, but it has an enough long distance. Finish merging. - const auto nominal_distance = - helper_.getMaxAvoidanceDistance(sl_combined.getRelativeLength()); - const auto long_distance = - isZero(sl_combined.end_shift_length) ? nominal_distance : nominal_distance * 5.0; - if (sl_combined.getRelativeLongitudinal() > long_distance) { - shift_lines.push_back(sl_combined); - DEBUG_PRINT("still negative, but long enough. Threshold = %f", long_distance); - break; - } - - // It reaches the last point. Still the shift is sharp, but merge with the current result. - if (j == shift_lines_orig.size() - 1) { - shift_lines.push_back(sl_combined); - DEBUG_PRINT("reach end point."); - break; - } - - // Still negative shift, and the distance is not enough. Search next. - sl_combined_prev = sl_combined; - sl_combined_array.push_back(shift_lines_orig.at(j)); - } - i = j; - continue; - } - } - - helper_.alignShiftLinesOrder(shift_lines); - - DEBUG_PRINT("trimSharpReturn: size %lu -> %lu", shift_lines_orig.size(), shift_lines.size()); -} - -void AvoidanceModule::addReturnShiftLineFromEgo(AvoidLineArray & sl_candidates) const -{ constexpr double ep = 1.0e-3; const auto & data = avoid_data_; - const bool has_candidate_point = !sl_candidates.empty(); + const bool has_candidate_point = !ret.empty(); const bool has_registered_point = !path_shifter_.getShiftLines().empty(); + const auto exist_unavoidable_object = std::any_of( + data.target_objects.begin(), data.target_objects.end(), + [](const auto & o) { return !o.is_avoidable && o.longitudinal > 0.0; }); + + if (exist_unavoidable_object) { + return ret; + } + // If the return-to-center shift points are already registered, do nothing. if (!has_registered_point && std::fabs(getCurrentBaseShift()) < ep) { DEBUG_PRINT("No shift points, not base offset. Do not have to add return-shift."); - return; + return ret; } constexpr double RETURN_SHIFT_THRESHOLD = 0.1; DEBUG_PRINT("registered last shift = %f", path_shifter_.getLastShiftLength()); if (std::abs(path_shifter_.getLastShiftLength()) < RETURN_SHIFT_THRESHOLD) { DEBUG_PRINT("Return shift is already registered. do nothing."); - return; + return ret; } // From here, the return-to-center is not registered. But perhaps the candidate is @@ -1671,8 +1706,8 @@ void AvoidanceModule::addReturnShiftLineFromEgo(AvoidLineArray & sl_candidates) { // avoidance points: Yes, shift points: No -> select last avoidance point. if (has_candidate_point && !has_registered_point) { - helper_.alignShiftLinesOrder(sl_candidates, false); - last_sl = sl_candidates.back(); + helper_.alignShiftLinesOrder(ret, false); + last_sl = ret.back(); } // avoidance points: No, shift points: Yes -> select last shift point. @@ -1683,8 +1718,8 @@ void AvoidanceModule::addReturnShiftLineFromEgo(AvoidLineArray & sl_candidates) // avoidance points: Yes, shift points: Yes -> select the last one from both. if (has_candidate_point && has_registered_point) { - helper_.alignShiftLinesOrder(sl_candidates, false); - const auto & al = sl_candidates.back(); + helper_.alignShiftLinesOrder(ret, false); + const auto & al = ret.back(); const auto & sl = utils::avoidance::fillAdditionalInfo( data, AvoidLine{path_shifter_.getLastShiftLine().get()}); last_sl = (sl.end_longitudinal > al.end_longitudinal) ? sl : al; @@ -1698,7 +1733,6 @@ void AvoidanceModule::addReturnShiftLineFromEgo(AvoidLineArray & sl_candidates) last_sl.end_shift_length = getCurrentBaseShift(); } } - printShiftLines(ShiftLineArray{last_sl}, "last shift point"); // There already is a shift point candidates to go back to center line, but it could be too sharp // due to detection noise or timing. @@ -1707,7 +1741,7 @@ void AvoidanceModule::addReturnShiftLineFromEgo(AvoidLineArray & sl_candidates) const auto current_base_shift = helper_.getEgoShift(); if (std::abs(current_base_shift) < ep) { DEBUG_PRINT("last shift almost is zero, and current base_shift is zero. do nothing."); - return; + return ret; } // Is there a shift point in the opposite direction of the current_base_shift? @@ -1715,21 +1749,21 @@ void AvoidanceModule::addReturnShiftLineFromEgo(AvoidLineArray & sl_candidates) // the shift length are for return-shift. // Yes -> we can NOT overwrite, because it might be not a return-shift, but a avoiding // shift to the opposite direction which can not be overwritten by the return-shift. - for (const auto & sl : sl_candidates) { + for (const auto & sl : ret) { if ( (current_base_shift > 0.0 && sl.end_shift_length < -ep) || (current_base_shift < 0.0 && sl.end_shift_length > ep)) { DEBUG_PRINT( "try to put overwrite return shift, but there is shift for opposite direction. Skip " "adding return shift."); - return; + return ret; } } // If return shift already exists in candidate or registered shift lines, skip adding return // shift. if (has_candidate_point || has_registered_point) { - return; + return ret; } // set the return-shift from ego. @@ -1746,7 +1780,7 @@ void AvoidanceModule::addReturnShiftLineFromEgo(AvoidLineArray & sl_candidates) const auto nominal_avoid_distance = helper_.getMaxAvoidanceDistance(last_sl.end_shift_length); if (arclength_from_ego.empty()) { - return; + return ret; } const auto remaining_distance = arclength_from_ego.back() - parameters_->remain_buffer_distance; @@ -1757,7 +1791,7 @@ void AvoidanceModule::addReturnShiftLineFromEgo(AvoidLineArray & sl_candidates) // check if there is enough distance for return. if (last_sl_distance > remaining_distance) { // tmp: add some small number (+1.0) RCLCPP_WARN_THROTTLE(getLogger(), *clock_, 1000, "No enough distance for return."); - return; + return ret; } // If the remaining distance is not enough, the return shift needs to be shrunk. @@ -1809,9 +1843,8 @@ void AvoidanceModule::addReturnShiftLineFromEgo(AvoidLineArray & sl_candidates) al.end_longitudinal = arclength_from_ego.at(al.end_idx); al.end_shift_length = last_sl.end_shift_length; al.start_shift_length = last_sl.end_shift_length; - sl_candidates.push_back(al); - printShiftLines(AvoidLineArray{al}, "prepare for return"); - debug_data_.extra_return_shift.push_back(al); + ret.push_back(al); + debug.step1_return_shift_line.push_back(al); } // shift point for return to center line @@ -1828,12 +1861,11 @@ void AvoidanceModule::addReturnShiftLineFromEgo(AvoidLineArray & sl_candidates) al.end_longitudinal = arclength_from_ego.at(al.end_idx); al.end_shift_length = 0.0; al.start_shift_length = last_sl.end_shift_length; - sl_candidates.push_back(al); - printShiftLines(AvoidLineArray{al}, "return point"); - debug_data_.extra_return_shift.push_back(al); + ret.push_back(al); + debug.step1_return_shift_line.push_back(al); } - DEBUG_PRINT("Return Shift is added."); + return ret; } bool AvoidanceModule::isSafePath( @@ -1845,8 +1877,17 @@ bool AvoidanceModule::isSafePath( return true; // if safety check is disabled, it always return safe. } - const auto ego_predicted_path = - utils::avoidance::convertToPredictedPath(shifted_path.path, planner_data_, parameters_); + const bool limit_to_max_velocity = false; + const auto ego_predicted_path_params = + std::make_shared( + parameters_->ego_predicted_path_params); + const size_t ego_seg_idx = planner_data_->findEgoSegmentIndex(shifted_path.path.points); + const auto ego_predicted_path_for_front_object = utils::path_safety_checker::createPredictedPath( + ego_predicted_path_params, shifted_path.path.points, getEgoPose(), getEgoSpeed(), ego_seg_idx, + true, limit_to_max_velocity); + const auto ego_predicted_path_for_rear_object = utils::path_safety_checker::createPredictedPath( + ego_predicted_path_params, shifted_path.path.points, getEgoPose(), getEgoSpeed(), ego_seg_idx, + false, limit_to_max_velocity); const auto ego_idx = planner_data_->findEgoIndex(shifted_path.path.points); const auto is_right_shift = [&]() -> std::optional { @@ -1869,199 +1910,62 @@ bool AvoidanceModule::isSafePath( return true; } + const auto hysteresis_factor = safe_ ? 1.0 : parameters_->hysteresis_factor_expand_rate; + const auto safety_check_target_objects = utils::avoidance::getSafetyCheckTargetObjects( avoid_data_, planner_data_, parameters_, is_right_shift.value()); for (const auto & object : safety_check_target_objects) { - const auto obj_predicted_paths = utils::path_safety_checker::getPredictedPathFromObj( - object, parameters_->check_all_predicted_path); - for (const auto & obj_path : obj_predicted_paths) { - CollisionCheckDebug collision{}; - if (!utils::path_safety_checker::checkCollision( - shifted_path.path, ego_predicted_path, object, obj_path, p, parameters_->rss_params, - collision)) { - return false; - } - } - } - - return true; -} - -void AvoidanceModule::generateExtendedDrivableArea(BehaviorModuleOutput & output) const -{ - const auto has_same_lane = - [](const lanelet::ConstLanelets lanes, const lanelet::ConstLanelet & lane) { - if (lanes.empty()) return false; - const auto has_same = [&](const auto & ll) { return ll.id() == lane.id(); }; - return std::find_if(lanes.begin(), lanes.end(), has_same) != lanes.end(); - }; + auto current_debug_data = marker_utils::createObjectDebug(object); - const auto & route_handler = planner_data_->route_handler; - const auto & current_lanes = avoid_data_.current_lanelets; - const auto & enable_opposite = parameters_->use_opposite_lane; - std::vector drivable_lanes; + const auto obj_polygon = + tier4_autoware_utils::toPolygon2d(object.initial_pose.pose, object.shape); - for (const auto & current_lane : current_lanes) { - DrivableLanes current_drivable_lanes; - current_drivable_lanes.left_lane = current_lane; - current_drivable_lanes.right_lane = current_lane; - - if (!parameters_->use_adjacent_lane) { - drivable_lanes.push_back(current_drivable_lanes); - continue; - } - - // 1. get left/right side lanes - const auto update_left_lanelets = [&](const lanelet::ConstLanelet & target_lane) { - const auto all_left_lanelets = - route_handler->getAllLeftSharedLinestringLanelets(target_lane, enable_opposite, true); - if (!all_left_lanelets.empty()) { - current_drivable_lanes.left_lane = all_left_lanelets.back(); // leftmost lanelet - pushUniqueVector( - current_drivable_lanes.middle_lanes, - lanelet::ConstLanelets(all_left_lanelets.begin(), all_left_lanelets.end() - 1)); - } - }; - const auto update_right_lanelets = [&](const lanelet::ConstLanelet & target_lane) { - const auto all_right_lanelets = - route_handler->getAllRightSharedLinestringLanelets(target_lane, enable_opposite, true); - if (!all_right_lanelets.empty()) { - current_drivable_lanes.right_lane = all_right_lanelets.back(); // rightmost lanelet - pushUniqueVector( - current_drivable_lanes.middle_lanes, - lanelet::ConstLanelets(all_right_lanelets.begin(), all_right_lanelets.end() - 1)); - } - }; - - update_left_lanelets(current_lane); - update_right_lanelets(current_lane); - - // 2.1 when there are multiple lanes whose previous lanelet is the same - const auto get_next_lanes_from_same_previous_lane = - [&route_handler](const lanelet::ConstLanelet & lane) { - // get previous lane, and return false if previous lane does not exist - lanelet::ConstLanelets prev_lanes; - if (!route_handler->getPreviousLaneletsWithinRoute(lane, &prev_lanes)) { - return lanelet::ConstLanelets{}; - } + const auto is_object_front = + utils::path_safety_checker::isTargetObjectFront(getEgoPose(), obj_polygon, p.vehicle_info); - lanelet::ConstLanelets next_lanes; - for (const auto & prev_lane : prev_lanes) { - const auto next_lanes_from_prev = route_handler->getNextLanelets(prev_lane); - pushUniqueVector(next_lanes, next_lanes_from_prev); - } - return next_lanes; - }; - - const auto next_lanes_for_right = - get_next_lanes_from_same_previous_lane(current_drivable_lanes.right_lane); - const auto next_lanes_for_left = - get_next_lanes_from_same_previous_lane(current_drivable_lanes.left_lane); - - // 2.2 look for neighbor lane recursively, where end line of the lane is connected to end line - // of the original lane - const auto update_drivable_lanes = - [&](const lanelet::ConstLanelets & next_lanes, const bool is_left) { - for (const auto & next_lane : next_lanes) { - const auto & edge_lane = - is_left ? current_drivable_lanes.left_lane : current_drivable_lanes.right_lane; - if (next_lane.id() == edge_lane.id()) { - continue; - } + const auto is_object_oncoming = + utils::path_safety_checker::isTargetObjectOncoming(getEgoPose(), object.initial_pose.pose); - const auto & left_lane = is_left ? next_lane : edge_lane; - const auto & right_lane = is_left ? edge_lane : next_lane; - if (!isEndPointsConnected(left_lane, right_lane)) { - continue; - } + const auto obj_predicted_paths = utils::path_safety_checker::getPredictedPathFromObj( + object, parameters_->check_all_predicted_path); - if (is_left) { - current_drivable_lanes.left_lane = next_lane; - } else { - current_drivable_lanes.right_lane = next_lane; - } + const auto & ego_predicted_path = is_object_front && !is_object_oncoming + ? ego_predicted_path_for_front_object + : ego_predicted_path_for_rear_object; - if (!has_same_lane(current_drivable_lanes.middle_lanes, edge_lane)) { - if (is_left) { - if (current_drivable_lanes.right_lane.id() != edge_lane.id()) { - current_drivable_lanes.middle_lanes.push_back(edge_lane); - } - } else { - if (current_drivable_lanes.left_lane.id() != edge_lane.id()) { - current_drivable_lanes.middle_lanes.push_back(edge_lane); - } - } - } + for (const auto & obj_path : obj_predicted_paths) { + if (!utils::path_safety_checker::checkCollision( + shifted_path.path, ego_predicted_path, object, obj_path, p, parameters_->rss_params, + hysteresis_factor, current_debug_data.second)) { + marker_utils::updateCollisionCheckDebugMap( + debug.collision_check, current_debug_data, false); - return true; - } + safe_count_ = 0; return false; - }; - - const auto expand_drivable_area_recursively = - [&](const lanelet::ConstLanelets & next_lanes, const bool is_left) { - // NOTE: set max search num to avoid infinity loop for drivable area expansion - constexpr size_t max_recursive_search_num = 3; - for (size_t i = 0; i < max_recursive_search_num; ++i) { - const bool is_update_kept = update_drivable_lanes(next_lanes, is_left); - if (!is_update_kept) { - break; - } - if (i == max_recursive_search_num - 1) { - RCLCPP_ERROR( - rclcpp::get_logger("behavior_path_planner").get_child("avoidance"), - "Drivable area expansion reaches max iteration."); - } - } - }; - expand_drivable_area_recursively(next_lanes_for_right, false); - expand_drivable_area_recursively(next_lanes_for_left, true); - - // 3. update again for new left/right lanes - update_left_lanelets(current_drivable_lanes.left_lane); - update_right_lanelets(current_drivable_lanes.right_lane); - - // 4. compensate that current_lane is in either of left_lane, right_lane or middle_lanes. - if ( - current_drivable_lanes.left_lane.id() != current_lane.id() && - current_drivable_lanes.right_lane.id() != current_lane.id()) { - current_drivable_lanes.middle_lanes.push_back(current_lane); + } } - - drivable_lanes.push_back(current_drivable_lanes); + marker_utils::updateCollisionCheckDebugMap(debug.collision_check, current_debug_data, true); } - { // for new architecture - DrivableAreaInfo current_drivable_area_info; - // generate drivable lanes - current_drivable_area_info.drivable_lanes = drivable_lanes; - // generate obstacle polygons - current_drivable_area_info.obstacles = - utils::avoidance::generateObstaclePolygonsForDrivableArea( - avoid_data_.target_objects, parameters_, planner_data_->parameters.vehicle_width / 2.0); - // expand hatched road markings - current_drivable_area_info.enable_expanding_hatched_road_markings = - parameters_->use_hatched_road_markings; - // expand intersection areas - current_drivable_area_info.enable_expanding_intersection_areas = - parameters_->use_intersection_areas; + safe_count_++; - output.drivable_area_info = utils::combineDrivableAreaInfo( - current_drivable_area_info, getPreviousModuleOutput().drivable_area_info); - } + return safe_ || safe_count_ > parameters_->hysteresis_factor_safe_count; } PathWithLaneId AvoidanceModule::extendBackwardLength(const PathWithLaneId & original_path) const { - // special for avoidance: take behind distance upt ot shift-start-point if it exist. + const auto previous_path = helper_.getPreviousReferencePath(); + const auto longest_dist_to_shift_point = [&]() { double max_dist = 0.0; for (const auto & pnt : path_shifter_.getShiftLines()) { - max_dist = std::max(max_dist, calcDistance2d(getEgoPose(), pnt.start)); + max_dist = std::max( + max_dist, calcSignedArcLength(previous_path.points, pnt.start.position, getEgoPosition())); } for (const auto & sp : registered_raw_shift_lines_) { - max_dist = std::max(max_dist, calcDistance2d(getEgoPose(), sp.start)); + max_dist = std::max( + max_dist, calcSignedArcLength(previous_path.points, sp.start.position, getEgoPosition())); } return max_dist; }(); @@ -2069,11 +1973,11 @@ PathWithLaneId AvoidanceModule::extendBackwardLength(const PathWithLaneId & orig const auto extra_margin = 10.0; // Since distance does not consider arclength, but just line. const auto backward_length = std::max( planner_data_->parameters.backward_path_length, longest_dist_to_shift_point + extra_margin); - const auto previous_path = helper_.getPreviousReferencePath(); const size_t orig_ego_idx = planner_data_->findEgoIndex(original_path.points); - const size_t prev_ego_idx = - findNearestSegmentIndex(previous_path.points, getPoint(original_path.points.at(orig_ego_idx))); + const size_t prev_ego_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + previous_path.points, getPose(original_path.points.at(orig_ego_idx)), + std::numeric_limits::max(), planner_data_->parameters.ego_nearest_yaw_threshold); size_t clip_idx = 0; for (size_t i = 0; i < prev_ego_idx; ++i) { @@ -2112,7 +2016,7 @@ BehaviorModuleOutput AvoidanceModule::plan() resetPathCandidate(); resetPathReference(); - updatePathShifter(data.safe_new_sl); + updatePathShifter(data.safe_shift_line); if (data.yield_required) { removeRegisteredShiftLines(); @@ -2182,8 +2086,26 @@ BehaviorModuleOutput AvoidanceModule::plan() utils::clipPathLength(*output.path, ego_idx, planner_data_->parameters); // Drivable area generation. - generateExtendedDrivableArea(output); - setDrivableLanes(output.drivable_area_info.drivable_lanes); + { + DrivableAreaInfo current_drivable_area_info; + // generate drivable lanes + current_drivable_area_info.drivable_lanes = avoid_data_.drivable_lanes; + // generate obstacle polygons + current_drivable_area_info.obstacles = + utils::avoidance::generateObstaclePolygonsForDrivableArea( + avoid_data_.target_objects, parameters_, planner_data_->parameters.vehicle_width / 2.0); + // expand hatched road markings + current_drivable_area_info.enable_expanding_hatched_road_markings = + parameters_->use_hatched_road_markings; + // expand intersection areas + current_drivable_area_info.enable_expanding_intersection_areas = + parameters_->use_intersection_areas; + + output.drivable_area_info = utils::combineDrivableAreaInfo( + current_drivable_area_info, getPreviousModuleOutput().drivable_area_info); + + setDrivableLanes(output.drivable_area_info.drivable_lanes); + } return output; } @@ -2196,37 +2118,35 @@ CandidateOutput AvoidanceModule::planCandidate() const auto shifted_path = data.candidate_path; - if (!data.safe_new_sl.empty()) { // clip from shift start index for visualize - utils::clipPathLength( - shifted_path.path, data.safe_new_sl.front().start_idx, std::numeric_limits::max(), - 0.0); + if (data.safe_shift_line.empty()) { + const size_t ego_idx = planner_data_->findEgoIndex(shifted_path.path.points); + utils::clipPathLength(shifted_path.path, ego_idx, planner_data_->parameters); - const auto sl = helper_.getMainShiftLine(data.safe_new_sl); - const auto sl_front = data.safe_new_sl.front(); - const auto sl_back = data.safe_new_sl.back(); + output.path_candidate = shifted_path.path; + return output; + } - output.lateral_shift = helper_.getRelativeShiftToPath(sl); - output.start_distance_to_path_change = sl_front.start_longitudinal; - output.finish_distance_to_path_change = sl_back.end_longitudinal; + const auto sl = helper_.getMainShiftLine(data.safe_shift_line); + const auto sl_front = data.safe_shift_line.front(); + const auto sl_back = data.safe_shift_line.back(); - 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( - {sl_front.start, sl_back.end}, - {output.start_distance_to_path_change, output.finish_distance_to_path_change}, - SteeringFactor::AVOIDANCE_PATH_CHANGE, steering_factor_direction, SteeringFactor::APPROACHING, - ""); - } + utils::clipPathLength( + shifted_path.path, sl_front.start_idx, std::numeric_limits::max(), 0.0); - const size_t ego_idx = planner_data_->findEgoIndex(shifted_path.path.points); - utils::clipPathLength(shifted_path.path, ego_idx, planner_data_->parameters); + output.lateral_shift = helper_.getRelativeShiftToPath(sl); + output.start_distance_to_path_change = sl_front.start_longitudinal; + output.finish_distance_to_path_change = sl_back.end_longitudinal; - output.path_candidate = shifted_path.path; + const uint16_t steering_factor_direction = std::invoke([&output]() { + return output.lateral_shift > 0.0 ? SteeringFactor::LEFT : SteeringFactor::RIGHT; + }); + steering_factor_interface_ptr_->updateSteeringFactor( + {sl_front.start, sl_back.end}, + {output.start_distance_to_path_change, output.finish_distance_to_path_change}, + SteeringFactor::AVOIDANCE_PATH_CHANGE, steering_factor_direction, SteeringFactor::APPROACHING, + ""); + output.path_candidate = shifted_path.path; return output; } @@ -2260,7 +2180,7 @@ void AvoidanceModule::updatePathShifter(const AvoidLineArray & shift_lines) addNewShiftLines(path_shifter_, shift_lines); - current_raw_shift_lines_ = avoid_data_.unapproved_raw_sl; + current_raw_shift_lines_ = avoid_data_.raw_shift_line; registerRawShiftLines(shift_lines); @@ -2317,7 +2237,7 @@ void AvoidanceModule::addNewShiftLines( continue; } - if (sl.end_idx >= new_shift_end_idx) { + if (sl.end_idx > new_shift_end_idx) { if ( sl.end_shift_length > -1e-3 && new_shift_length > -1e-3 && sl.end_shift_length < new_shift_length) { @@ -2349,18 +2269,19 @@ void AvoidanceModule::addNewShiftLines( path_shifter.setLateralAccelerationLimit(helper_.getLateralMaxAccelLimit()); } -AvoidLineArray AvoidanceModule::findNewShiftLine(const AvoidLineArray & candidates) const +AvoidLineArray AvoidanceModule::findNewShiftLine( + const AvoidLineArray & shift_lines, DebugData & debug) const { - if (candidates.empty()) { + if (shift_lines.empty()) { return {}; } // add small shift lines. const auto add_straight_shift = [&, this](auto & subsequent, bool has_large_shift, const size_t start_idx) { - for (size_t i = start_idx; i < candidates.size(); ++i) { + for (size_t i = start_idx; i < shift_lines.size(); ++i) { if ( - std::abs(candidates.at(i).getRelativeLength()) > + std::abs(shift_lines.at(i).getRelativeLength()) > parameters_->lateral_small_shift_threshold) { if (has_large_shift) { return; @@ -2369,37 +2290,38 @@ AvoidLineArray AvoidanceModule::findNewShiftLine(const AvoidLineArray & candidat has_large_shift = true; } - if (!isComfortable(AvoidLineArray{candidates.at(i)})) { + if (!isComfortable(AvoidLineArray{shift_lines.at(i)})) { return; } - subsequent.push_back(candidates.at(i)); + subsequent.push_back(shift_lines.at(i)); } }; // get subsequent shift lines. const auto get_subsequent_shift = [&, this](size_t i) { - AvoidLineArray subsequent{candidates.at(i)}; + AvoidLineArray subsequent{shift_lines.at(i)}; if (!isComfortable(subsequent)) { return subsequent; } - if (candidates.size() == i + 1) { + if (shift_lines.size() == i + 1) { return subsequent; } - if (!isComfortable(AvoidLineArray{candidates.at(i + 1)})) { + if (!isComfortable(AvoidLineArray{shift_lines.at(i + 1)})) { return subsequent; } if ( - std::abs(candidates.at(i).getRelativeLength()) < parameters_->lateral_small_shift_threshold) { + std::abs(shift_lines.at(i).getRelativeLength()) < + parameters_->lateral_small_shift_threshold) { const auto has_large_shift = - candidates.at(i + 1).getRelativeLength() > parameters_->lateral_small_shift_threshold; + shift_lines.at(i + 1).getRelativeLength() > parameters_->lateral_small_shift_threshold; // candidate.at(i) is small length shift line. add large length shift line. - subsequent.push_back(candidates.at(i + 1)); + subsequent.push_back(shift_lines.at(i + 1)); add_straight_shift(subsequent, has_large_shift, i + 2); } else { // candidate.at(i) is large length shift line. add small length shift lines. @@ -2414,18 +2336,19 @@ AvoidLineArray AvoidanceModule::findNewShiftLine(const AvoidLineArray & candidat return std::abs(helper_.getRelativeShiftToPath(s)) < parameters_->lateral_execution_threshold; }; - for (size_t i = 0; i < candidates.size(); ++i) { - const auto & candidate = candidates.at(i); + for (size_t i = 0; i < shift_lines.size(); ++i) { + const auto & candidate = shift_lines.at(i); // new shift points must exist in front of Ego // this value should be larger than -eps consider path shifter calculation error. - const double eps = 0.01; - if (candidate.start_longitudinal < -eps) { + if (candidate.start_idx < avoid_data_.ego_closest_path_index) { break; } if (!is_ignore_shift(candidate)) { - return get_subsequent_shift(i); + const auto new_shift_lines = get_subsequent_shift(i); + debug.step4_new_shift_line = new_shift_lines; + return new_shift_lines; } } @@ -2437,7 +2360,7 @@ bool AvoidanceModule::isValidShiftLine( const AvoidLineArray & shift_lines, const PathShifter & shifter) const { if (shift_lines.empty()) { - return false; + return true; } auto shifter_for_validate = shifter; @@ -2447,6 +2370,8 @@ bool AvoidanceModule::isValidShiftLine( ShiftedPath proposed_shift_path; shifter_for_validate.generate(&proposed_shift_path); + debug_data_.proposed_spline_shift = proposed_shift_path.shift_length; + // check offset between new shift path and ego position. { const auto new_idx = planner_data_->findEgoIndex(proposed_shift_path.path.points); @@ -2461,7 +2386,31 @@ bool AvoidanceModule::isValidShiftLine( } } - debug_data_.proposed_spline_shift = proposed_shift_path.shift_length; + // check if the vehicle is in road. (yaw angle is not considered) + { + const auto minimum_distance = 0.5 * planner_data_->parameters.vehicle_width + + parameters_->hard_road_shoulder_margin - + parameters_->max_deviation_from_lane; + + const size_t start_idx = shift_lines.front().start_idx; + const size_t end_idx = shift_lines.back().end_idx; + + for (size_t i = start_idx; i <= end_idx; ++i) { + const auto p = getPoint(shifter_for_validate.getReferencePath().points.at(i)); + lanelet::BasicPoint2d basic_point{p.x, p.y}; + + const auto shift_length = proposed_shift_path.shift_length.at(i); + const auto bound = shift_length > 0.0 ? avoid_data_.left_bound : avoid_data_.right_bound; + const auto THRESHOLD = minimum_distance + std::abs(shift_length); + + if (boost::geometry::distance(basic_point, lanelet::utils::to2D(bound)) < THRESHOLD) { + RCLCPP_DEBUG_THROTTLE( + getLogger(), *clock_, 1000, + "following latest new shift line may cause deviation from drivable area."); + return false; + } + } + } return true; // valid shift line. } @@ -2505,11 +2454,14 @@ void AvoidanceModule::updateData() // update rtc status. updateRTCData(); + + safe_ = avoid_data_.safe; } void AvoidanceModule::processOnEntry() { initVariables(); + removeRTCStatus(); } void AvoidanceModule::processOnExit() @@ -2549,7 +2501,7 @@ void AvoidanceModule::updateRTCData() updateRegisteredRTCStatus(helper_.getPreviousSplineShiftPath().path); - const auto candidates = data.safe ? data.safe_new_sl : data.unapproved_new_sl; + const auto candidates = data.safe ? data.safe_shift_line : data.new_shift_line; if (candidates.empty()) { removeCandidateRTCStatus(); @@ -2653,9 +2605,31 @@ TurnSignalInfo AvoidanceModule::calcTurnSignalInfo(const ShiftedPath & path) con turn_signal_info.turn_signal.command = TurnIndicatorsCommand::ENABLE_RIGHT; } } else { - turn_signal_info.turn_signal.command = TurnIndicatorsCommand::DISABLE; + const lanelet::ConstLanelets current_lanes = utils::getCurrentLanes(planner_data_); + const auto local_vehicle_footprint = + createVehicleFootprint(planner_data_->parameters.vehicle_info); + boost::geometry::model::ring shifted_vehicle_footprint; + for (const auto & cl : current_lanes) { + // get left and right bounds of current lane + const auto lane_left_bound = cl.leftBound2d().basicLineString(); + const auto lane_right_bound = cl.rightBound2d().basicLineString(); + for (size_t i = start_idx; i < end_idx; ++i) { + // transform vehicle footprint onto path points + shifted_vehicle_footprint = transformVector( + local_vehicle_footprint, + tier4_autoware_utils::pose2transform(path.path.points.at(i).point.pose)); + if ( + boost::geometry::intersects(lane_left_bound, shifted_vehicle_footprint) || + boost::geometry::intersects(lane_right_bound, shifted_vehicle_footprint)) { + if (segment_shift_length > 0.0) { + turn_signal_info.turn_signal.command = TurnIndicatorsCommand::ENABLE_LEFT; + } else { + turn_signal_info.turn_signal.command = TurnIndicatorsCommand::ENABLE_RIGHT; + } + } + } + } } - if (ego_front_to_shift_start > 0.0) { turn_signal_info.desired_start_point = planner_data_->self_odometry->pose.pose; } else { @@ -2688,12 +2662,14 @@ void AvoidanceModule::updateDebugMarker( using marker_utils::createShiftGradMarkerArray; using marker_utils::createShiftLengthMarkerArray; using marker_utils::createShiftLineMarkerArray; + using marker_utils::showPolygon; + using marker_utils::showPredictedPath; + using marker_utils::showSafetyCheckInfo; using marker_utils::avoidance_marker::createAvoidLineMarkerArray; using marker_utils::avoidance_marker::createEgoStatusMarkerArray; using marker_utils::avoidance_marker::createOtherObjectsMarkerArray; using marker_utils::avoidance_marker::createOverhangFurthestLineStringMarkerArray; using marker_utils::avoidance_marker::createPredictedVehiclePositions; - using marker_utils::avoidance_marker::createUnsafeObjectsMarkerArray; using marker_utils::avoidance_marker::makeOverhangToRoadShoulderMarkerArray; using tier4_autoware_utils::appendMarkerArray; @@ -2703,6 +2679,8 @@ void AvoidanceModule::updateDebugMarker( return; } + const auto & path = data.reference_path; + const auto current_time = rclcpp::Clock{RCL_ROS_TIME}.now(); const auto add = [this](const MarkerArray & added) { appendMarkerArray(added, &debug_marker_); }; @@ -2717,100 +2695,96 @@ void AvoidanceModule::updateDebugMarker( add(createShiftLineMarkerArray(sl_arr, shifter.getBaseOffset(), ns, r, g, b, w)); }; - add(createEgoStatusMarkerArray(data, getEgoPose(), "ego_status")); - add(createPredictedVehiclePositions( - debug.path_with_planned_velocity, "predicted_vehicle_positions")); + const auto addObjects = [&](const ObjectDataArray & objects, const auto & ns) { + add(createOtherObjectsMarkerArray(objects, ns)); + }; - const auto & path = data.reference_path; - add(createPathMarkerArray(debug.center_line, "centerline", 0, 0.0, 0.5, 0.9)); - add(createPathMarkerArray(path, "centerline_resampled", 0, 0.0, 0.9, 0.5)); - add(createPathMarkerArray( - helper_.getPreviousLinearShiftPath().path, "prev_linear_shift", 0, 0.5, 0.4, 0.6)); - add(createPoseMarkerArray(data.reference_pose, "reference_pose", 0, 0.9, 0.3, 0.3)); - - add(createLaneletsAreaMarkerArray(*debug.current_lanelets, "current_lanelet", 0.0, 1.0, 0.0)); - add(createPolygonMarkerArray(debug.detection_area, "detection_area", 0L, 0.16, 1.0, 0.69, 0.1)); - - add(createOtherObjectsMarkerArray( - data.other_objects, AvoidanceDebugFactor::OBJECT_IS_BEHIND_THRESHOLD)); - add(createOtherObjectsMarkerArray( - data.other_objects, AvoidanceDebugFactor::OBJECT_IS_IN_FRONT_THRESHOLD)); - add(createOtherObjectsMarkerArray( - data.other_objects, AvoidanceDebugFactor::OBJECT_BEHIND_PATH_GOAL)); - add(createOtherObjectsMarkerArray( - data.other_objects, AvoidanceDebugFactor::TOO_NEAR_TO_CENTERLINE)); - add(createOtherObjectsMarkerArray(data.other_objects, AvoidanceDebugFactor::OBJECT_IS_NOT_TYPE)); - add(createOtherObjectsMarkerArray(data.other_objects, AvoidanceDebugFactor::NOT_PARKING_OBJECT)); - add(createOtherObjectsMarkerArray(data.other_objects, std::string("MovingObject"))); - add(createOtherObjectsMarkerArray(data.other_objects, std::string("CrosswalkUser"))); - add(createOtherObjectsMarkerArray(data.other_objects, std::string("OutOfTargetArea"))); - add(createOtherObjectsMarkerArray(data.other_objects, std::string("NotNeedAvoidance"))); - add(createOtherObjectsMarkerArray(data.other_objects, std::string("LessThanExecutionThreshold"))); - - add(makeOverhangToRoadShoulderMarkerArray(data.target_objects, "overhang")); - add(createOverhangFurthestLineStringMarkerArray(debug.bounds, "bounds", 1.0, 0.0, 1.0)); - - add(createUnsafeObjectsMarkerArray(debug.unsafe_objects, "unsafe_objects")); - - // parent object info - addAvoidLine(debug.registered_raw_shift, "p_registered_shift", 0.8, 0.8, 0.0); - addAvoidLine(debug.current_raw_shift, "p_current_raw_shift", 0.5, 0.2, 0.2); - addAvoidLine(debug.extra_return_shift, "p_extra_return_shift", 0.0, 0.5, 0.8); + const auto addShiftLength = + [&](const auto & shift_length, const auto & ns, auto r, auto g, auto b) { + add(createShiftLengthMarkerArray(shift_length, path, ns, r, g, b)); + }; + + const auto addShiftGrad = [&]( + const auto & shift_grad, const auto & shift_length, const auto & ns, + auto r, auto g, auto b) { + add(createShiftGradMarkerArray(shift_grad, shift_length, path, ns, r, g, b)); + }; + + // ignore objects + { + addObjects(data.other_objects, AvoidanceDebugFactor::OBJECT_IS_BEHIND_THRESHOLD); + addObjects(data.other_objects, AvoidanceDebugFactor::OBJECT_IS_IN_FRONT_THRESHOLD); + addObjects(data.other_objects, AvoidanceDebugFactor::OBJECT_IS_NOT_TYPE); + addObjects(data.other_objects, AvoidanceDebugFactor::OBJECT_BEHIND_PATH_GOAL); + addObjects(data.other_objects, AvoidanceDebugFactor::TOO_NEAR_TO_CENTERLINE); + addObjects(data.other_objects, AvoidanceDebugFactor::NOT_PARKING_OBJECT); + addObjects(data.other_objects, std::string("MovingObject")); + addObjects(data.other_objects, std::string("CrosswalkUser")); + addObjects(data.other_objects, std::string("OutOfTargetArea")); + addObjects(data.other_objects, std::string("NotNeedAvoidance")); + addObjects(data.other_objects, std::string("LessThanExecutionThreshold")); + addObjects(data.other_objects, std::string("TooNearToGoal")); + } + + // shift line pre-process + { + addAvoidLine(debug.step1_registered_shift_line, "step1_registered_shift_line", 0.2, 0.2, 1.0); + addAvoidLine(debug.step1_current_shift_line, "step1_current_shift_line", 0.2, 0.4, 0.8, 0.3); + addAvoidLine(debug.step1_merged_shift_line, "step1_merged_shift_line", 0.2, 0.6, 0.6, 0.3); + addAvoidLine(debug.step1_filled_shift_line, "step1_filled_shift_line", 0.2, 0.8, 0.4, 0.3); + addAvoidLine(debug.step1_return_shift_line, "step1_return_shift_line", 0.2, 1.0, 0.2, 0.3); + } + + // merge process + { + addAvoidLine(debug.step2_merged_shift_line, "step2_merged_shift_line", 0.2, 1.0, 0.0, 0.3); + } + + // trimming process + { + addAvoidLine(debug.step3_grad_filtered_1st, "step3_grad_filtered_1st", 0.2, 0.8, 0.0, 0.3); + addAvoidLine(debug.step3_grad_filtered_2nd, "step3_grad_filtered_2nd", 0.4, 0.6, 0.0, 0.3); + addAvoidLine(debug.step3_grad_filtered_3rd, "step3_grad_filtered_3rd", 0.6, 0.4, 0.0, 0.3); + } + + // registering process + { + addShiftLine(shifter.getShiftLines(), "step4_old_shift_line", 1.0, 1.0, 0.0, 0.3); + addAvoidLine(data.raw_shift_line, "step4_raw_shift_line", 1.0, 0.0, 0.0, 0.3); + addAvoidLine(data.new_shift_line, "step4_new_shift_line", 1.0, 0.0, 0.0, 0.3); + } + + // safety check + { + add(showSafetyCheckInfo(debug.collision_check, "object_debug_info")); + add(showPredictedPath(debug.collision_check, "ego_predicted_path")); + add(showPolygon(debug.collision_check, "ego_and_target_polygon_relation")); + } // shift length { - const std::string ns = "shift_length"; - add(createShiftLengthMarkerArray(debug.pos_shift, path, ns + "_pos", 0.0, 0.7, 0.5)); - add(createShiftLengthMarkerArray(debug.neg_shift, path, ns + "_neg", 0.0, 0.5, 0.7)); - add(createShiftLengthMarkerArray(debug.total_shift, path, ns + "_total", 0.99, 0.4, 0.2)); + addShiftLength(debug.pos_shift, "merged_length_pos", 0.0, 0.7, 0.5); + addShiftLength(debug.neg_shift, "merged_length_neg", 0.0, 0.5, 0.7); + addShiftLength(debug.total_shift, "merged_length_total", 0.99, 0.4, 0.2); } // shift grad { - const std::string ns = "shift_grad"; - add(createShiftGradMarkerArray( - debug.pos_shift_grad, debug.pos_shift, path, ns + "_pos", 0.0, 0.7, 0.5)); - add(createShiftGradMarkerArray( - debug.neg_shift_grad, debug.neg_shift, path, ns + "_neg", 0.0, 0.5, 0.7)); - add(createShiftGradMarkerArray( - debug.total_forward_grad, debug.total_shift, path, ns + "_total_forward", 0.99, 0.4, 0.2)); - add(createShiftGradMarkerArray( - debug.total_backward_grad, debug.total_shift, path, ns + "_total_backward", 0.4, 0.2, 0.99)); - } - - // shift path - { - const std::string ns = "shift_line"; - add(createShiftLengthMarkerArray( - helper_.getPreviousLinearShiftPath().shift_length, path, ns + "_linear_registered", 0.9, 0.3, - 0.3)); - add(createShiftLengthMarkerArray( - debug.proposed_spline_shift, path, ns + "_spline_proposed", 1.0, 1.0, 1.0)); + addShiftGrad(debug.pos_shift_grad, debug.pos_shift, "merged_grad_pos", 0.0, 0.7, 0.5); + addShiftGrad(debug.neg_shift_grad, debug.neg_shift, "merged_grad_neg", 0.0, 0.5, 0.7); + addShiftGrad(debug.total_forward_grad, debug.total_shift, "grad_forward", 0.99, 0.4, 0.2); + addShiftGrad(debug.total_backward_grad, debug.total_shift, "grad_backward", 0.4, 0.2, 0.9); } - // child shift points + // misc { - const std::string ns = "pipeline"; - add(createAvoidLineMarkerArray(debug.gap_filled, ns + "_1_gap_filled", 0.5, 0.8, 1.0, 0.05)); - add(createAvoidLineMarkerArray(debug.merged, ns + "_2_merge", 0.345, 0.968, 1.0, 0.05)); - add(createAvoidLineMarkerArray( - debug.trim_similar_grad_shift, ns + "_3_concat_by_grad", 0.976, 0.328, 0.910, 0.05)); - add( - createAvoidLineMarkerArray(debug.quantized, ns + "_4_quantized", 0.505, 0.745, 0.969, 0.05)); - add(createAvoidLineMarkerArray( - debug.trim_small_shift, ns + "_5_trim_small_shift", 0.663, 0.525, 0.941, 0.05)); - add(createAvoidLineMarkerArray( - debug.trim_similar_grad_shift_second, ns + "_6_concat_by_grad", 0.97, 0.32, 0.91, 0.05)); - add(createAvoidLineMarkerArray( - debug.trim_momentary_return, ns + "_7_trim_momentary_return", 0.976, 0.078, 0.878, 0.05)); - add(createAvoidLineMarkerArray( - debug.trim_too_sharp_shift, ns + "_8_trim_sharp_shift", 0.576, 0.0, 0.978, 0.05)); - add(createAvoidLineMarkerArray( - debug.trim_similar_grad_shift_third, ns + "_9_concat_by_grad", 1.0, 0.0, 0.0, 0.05)); - } - - addShiftLine(shifter.getShiftLines(), "path_shifter_registered_points", 0.99, 0.99, 0.0, 0.5); - addAvoidLine(debug.new_shift_lines, "path_shifter_proposed_points", 0.99, 0.0, 0.0, 0.5); + add(createEgoStatusMarkerArray(data, getEgoPose(), "ego_status")); + add(createPathMarkerArray(path, "centerline_resampled", 0, 0.0, 0.9, 0.5)); + add(createLaneletsAreaMarkerArray(*debug.current_lanelets, "current_lanelet", 0.0, 1.0, 0.0)); + add(createPolygonMarkerArray(debug.detection_area, "detection_area", 0L, 0.16, 1.0, 0.69, 0.1)); + add(makeOverhangToRoadShoulderMarkerArray(data.target_objects, "overhang")); + add(createOverhangFurthestLineStringMarkerArray(debug.bounds, "bounds", 1.0, 0.0, 1.0)); + } } void AvoidanceModule::updateAvoidanceDebugData( @@ -2864,6 +2838,77 @@ double AvoidanceModule::calcDistanceToStopLine(const ObjectData & object) const return object.longitudinal - std::min(variable + constant, p->stop_max_distance); } +void AvoidanceModule::insertReturnDeadLine( + const bool use_constraints_for_decel, ShiftedPath & shifted_path) const +{ + const auto & data = avoid_data_; + + if (!planner_data_->route_handler->isInGoalRouteSection(data.current_lanelets.back())) { + RCLCPP_DEBUG(getLogger(), "goal is far enough."); + return; + } + + const auto shift_length = path_shifter_.getLastShiftLength(); + + if (std::abs(shift_length) < 1e-3) { + RCLCPP_DEBUG(getLogger(), "don't have to consider return shift."); + return; + } + + const auto min_return_distance = helper_.getMinAvoidanceDistance(shift_length); + + const auto to_goal = calcSignedArcLength( + shifted_path.path.points, getEgoPosition(), shifted_path.path.points.size() - 1); + const auto to_stop_line = to_goal - min_return_distance - parameters_->remain_buffer_distance; + + // If we don't need to consider deceleration constraints, insert a deceleration point + // and return immediately + if (!use_constraints_for_decel) { + utils::avoidance::insertDecelPoint( + getEgoPosition(), to_stop_line - parameters_->stop_buffer, 0.0, shifted_path.path, + stop_pose_); + return; + } + + // If the stop distance is not enough for comfortable stop, don't insert wait point. + const auto is_comfortable_stop = helper_.getFeasibleDecelDistance(0.0) < to_stop_line; + if (!is_comfortable_stop) { + RCLCPP_DEBUG(getLogger(), "stop distance is not enough."); + return; + } + + utils::avoidance::insertDecelPoint( + getEgoPosition(), to_stop_line - parameters_->stop_buffer, 0.0, shifted_path.path, stop_pose_); + + // insert slow down speed. + const double current_target_velocity = PathShifter::calcFeasibleVelocityFromJerk( + shift_length, helper_.getLateralMinJerkLimit(), to_stop_line); + if (current_target_velocity < getEgoSpeed()) { + RCLCPP_DEBUG(getLogger(), "current velocity exceeds target slow down speed."); + return; + } + + const auto start_idx = planner_data_->findEgoIndex(shifted_path.path.points); + for (size_t i = start_idx; i < shifted_path.path.points.size(); ++i) { + const auto distance_from_ego = calcSignedArcLength(shifted_path.path.points, start_idx, i); + + // slow down speed is inserted only in front of the object. + const auto shift_longitudinal_distance = to_stop_line - distance_from_ego; + if (shift_longitudinal_distance < 0.0) { + break; + } + + // target speed with nominal jerk limits. + const double v_target = PathShifter::calcFeasibleVelocityFromJerk( + shift_length, helper_.getLateralMinJerkLimit(), shift_longitudinal_distance); + const double v_original = shifted_path.path.points.at(i).point.longitudinal_velocity_mps; + const double v_insert = + std::max(v_target - parameters_->buf_slow_down_speed, parameters_->min_slow_down_speed); + + shifted_path.path.points.at(i).point.longitudinal_velocity_mps = std::min(v_original, v_insert); + } +} + void AvoidanceModule::insertWaitPoint( const bool use_constraints_for_decel, ShiftedPath & shifted_path) const { @@ -2994,6 +3039,12 @@ void AvoidanceModule::insertPrepareVelocity(ShiftedPath & shifted_path) const const auto object = data.target_objects.front(); + const auto enough_space = + object.is_avoidable || object.reason == AvoidanceDebugFactor::TOO_LARGE_JERK; + if (!enough_space) { + return; + } + // calculate shift length for front object. const auto & vehicle_width = planner_data_->parameters.vehicle_width; const auto object_type = utils::getHighestProbLabel(object.object.classification); diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp index c13c237673625..763cf17890721 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp @@ -14,6 +14,9 @@ #include "behavior_path_planner/scene_module/avoidance/manager.hpp" +#include "tier4_autoware_utils/ros/parameter.hpp" +#include "tier4_autoware_utils/ros/update_param.hpp" + #include #include @@ -23,68 +26,62 @@ namespace behavior_path_planner { -namespace -{ -template -T get_parameter(rclcpp::Node * node, const std::string & ns) -{ - if (node->has_parameter(ns)) { - return node->get_parameter(ns).get_value(); - } - - return node->declare_parameter(ns); -} -} // namespace - AvoidanceModuleManager::AvoidanceModuleManager( rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config) : SceneModuleManagerInterface(node, name, config, {"left", "right"}) { using autoware_auto_perception_msgs::msg::ObjectClassification; + using tier4_autoware_utils::getOrDeclareParameter; AvoidanceParameters p{}; // general params { std::string ns = "avoidance."; p.resample_interval_for_planning = - get_parameter(node, ns + "resample_interval_for_planning"); + getOrDeclareParameter(*node, ns + "resample_interval_for_planning"); p.resample_interval_for_output = - get_parameter(node, ns + "resample_interval_for_output"); - p.enable_bound_clipping = get_parameter(node, ns + "enable_bound_clipping"); - p.enable_cancel_maneuver = get_parameter(node, ns + "enable_cancel_maneuver"); + getOrDeclareParameter(*node, ns + "resample_interval_for_output"); + p.enable_bound_clipping = getOrDeclareParameter(*node, ns + "enable_bound_clipping"); + p.enable_cancel_maneuver = getOrDeclareParameter(*node, ns + "enable_cancel_maneuver"); p.enable_force_avoidance_for_stopped_vehicle = - get_parameter(node, ns + "enable_force_avoidance_for_stopped_vehicle"); - p.enable_yield_maneuver = get_parameter(node, ns + "enable_yield_maneuver"); + getOrDeclareParameter(*node, ns + "enable_force_avoidance_for_stopped_vehicle"); + p.enable_yield_maneuver = getOrDeclareParameter(*node, ns + "enable_yield_maneuver"); p.enable_yield_maneuver_during_shifting = - get_parameter(node, ns + "enable_yield_maneuver_during_shifting"); - p.disable_path_update = get_parameter(node, ns + "disable_path_update"); - p.publish_debug_marker = get_parameter(node, ns + "publish_debug_marker"); - p.print_debug_info = get_parameter(node, ns + "print_debug_info"); + getOrDeclareParameter(*node, ns + "enable_yield_maneuver_during_shifting"); + p.disable_path_update = getOrDeclareParameter(*node, ns + "disable_path_update"); + p.publish_debug_marker = getOrDeclareParameter(*node, ns + "publish_debug_marker"); + p.print_debug_info = getOrDeclareParameter(*node, ns + "print_debug_info"); } // drivable area { std::string ns = "avoidance."; - p.use_adjacent_lane = get_parameter(node, ns + "use_adjacent_lane"); - p.use_opposite_lane = get_parameter(node, ns + "use_opposite_lane"); - p.use_intersection_areas = get_parameter(node, ns + "use_intersection_areas"); - p.use_hatched_road_markings = get_parameter(node, ns + "use_hatched_road_markings"); + p.use_adjacent_lane = getOrDeclareParameter(*node, ns + "use_adjacent_lane"); + p.use_opposite_lane = getOrDeclareParameter(*node, ns + "use_opposite_lane"); + p.use_intersection_areas = getOrDeclareParameter(*node, ns + "use_intersection_areas"); + p.use_hatched_road_markings = + getOrDeclareParameter(*node, ns + "use_hatched_road_markings"); } // target object { const auto get_object_param = [&](std::string && ns) { ObjectParameter param{}; - param.is_target = get_parameter(node, ns + "is_target"); - param.execute_num = get_parameter(node, ns + "execute_num"); - param.moving_speed_threshold = get_parameter(node, ns + "moving_speed_threshold"); - param.moving_time_threshold = get_parameter(node, ns + "moving_time_threshold"); - param.max_expand_ratio = get_parameter(node, ns + "max_expand_ratio"); - param.envelope_buffer_margin = get_parameter(node, ns + "envelope_buffer_margin"); - param.avoid_margin_lateral = get_parameter(node, ns + "avoid_margin_lateral"); - param.safety_buffer_lateral = get_parameter(node, ns + "safety_buffer_lateral"); + param.is_target = getOrDeclareParameter(*node, ns + "is_target"); + param.execute_num = getOrDeclareParameter(*node, ns + "execute_num"); + param.moving_speed_threshold = + getOrDeclareParameter(*node, ns + "moving_speed_threshold"); + param.moving_time_threshold = + getOrDeclareParameter(*node, ns + "moving_time_threshold"); + param.max_expand_ratio = getOrDeclareParameter(*node, ns + "max_expand_ratio"); + param.envelope_buffer_margin = + getOrDeclareParameter(*node, ns + "envelope_buffer_margin"); + param.avoid_margin_lateral = + getOrDeclareParameter(*node, ns + "avoid_margin_lateral"); + param.safety_buffer_lateral = + getOrDeclareParameter(*node, ns + "safety_buffer_lateral"); param.safety_buffer_longitudinal = - get_parameter(node, ns + "safety_buffer_longitudinal"); + getOrDeclareParameter(*node, ns + "safety_buffer_longitudinal"); return param; }; @@ -101,118 +98,152 @@ AvoidanceModuleManager::AvoidanceModuleManager( p.object_parameters.emplace(ObjectClassification::UNKNOWN, get_object_param(ns + "unknown.")); p.lower_distance_for_polygon_expansion = - get_parameter(node, ns + "lower_distance_for_polygon_expansion"); + getOrDeclareParameter(*node, ns + "lower_distance_for_polygon_expansion"); p.upper_distance_for_polygon_expansion = - get_parameter(node, ns + "upper_distance_for_polygon_expansion"); + getOrDeclareParameter(*node, ns + "upper_distance_for_polygon_expansion"); } // target filtering { std::string ns = "avoidance.target_filtering."; - p.threshold_time_force_avoidance_for_stopped_vehicle = - get_parameter(node, ns + "threshold_time_force_avoidance_for_stopped_vehicle"); - p.object_ignore_section_traffic_light_in_front_distance = - get_parameter(node, ns + "object_ignore_section_traffic_light_in_front_distance"); - p.object_ignore_section_crosswalk_in_front_distance = - get_parameter(node, ns + "object_ignore_section_crosswalk_in_front_distance"); + p.threshold_time_force_avoidance_for_stopped_vehicle = getOrDeclareParameter( + *node, ns + "threshold_time_force_avoidance_for_stopped_vehicle"); + p.object_ignore_section_traffic_light_in_front_distance = getOrDeclareParameter( + *node, ns + "object_ignore_section_traffic_light_in_front_distance"); + p.object_ignore_section_crosswalk_in_front_distance = getOrDeclareParameter( + *node, ns + "object_ignore_section_crosswalk_in_front_distance"); p.object_ignore_section_crosswalk_behind_distance = - get_parameter(node, ns + "object_ignore_section_crosswalk_behind_distance"); - p.object_check_forward_distance = - get_parameter(node, ns + "object_check_forward_distance"); - p.object_check_backward_distance = - get_parameter(node, ns + "object_check_backward_distance"); - p.object_check_goal_distance = get_parameter(node, ns + "object_check_goal_distance"); + getOrDeclareParameter(*node, ns + "object_ignore_section_crosswalk_behind_distance"); + p.object_check_goal_distance = + getOrDeclareParameter(*node, ns + "object_check_goal_distance"); p.threshold_distance_object_is_on_center = - get_parameter(node, ns + "threshold_distance_object_is_on_center"); + getOrDeclareParameter(*node, ns + "threshold_distance_object_is_on_center"); p.object_check_shiftable_ratio = - get_parameter(node, ns + "object_check_shiftable_ratio"); + getOrDeclareParameter(*node, ns + "object_check_shiftable_ratio"); p.object_check_min_road_shoulder_width = - get_parameter(node, ns + "object_check_min_road_shoulder_width"); - p.object_last_seen_threshold = get_parameter(node, ns + "object_last_seen_threshold"); + getOrDeclareParameter(*node, ns + "object_check_min_road_shoulder_width"); + p.object_last_seen_threshold = + getOrDeclareParameter(*node, ns + "object_last_seen_threshold"); + } + + { + std::string ns = "avoidance.target_filtering.detection_area."; + p.use_static_detection_area = getOrDeclareParameter(*node, ns + "static"); + p.object_check_min_forward_distance = + getOrDeclareParameter(*node, ns + "min_forward_distance"); + p.object_check_max_forward_distance = + getOrDeclareParameter(*node, ns + "max_forward_distance"); + p.object_check_backward_distance = + getOrDeclareParameter(*node, ns + "backward_distance"); } // safety check general params { std::string ns = "avoidance.safety_check."; - p.enable_safety_check = get_parameter(node, ns + "enable"); - p.check_current_lane = get_parameter(node, ns + "check_current_lane"); - p.check_shift_side_lane = get_parameter(node, ns + "check_shift_side_lane"); - p.check_other_side_lane = get_parameter(node, ns + "check_other_side_lane"); - p.check_unavoidable_object = get_parameter(node, ns + "check_unavoidable_object"); - p.check_other_object = get_parameter(node, ns + "check_other_object"); - p.check_all_predicted_path = get_parameter(node, ns + "check_all_predicted_path"); - p.safety_check_time_horizon = get_parameter(node, ns + "time_horizon"); - p.safety_check_time_resolution = get_parameter(node, ns + "time_resolution"); + p.enable_safety_check = getOrDeclareParameter(*node, ns + "enable"); + p.check_current_lane = getOrDeclareParameter(*node, ns + "check_current_lane"); + p.check_shift_side_lane = getOrDeclareParameter(*node, ns + "check_shift_side_lane"); + p.check_other_side_lane = getOrDeclareParameter(*node, ns + "check_other_side_lane"); + p.check_unavoidable_object = + getOrDeclareParameter(*node, ns + "check_unavoidable_object"); + p.check_other_object = getOrDeclareParameter(*node, ns + "check_other_object"); + p.check_all_predicted_path = + getOrDeclareParameter(*node, ns + "check_all_predicted_path"); p.safety_check_backward_distance = - get_parameter(node, ns + "safety_check_backward_distance"); - p.safety_check_hysteresis_factor = - get_parameter(node, ns + "safety_check_hysteresis_factor"); + getOrDeclareParameter(*node, ns + "safety_check_backward_distance"); + p.hysteresis_factor_expand_rate = + getOrDeclareParameter(*node, ns + "hysteresis_factor_expand_rate"); + p.hysteresis_factor_safe_count = + getOrDeclareParameter(*node, ns + "hysteresis_factor_safe_count"); + } + + // safety check predicted path params + { + std::string ns = "avoidance.safety_check."; + p.ego_predicted_path_params.min_velocity = + getOrDeclareParameter(*node, ns + "min_velocity"); + p.ego_predicted_path_params.max_velocity = + getOrDeclareParameter(*node, ns + "max_velocity"); + p.ego_predicted_path_params.acceleration = + getOrDeclareParameter(*node, "avoidance.constraints.longitudinal.max_acceleration"); + p.ego_predicted_path_params.time_horizon_for_rear_object = + getOrDeclareParameter(*node, ns + "time_horizon_for_rear_object"); + p.ego_predicted_path_params.time_resolution = + getOrDeclareParameter(*node, ns + "time_resolution"); + p.ego_predicted_path_params.delay_until_departure = + getOrDeclareParameter(*node, ns + "delay_until_departure"); } // safety check rss params { std::string ns = "avoidance.safety_check."; p.rss_params.longitudinal_distance_min_threshold = - get_parameter(node, ns + "longitudinal_distance_min_threshold"); + getOrDeclareParameter(*node, ns + "longitudinal_distance_min_threshold"); p.rss_params.longitudinal_velocity_delta_time = - get_parameter(node, ns + "longitudinal_velocity_delta_time"); + getOrDeclareParameter(*node, ns + "longitudinal_velocity_delta_time"); p.rss_params.front_vehicle_deceleration = - get_parameter(node, ns + "expected_front_deceleration"); + getOrDeclareParameter(*node, ns + "expected_front_deceleration"); p.rss_params.rear_vehicle_deceleration = - get_parameter(node, ns + "expected_rear_deceleration"); + getOrDeclareParameter(*node, ns + "expected_rear_deceleration"); p.rss_params.rear_vehicle_reaction_time = - get_parameter(node, ns + "rear_vehicle_reaction_time"); + getOrDeclareParameter(*node, ns + "rear_vehicle_reaction_time"); p.rss_params.rear_vehicle_safety_time_margin = - get_parameter(node, ns + "rear_vehicle_safety_time_margin"); + getOrDeclareParameter(*node, ns + "rear_vehicle_safety_time_margin"); p.rss_params.lateral_distance_max_threshold = - get_parameter(node, ns + "lateral_distance_max_threshold"); + getOrDeclareParameter(*node, ns + "lateral_distance_max_threshold"); } // avoidance maneuver (lateral) { std::string ns = "avoidance.avoidance.lateral."; - p.soft_road_shoulder_margin = get_parameter(node, ns + "soft_road_shoulder_margin"); - p.hard_road_shoulder_margin = get_parameter(node, ns + "hard_road_shoulder_margin"); - p.lateral_execution_threshold = get_parameter(node, ns + "lateral_execution_threshold"); + p.soft_road_shoulder_margin = + getOrDeclareParameter(*node, ns + "soft_road_shoulder_margin"); + p.hard_road_shoulder_margin = + getOrDeclareParameter(*node, ns + "hard_road_shoulder_margin"); + p.lateral_execution_threshold = + getOrDeclareParameter(*node, ns + "lateral_execution_threshold"); p.lateral_small_shift_threshold = - get_parameter(node, ns + "lateral_small_shift_threshold"); + getOrDeclareParameter(*node, ns + "lateral_small_shift_threshold"); p.lateral_avoid_check_threshold = - get_parameter(node, ns + "lateral_avoid_check_threshold"); - p.max_right_shift_length = get_parameter(node, ns + "max_right_shift_length"); - p.max_left_shift_length = get_parameter(node, ns + "max_left_shift_length"); + getOrDeclareParameter(*node, ns + "lateral_avoid_check_threshold"); + p.max_right_shift_length = getOrDeclareParameter(*node, ns + "max_right_shift_length"); + p.max_left_shift_length = getOrDeclareParameter(*node, ns + "max_left_shift_length"); + p.max_deviation_from_lane = + getOrDeclareParameter(*node, ns + "max_deviation_from_lane"); } // avoidance maneuver (longitudinal) { std::string ns = "avoidance.avoidance.longitudinal."; - p.prepare_time = get_parameter(node, ns + "prepare_time"); - p.min_prepare_distance = get_parameter(node, ns + "min_prepare_distance"); - p.remain_buffer_distance = get_parameter(node, ns + "remain_buffer_distance"); - p.min_slow_down_speed = get_parameter(node, ns + "min_slow_down_speed"); - p.buf_slow_down_speed = get_parameter(node, ns + "buf_slow_down_speed"); - p.nominal_avoidance_speed = get_parameter(node, ns + "nominal_avoidance_speed"); + p.prepare_time = getOrDeclareParameter(*node, ns + "prepare_time"); + p.min_prepare_distance = getOrDeclareParameter(*node, ns + "min_prepare_distance"); + p.remain_buffer_distance = getOrDeclareParameter(*node, ns + "remain_buffer_distance"); + p.min_slow_down_speed = getOrDeclareParameter(*node, ns + "min_slow_down_speed"); + p.buf_slow_down_speed = getOrDeclareParameter(*node, ns + "buf_slow_down_speed"); + p.nominal_avoidance_speed = + getOrDeclareParameter(*node, ns + "nominal_avoidance_speed"); } // yield { std::string ns = "avoidance.yield."; - p.yield_velocity = get_parameter(node, ns + "yield_velocity"); + p.yield_velocity = getOrDeclareParameter(*node, ns + "yield_velocity"); } // stop { std::string ns = "avoidance.stop."; - p.stop_max_distance = get_parameter(node, ns + "max_distance"); - p.stop_buffer = get_parameter(node, ns + "stop_buffer"); + p.stop_max_distance = getOrDeclareParameter(*node, ns + "max_distance"); + p.stop_buffer = getOrDeclareParameter(*node, ns + "stop_buffer"); } // policy { std::string ns = "avoidance.policy."; - p.policy_deceleration = get_parameter(node, ns + "deceleration"); - p.policy_lateral_margin = get_parameter(node, ns + "lateral_margin"); + p.policy_deceleration = getOrDeclareParameter(*node, ns + "deceleration"); + p.policy_lateral_margin = getOrDeclareParameter(*node, ns + "lateral_margin"); p.use_shorten_margin_immediately = - get_parameter(node, ns + "use_shorten_margin_immediately"); + getOrDeclareParameter(*node, ns + "use_shorten_margin_immediately"); if (p.policy_deceleration != "best_effort" && p.policy_deceleration != "reliable") { throw std::domain_error("invalid policy. please select 'best_effort' or 'reliable'."); @@ -226,20 +257,23 @@ AvoidanceModuleManager::AvoidanceModuleManager( // constraints (longitudinal) { std::string ns = "avoidance.constraints.longitudinal."; - p.nominal_deceleration = get_parameter(node, ns + "nominal_deceleration"); - p.nominal_jerk = get_parameter(node, ns + "nominal_jerk"); - p.max_deceleration = get_parameter(node, ns + "max_deceleration"); - p.max_jerk = get_parameter(node, ns + "max_jerk"); - p.max_acceleration = get_parameter(node, ns + "max_acceleration"); + p.nominal_deceleration = getOrDeclareParameter(*node, ns + "nominal_deceleration"); + p.nominal_jerk = getOrDeclareParameter(*node, ns + "nominal_jerk"); + p.max_deceleration = getOrDeclareParameter(*node, ns + "max_deceleration"); + p.max_jerk = getOrDeclareParameter(*node, ns + "max_jerk"); + p.max_acceleration = getOrDeclareParameter(*node, ns + "max_acceleration"); } // constraints (lateral) { std::string ns = "avoidance.constraints.lateral."; - p.velocity_map = get_parameter>(node, ns + "velocity"); - p.lateral_max_accel_map = get_parameter>(node, ns + "max_accel_values"); - p.lateral_min_jerk_map = get_parameter>(node, ns + "min_jerk_values"); - p.lateral_max_jerk_map = get_parameter>(node, ns + "max_jerk_values"); + p.velocity_map = getOrDeclareParameter>(*node, ns + "velocity"); + p.lateral_max_accel_map = + getOrDeclareParameter>(*node, ns + "max_accel_values"); + p.lateral_min_jerk_map = + getOrDeclareParameter>(*node, ns + "min_jerk_values"); + p.lateral_max_jerk_map = + getOrDeclareParameter>(*node, ns + "max_jerk_values"); if (p.velocity_map.empty()) { throw std::domain_error("invalid velocity map."); @@ -262,15 +296,15 @@ AvoidanceModuleManager::AvoidanceModuleManager( { std::string ns = "avoidance.shift_line_pipeline."; p.quantize_filter_threshold = - get_parameter(node, ns + "trim.quantize_filter_threshold"); + getOrDeclareParameter(*node, ns + "trim.quantize_filter_threshold"); p.same_grad_filter_1_threshold = - get_parameter(node, ns + "trim.same_grad_filter_1_threshold"); + getOrDeclareParameter(*node, ns + "trim.same_grad_filter_1_threshold"); p.same_grad_filter_2_threshold = - get_parameter(node, ns + "trim.same_grad_filter_2_threshold"); + getOrDeclareParameter(*node, ns + "trim.same_grad_filter_2_threshold"); p.same_grad_filter_3_threshold = - get_parameter(node, ns + "trim.same_grad_filter_3_threshold"); + getOrDeclareParameter(*node, ns + "trim.same_grad_filter_3_threshold"); p.sharp_shift_filter_threshold = - get_parameter(node, ns + "trim.sharp_shift_filter_threshold"); + getOrDeclareParameter(*node, ns + "trim.sharp_shift_filter_threshold"); } parameters_ = std::make_shared(p); diff --git a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp index 8e6ab24929a00..2c5e78a267d5a 100644 --- a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp @@ -14,18 +14,22 @@ #include "behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp" -#include "behavior_path_planner/utils/path_utils.hpp" #include "behavior_path_planner/utils/utils.hpp" +#include "object_recognition_utils/predicted_path_utils.hpp" #include "signal_processing/lowpass_filter_1d.hpp" #include #include -#include +#include + +#include + +#include +#include #include #include #include -#include #include #include @@ -63,7 +67,7 @@ void appendObjectMarker(MarkerArray & marker_array, const geometry_msgs::msg::Po } void appendExtractedPolygonMarker( - MarkerArray & marker_array, const tier4_autoware_utils::Polygon2d & obj_poly) + MarkerArray & marker_array, const tier4_autoware_utils::Polygon2d & obj_poly, const double obj_z) { auto marker = tier4_autoware_utils::createDefaultMarker( "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "extracted_polygons", marker_array.markers.size(), @@ -78,6 +82,7 @@ void appendExtractedPolygonMarker( geometry_msgs::msg::Point bound_geom_point; bound_geom_point.x = bound_point.x(); bound_geom_point.y = bound_point.y(); + bound_geom_point.z = obj_z; marker.points.push_back(bound_geom_point); } @@ -101,20 +106,17 @@ std::pair projectObstacleVelocityToTrajectory( const std::vector & path_points, const PredictedObject & object) { const auto & obj_pose = object.kinematics.initial_pose_with_covariance.pose; - const double obj_vel_norm = std::hypot( - object.kinematics.initial_twist_with_covariance.twist.linear.x, - object.kinematics.initial_twist_with_covariance.twist.linear.y); - + const double obj_yaw = tf2::getYaw(obj_pose.orientation); const size_t obj_idx = motion_utils::findNearestIndex(path_points, obj_pose.position); - - const double obj_vel_yaw = std::atan2( - object.kinematics.initial_twist_with_covariance.twist.linear.y, - object.kinematics.initial_twist_with_covariance.twist.linear.x); const double path_yaw = tf2::getYaw(path_points.at(obj_idx).point.pose.orientation); - return std::make_pair( - obj_vel_norm * std::cos(obj_vel_yaw - path_yaw), - obj_vel_norm * std::sin(obj_vel_yaw - path_yaw)); + const Eigen::Rotation2Dd R_ego_to_obstacle(obj_yaw - path_yaw); + const Eigen::Vector2d obstacle_velocity( + object.kinematics.initial_twist_with_covariance.twist.linear.x, + object.kinematics.initial_twist_with_covariance.twist.linear.y); + const Eigen::Vector2d projected_velocity = R_ego_to_obstacle * obstacle_velocity; + + return std::make_pair(projected_velocity[0], projected_velocity[1]); } double calcObstacleMaxLength(const autoware_auto_perception_msgs::msg::Shape & shape) @@ -150,6 +152,25 @@ double calcDiffAngleAgainstPath( return diff_yaw; } +double calcDiffAngleBetweenPaths( + const std::vector & path_points, const PredictedPath & predicted_path) +{ + const size_t nearest_idx = + motion_utils::findNearestSegmentIndex(path_points, predicted_path.path.front().position); + const double ego_yaw = tf2::getYaw(path_points.at(nearest_idx).point.pose.orientation); + + constexpr size_t max_predicted_path_size = 5; + double signed_max_angle{0.0}; + for (size_t i = 0; i < std::min(max_predicted_path_size, predicted_path.path.size()); ++i) { + const double obj_yaw = tf2::getYaw(predicted_path.path.at(i).orientation); + const double diff_yaw = tier4_autoware_utils::normalizeRadian(obj_yaw - ego_yaw); + if (std::abs(signed_max_angle) < std::abs(diff_yaw)) { + signed_max_angle = diff_yaw; + } + } + return signed_max_angle; +} + double calcDistanceToPath( const std::vector & path_points, const geometry_msgs::msg::Point & target_pos) @@ -280,10 +301,10 @@ BehaviorModuleOutput DynamicAvoidanceModule::plan() std::vector obstacles_for_drivable_area; for (const auto & object : target_objects_) { const auto obstacle_poly = [&]() { - if (parameters_->polygon_generation_method == "ego_path_base") { + if (object.polygon_generation_method == PolygonGenerationMethod::EGO_PATH_BASE) { return calcEgoPathBasedDynamicObstaclePolygon(object); } - if (parameters_->polygon_generation_method == "object_path_base") { + if (object.polygon_generation_method == PolygonGenerationMethod::OBJECT_PATH_BASE) { return calcObjectPathBasedDynamicObstaclePolygon(object); } throw std::logic_error("The polygon_generation_method's string is invalid."); @@ -293,7 +314,7 @@ BehaviorModuleOutput DynamicAvoidanceModule::plan() {object.pose, obstacle_poly.value(), object.is_collision_left}); appendObjectMarker(info_marker_, object.pose); - appendExtractedPolygonMarker(debug_marker_, obstacle_poly.value()); + appendExtractedPolygonMarker(debug_marker_, obstacle_poly.value(), object.pose.position.z); } } @@ -374,6 +395,10 @@ void DynamicAvoidanceModule::updateTargetObjects() predicted_object.kinematics.initial_twist_with_covariance.twist.linear.x, predicted_object.kinematics.initial_twist_with_covariance.twist.linear.y); const auto prev_object = getObstacleFromUuid(prev_objects, obj_uuid); + const auto obj_path = *std::max_element( + predicted_object.kinematics.predicted_paths.begin(), + predicted_object.kinematics.predicted_paths.end(), + [](const PredictedPath & a, const PredictedPath & b) { return a.confidence < b.confidence; }); // 1.a. check label const bool is_label_target_obstacle = @@ -382,15 +407,17 @@ void DynamicAvoidanceModule::updateTargetObjects() continue; } - // 1.b. check if velocity is large enough + // 1.b. check obstacle velocity const auto [obj_tangent_vel, obj_normal_vel] = projectObstacleVelocityToTrajectory(prev_module_path->points, predicted_object); - if (std::abs(obj_tangent_vel) < parameters_->min_obstacle_vel) { + if ( + std::abs(obj_tangent_vel) < parameters_->min_obstacle_vel || + parameters_->max_obstacle_vel < std::abs(obj_tangent_vel)) { continue; } // 1.c. check if object is not crossing ego's path - const double obj_angle = calcDiffAngleAgainstPath(prev_module_path->points, obj_pose); + const double obj_angle = calcDiffAngleBetweenPaths(prev_module_path->points, obj_path); const double max_crossing_object_angle = 0.0 <= obj_tangent_vel ? parameters_->max_overtaking_crossing_object_angle : parameters_->max_oncoming_crossing_object_angle; @@ -447,6 +474,7 @@ void DynamicAvoidanceModule::updateTargetObjects() // 2. Precise filtering of target objects and check if they should be avoided for (const auto & object : target_objects_manager_.getValidObjects()) { + PolygonGenerationMethod polygon_generation_method{PolygonGenerationMethod::EGO_PATH_BASE}; const auto obj_uuid = object.uuid; const auto prev_object = getObstacleFromUuid(prev_objects, obj_uuid); const auto obj_path = *std::max_element( @@ -471,8 +499,8 @@ void DynamicAvoidanceModule::updateTargetObjects() getLateralLongitudinalOffset(prev_module_path->points, object.pose, object.shape); // 2.c. check if object will not cut in - const bool will_object_cut_in = - willObjectCutIn(prev_module_path->points, obj_path, object.vel, lat_lon_offset); + const bool will_object_cut_in = willObjectCutIn( + prev_module_path->points, obj_path, object.vel, lat_lon_offset, polygon_generation_method); if (will_object_cut_in) { RCLCPP_INFO_EXPRESSION( getLogger(), parameters_->enable_debug_info, @@ -517,7 +545,31 @@ void DynamicAvoidanceModule::updateTargetObjects() ? isLeft(prev_module_path->points, future_obj_pose->position) : is_object_left; - // 2.g. calculate longitudinal and lateral offset to avoid + // 2.g. check if the ego is not ahead of the object. + const double signed_dist_ego_to_obj = [&]() { + const size_t ego_seg_idx = planner_data_->findEgoSegmentIndex(prev_module_path->points); + const double lon_offset_ego_to_obj = motion_utils::calcSignedArcLength( + prev_module_path->points, getEgoPose().position, ego_seg_idx, lat_lon_offset.nearest_idx); + if (0 < lon_offset_ego_to_obj) { + return std::max( + 0.0, lon_offset_ego_to_obj - planner_data_->parameters.front_overhang + + lat_lon_offset.min_lon_offset); + } + return std::min( + 0.0, lon_offset_ego_to_obj + planner_data_->parameters.rear_overhang + + lat_lon_offset.max_lon_offset); + }(); + if (signed_dist_ego_to_obj < 0) { + RCLCPP_INFO_EXPRESSION( + getLogger(), parameters_->enable_debug_info, + "[DynamicAvoidance] Ignore obstacle (%s) since distance from ego to object (%f) is less " + "than 0.", + obj_uuid.c_str(), signed_dist_ego_to_obj); + continue; + } + + // 2.h. calculate longitudinal and lateral offset to avoid to generate object polygon by + // "object_path_base" const auto obj_points = tier4_autoware_utils::toPolygon2d(object.pose, object.shape); const auto lon_offset_to_avoid = calcMinMaxLongitudinalOffsetToAvoid( path_points_for_object_polygon, object.pose, obj_points, object.vel, time_to_collision); @@ -526,7 +578,8 @@ void DynamicAvoidanceModule::updateTargetObjects() const bool should_be_avoided = true; target_objects_manager_.updateObject( - obj_uuid, lon_offset_to_avoid, lat_offset_to_avoid, is_collision_left, should_be_avoided); + obj_uuid, lon_offset_to_avoid, lat_offset_to_avoid, is_collision_left, should_be_avoided, + polygon_generation_method); } } @@ -602,16 +655,26 @@ bool DynamicAvoidanceModule::isObjectFarFromPath( bool DynamicAvoidanceModule::willObjectCutIn( const std::vector & ego_path, const PredictedPath & predicted_path, - const double obj_tangent_vel, const LatLonOffset & lat_lon_offset) const + const double obj_tangent_vel, const LatLonOffset & lat_lon_offset, + PolygonGenerationMethod & polygon_generation_method) const { - constexpr double epsilon_path_lat_diff = 0.3; - - // Ignore oncoming object - if (obj_tangent_vel < 0) { + // Check if ego's path and object's path are close. + const bool will_object_cut_in = [&]() { + for (const auto & predicted_path_point : predicted_path.path) { + const double paths_lat_diff = + motion_utils::calcLateralOffset(ego_path, predicted_path_point.position); + if (std::abs(paths_lat_diff) < planner_data_->parameters.vehicle_width / 2.0) { + return true; + } + } + return false; + }(); + if (!will_object_cut_in) { + // The object's path will not cut in return false; } - // Ignore object close to the ego + // Ignore object longitudinally close to the ego const size_t ego_seg_idx = planner_data_->findEgoSegmentIndex(ego_path); const double relative_velocity = getEgoSpeed() - obj_tangent_vel; const double lon_offset_ego_to_obj = @@ -622,17 +685,11 @@ bool DynamicAvoidanceModule::willObjectCutIn( lon_offset_ego_to_obj < std::max( parameters_->min_lon_offset_ego_to_cut_in_object, relative_velocity * parameters_->min_time_to_start_cut_in)) { + polygon_generation_method = PolygonGenerationMethod::EGO_PATH_BASE; return false; } - for (const auto & predicted_path_point : predicted_path.path) { - const double paths_lat_diff = - motion_utils::calcLateralOffset(ego_path, predicted_path_point.position); - if (std::abs(paths_lat_diff) < epsilon_path_lat_diff) { - return true; - } - } - return false; + return true; } DynamicAvoidanceModule::DecisionWithReason DynamicAvoidanceModule::willObjectCutOut( @@ -924,11 +981,19 @@ DynamicAvoidanceModule::calcObjectPathBasedDynamicObstaclePolygon( // calculate left and right bound std::vector obj_left_bound_points; std::vector obj_right_bound_points; + const double obj_path_length = motion_utils::calcArcLength(obj_path.path); for (size_t i = 0; i < obj_path.path.size(); ++i) { const double lon_offset = [&]() { - if (i == 0) return -object.shape.dimensions.x / 2.0 - parameters_->lat_offset_from_obstacle; + if (i == 0) + return -object.shape.dimensions.x / 2.0 - + std::max( + parameters_->min_obj_path_based_lon_polygon_margin, + parameters_->lat_offset_from_obstacle); if (i == obj_path.path.size() - 1) - return object.shape.dimensions.x / 2.0 + parameters_->lat_offset_from_obstacle; + return object.shape.dimensions.x / 2.0 + + std::max( + parameters_->min_obj_path_based_lon_polygon_margin - obj_path_length, + parameters_->lat_offset_from_obstacle); return 0.0; }(); diff --git a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp index 3b2e2caf92e6f..6f624a9f6f017 100644 --- a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp @@ -14,6 +14,8 @@ #include "behavior_path_planner/scene_module/dynamic_avoidance/manager.hpp" +#include "tier4_autoware_utils/ros/update_param.hpp" + #include #include @@ -45,6 +47,7 @@ DynamicAvoidanceModuleManager::DynamicAvoidanceModuleManager( p.avoid_bicycle = node->declare_parameter(ns + "bicycle"); p.avoid_motorcycle = node->declare_parameter(ns + "motorcycle"); p.avoid_pedestrian = node->declare_parameter(ns + "pedestrian"); + p.max_obstacle_vel = node->declare_parameter(ns + "max_obstacle_vel"); p.min_obstacle_vel = node->declare_parameter(ns + "min_obstacle_vel"); p.successive_num_to_entry_dynamic_avoidance_condition = node->declare_parameter(ns + "successive_num_to_entry_dynamic_avoidance_condition"); @@ -81,8 +84,8 @@ DynamicAvoidanceModuleManager::DynamicAvoidanceModuleManager( { // drivable_area_generation std::string ns = "dynamic_avoidance.drivable_area_generation."; - p.polygon_generation_method = - node->declare_parameter(ns + "polygon_generation_method"); + p.min_obj_path_based_lon_polygon_margin = + node->declare_parameter(ns + "object_path_base.min_longitudinal_polygon_margin"); p.lat_offset_from_obstacle = node->declare_parameter(ns + "lat_offset_from_obstacle"); p.max_lat_offset_to_avoid = node->declare_parameter(ns + "max_lat_offset_to_avoid"); p.max_time_for_lat_shift = @@ -134,6 +137,7 @@ void DynamicAvoidanceModuleManager::updateModuleParams( updateParam(parameters, ns + "motorcycle", p->avoid_motorcycle); updateParam(parameters, ns + "pedestrian", p->avoid_pedestrian); + updateParam(parameters, ns + "max_obstacle_vel", p->max_obstacle_vel); updateParam(parameters, ns + "min_obstacle_vel", p->min_obstacle_vel); updateParam( @@ -179,9 +183,9 @@ void DynamicAvoidanceModuleManager::updateModuleParams( { // drivable_area_generation const std::string ns = "dynamic_avoidance.drivable_area_generation."; - - updateParam( - parameters, ns + "polygon_generation_method", p->polygon_generation_method); + updateParam( + parameters, ns + "object_path_base.min_longitudinal_polygon_margin", + p->min_obj_path_based_lon_polygon_margin); updateParam(parameters, ns + "lat_offset_from_obstacle", p->lat_offset_from_obstacle); updateParam(parameters, ns + "max_lat_offset_to_avoid", p->max_lat_offset_to_avoid); updateParam( diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp index f37149f706436..87417bfe6a42d 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp @@ -16,16 +16,20 @@ #include "behavior_path_planner/utils/create_vehicle_footprint.hpp" #include "behavior_path_planner/utils/goal_planner/util.hpp" +#include "behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp" +#include "behavior_path_planner/utils/path_safety_checker/safety_check.hpp" #include "behavior_path_planner/utils/path_shifter/path_shifter.hpp" #include "behavior_path_planner/utils/path_utils.hpp" +#include "behavior_path_planner/utils/start_goal_planner_common/utils.hpp" #include "behavior_path_planner/utils/utils.hpp" +#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" +#include "tier4_autoware_utils/math/unit_conversion.hpp" #include +#include #include #include -#include #include -#include #include #include @@ -34,7 +38,7 @@ #include #include -using motion_utils::calcDecelDistWithJerkAndAccConstraints; +using behavior_path_planner::utils::start_goal_planner_common::calcFeasibleDecelDistance; using motion_utils::calcLongitudinalOffsetPose; using motion_utils::calcSignedArcLength; using motion_utils::findFirstNearestSegmentIndexWithSoftConstraints; @@ -51,10 +55,13 @@ GoalPlannerModule::GoalPlannerModule( const std::string & name, rclcpp::Node & node, const std::shared_ptr & parameters, const std::unordered_map > & rtc_interface_ptr_map) -: SceneModuleInterface{name, node, rtc_interface_ptr_map}, parameters_{parameters} +: SceneModuleInterface{name, node, rtc_interface_ptr_map}, + parameters_{parameters}, + vehicle_info_{vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo()}, + status_{mutex_} { LaneDepartureChecker lane_departure_checker{}; - lane_departure_checker.setVehicleInfo(vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo()); + lane_departure_checker.setVehicleInfo(vehicle_info_); occupancy_grid_map_ = std::make_shared(); @@ -63,24 +70,19 @@ GoalPlannerModule::GoalPlannerModule( // planner when goal modification is not allowed fixed_goal_planner_ = std::make_unique(); - // set enabled planner - if (parameters_->enable_shift_parking) { - pull_over_planners_.push_back(std::make_shared( - node, *parameters, lane_departure_checker, occupancy_grid_map_)); - } - // currently only support geometric_parallel_parking for left side parking - if (left_side_parking_) { - if (parameters_->enable_arc_forward_parking) { - constexpr bool is_forward = true; + for (const std::string & planner_type : parameters_->efficient_path_order) { + if (planner_type == "SHIFT" && parameters_->enable_shift_parking) { + pull_over_planners_.push_back(std::make_shared( + node, *parameters, lane_departure_checker, occupancy_grid_map_)); + } else if (planner_type == "ARC_FORWARD" && parameters_->enable_arc_forward_parking) { pull_over_planners_.push_back(std::make_shared( - node, *parameters, lane_departure_checker, occupancy_grid_map_, is_forward)); - } - if (parameters_->enable_arc_backward_parking) { - constexpr bool is_forward = false; + node, *parameters, lane_departure_checker, occupancy_grid_map_, /*is_forward*/ true)); + } else if (planner_type == "ARC_BACKWARD" && parameters_->enable_arc_backward_parking) { pull_over_planners_.push_back(std::make_shared( - node, *parameters, lane_departure_checker, occupancy_grid_map_, is_forward)); + node, *parameters, lane_departure_checker, occupancy_grid_map_, /*is_forward*/ false)); } } + if (pull_over_planners_.empty()) { RCLCPP_ERROR(getLogger(), "Not found enabled planner"); } @@ -112,16 +114,7 @@ GoalPlannerModule::GoalPlannerModule( freespace_parking_timer_cb_group_); } - resetStatus(); -} - -void GoalPlannerModule::resetStatus() -{ - PUllOverStatus initial_status{}; - status_ = initial_status; - pull_over_path_candidates_.clear(); - closest_start_pose_.reset(); - goal_candidates_.clear(); + status_.reset(); } // This function is needed for waiting for planner_data_ @@ -138,17 +131,15 @@ void GoalPlannerModule::updateOccupancyGrid() void GoalPlannerModule::onTimer() { // already generated pull over candidate paths - if (!pull_over_path_candidates_.empty()) { + if (!status_.get_pull_over_path_candidates().empty()) { return; } // goals are not yet available. - if (goal_candidates_.empty()) { + if (status_.get_goal_candidates().empty()) { return; } - mutex_.lock(); - const auto goal_candidates = goal_candidates_; - mutex_.unlock(); + const auto goal_candidates = status_.get_goal_candidates(); // generate valid pull over path candidates and calculate closest start pose const auto current_lanes = utils::getExtendedCurrentLanes( @@ -176,15 +167,14 @@ void GoalPlannerModule::onTimer() } } }; - // plan candidate paths and set them to the member variable - if (parameters_->search_priority == "efficient_path") { + if (parameters_->path_priority == "efficient_path") { for (const auto & planner : pull_over_planners_) { for (const auto & goal_candidate : goal_candidates) { planCandidatePaths(planner, goal_candidate); } } - } else if (parameters_->search_priority == "close_goal") { + } else if (parameters_->path_priority == "close_goal") { for (const auto & goal_candidate : goal_candidates) { for (const auto & planner : pull_over_planners_) { planCandidatePaths(planner, goal_candidate); @@ -192,16 +182,17 @@ void GoalPlannerModule::onTimer() } } else { RCLCPP_ERROR( - getLogger(), "search_priority should be efficient_path or close_goal, but %s is given.", - parameters_->search_priority.c_str()); - throw std::domain_error("[pull_over] invalid search_priority"); + getLogger(), "path_priority should be efficient_path or close_goal, but %s is given.", + parameters_->path_priority.c_str()); + throw std::domain_error("[pull_over] invalid path_priority"); } // set member variables - mutex_.lock(); - pull_over_path_candidates_ = path_candidates; - closest_start_pose_ = closest_start_pose; - mutex_.unlock(); + { + const std::lock_guard lock(mutex_); + status_.set_pull_over_path_candidates(path_candidates); + status_.set_closest_start_pose(closest_start_pose); + } } void GoalPlannerModule::onFreespaceParkingTimer() @@ -212,6 +203,14 @@ void GoalPlannerModule::onFreespaceParkingTimer() if (!planner_data_->costmap) { return; } + // fixed goal planner do not use freespace planner + if (!goal_planner_utils::isAllowedGoalModification(planner_data_->route_handler)) { + return; + } + + if (isOnModifiedGoal()) { + return; + } const bool is_new_costmap = (clock_->now() - planner_data_->costmap->header.stamp).seconds() < 1.0; @@ -248,12 +247,29 @@ void GoalPlannerModule::initializeOccupancyGridMap() occupancy_grid_map_->setParam(occupancy_grid_map_param); } +void GoalPlannerModule::initializeSafetyCheckParameters() +{ + utils::start_goal_planner_common::updateEgoPredictedPathParams( + ego_predicted_path_params_, parameters_); + utils::start_goal_planner_common::updateSafetyCheckParams(safety_check_params_, parameters_); + utils::start_goal_planner_common::updateObjectsFilteringParams( + objects_filtering_params_, parameters_); +} + void GoalPlannerModule::processOnEntry() { // Initialize occupancy grid map - if (parameters_->use_occupancy_grid) { + if ( + parameters_->use_occupancy_grid_for_goal_search || + parameters_->use_occupancy_grid_for_path_collision_check) { initializeOccupancyGridMap(); } + // Initialize safety checker + if (parameters_->safety_check_params.enable_safety_check) { + initializeSafetyCheckParameters(); + utils::start_goal_planner_common::initializeCollisionCheckDebugMap( + goal_planner_data_.collision_check); + } } void GoalPlannerModule::processOnExit() @@ -271,11 +287,7 @@ bool GoalPlannerModule::isExecutionRequested() const const auto & route_handler = planner_data_->route_handler; const Pose & current_pose = planner_data_->self_odometry->pose.pose; - const Pose & goal_pose = route_handler->getGoalPose(); - - // if goal is shoulder lane, allow goal modification - allow_goal_modification_ = - route_handler->isAllowedGoalModification() || checkOriginalGoalIsInShoulder(); + const Pose goal_pose = route_handler->getOriginalGoalPose(); // check if goal_pose is in current_lanes. lanelet::ConstLanelet current_lane{}; @@ -306,13 +318,19 @@ bool GoalPlannerModule::isExecutionRequested() const return false; } + // if goal modification is not allowed + // 1) goal_pose is in current_lanes, plan path to the original fixed goal + // 2) goal_pose is NOT in current_lanes, do not execute goal_planner + if (!goal_planner_utils::isAllowedGoalModification(route_handler)) { + return goal_is_in_current_lanes; + } + // if goal arc coordinates can be calculated, check if goal is in request_length const double self_to_goal_arc_length = utils::getSignedDistance(current_pose, goal_pose, current_lanes); - const double request_length = - goal_planner_utils::isAllowedGoalModification(route_handler, left_side_parking_) - ? calcModuleRequestLength() - : parameters_->minimum_request_length; + const double request_length = goal_planner_utils::isAllowedGoalModification(route_handler) + ? calcModuleRequestLength() + : parameters_->pull_over_minimum_request_length; if (self_to_goal_arc_length < 0.0 || self_to_goal_arc_length > request_length) { // if current position is far from goal or behind goal, do not execute goal_planner return false; @@ -321,15 +339,16 @@ bool GoalPlannerModule::isExecutionRequested() const // if goal modification is not allowed // 1) goal_pose is in current_lanes, plan path to the original fixed goal // 2) goal_pose is NOT in current_lanes, do not execute goal_planner - if (!goal_planner_utils::isAllowedGoalModification(route_handler, left_side_parking_)) { + if (!goal_planner_utils::isAllowedGoalModification(route_handler)) { return goal_is_in_current_lanes; } // if (A) or (B) is met execute pull over // (A) target lane is `road` and same to the current lanes // (B) target lane is `road_shoulder` and neighboring to the current lanes - const lanelet::ConstLanelets pull_over_lanes = - goal_planner_utils::getPullOverLanes(*(route_handler), left_side_parking_); + const lanelet::ConstLanelets pull_over_lanes = goal_planner_utils::getPullOverLanes( + *(route_handler), left_side_parking_, parameters_->backward_goal_search_length, + parameters_->forward_goal_search_length); lanelet::ConstLanelet target_lane{}; lanelet::utils::query::getClosestLanelet(pull_over_lanes, goal_pose, &target_lane); if (!isCrossingPossible(current_lane, target_lane)) { @@ -341,26 +360,38 @@ bool GoalPlannerModule::isExecutionRequested() const bool GoalPlannerModule::isExecutionReady() const { + if (parameters_->safety_check_params.enable_safety_check && isWaitingApproval()) { + if (!isSafePath()) { + RCLCPP_ERROR_THROTTLE(getLogger(), *clock_, 5000, "Path is not safe against dynamic objects"); + return false; + } + } return true; } double GoalPlannerModule::calcModuleRequestLength() const { - const auto min_stop_distance = calcFeasibleDecelDistance(0.0); + const auto min_stop_distance = calcFeasibleDecelDistance( + planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk, 0.0); if (!min_stop_distance) { - return parameters_->minimum_request_length; + return parameters_->pull_over_minimum_request_length; } const double minimum_request_length = *min_stop_distance + parameters_->backward_goal_search_length + approximate_pull_over_distance_; - return std::max(minimum_request_length, parameters_->minimum_request_length); + return std::max(minimum_request_length, parameters_->pull_over_minimum_request_length); } Pose GoalPlannerModule::calcRefinedGoal(const Pose & goal_pose) const { - const lanelet::ConstLanelets pull_over_lanes = - goal_planner_utils::getPullOverLanes(*(planner_data_->route_handler), left_side_parking_); + const double vehicle_width = planner_data_->parameters.vehicle_width; + const double base_link2front = planner_data_->parameters.base_link2front; + const double base_link2rear = planner_data_->parameters.base_link2rear; + + const lanelet::ConstLanelets pull_over_lanes = goal_planner_utils::getPullOverLanes( + *(planner_data_->route_handler), left_side_parking_, parameters_->backward_goal_search_length, + parameters_->forward_goal_search_length); lanelet::Lanelet closest_pull_over_lanelet{}; lanelet::utils::query::getClosestLanelet(pull_over_lanes, goal_pose, &closest_pull_over_lanelet); @@ -390,15 +421,18 @@ Pose GoalPlannerModule::calcRefinedGoal(const Pose & goal_pose) const center_pose.orientation = tf2::toMsg(tf_quat); } - const auto distance_from_left_bound = utils::getSignedDistanceFromBoundary( - pull_over_lanes, vehicle_footprint_, center_pose, left_side_parking_); - if (!distance_from_left_bound) { + const auto distance_from_bound = utils::getSignedDistanceFromBoundary( + pull_over_lanes, vehicle_width, base_link2front, base_link2rear, center_pose, + left_side_parking_); + if (!distance_from_bound) { RCLCPP_ERROR(getLogger(), "fail to calculate refined goal"); return goal_pose; } + const double sign = left_side_parking_ ? -1.0 : 1.0; const double offset_from_center_line = - distance_from_left_bound.value() + parameters_->margin_from_boundary; + sign * (distance_from_bound.value() + parameters_->margin_from_boundary); + const auto refined_goal_pose = calcOffsetPose(center_pose, 0, -offset_from_center_line, 0); return refined_goal_pose; @@ -406,14 +440,6 @@ Pose GoalPlannerModule::calcRefinedGoal(const Pose & goal_pose) const ModuleStatus GoalPlannerModule::updateState() { - // finish module only when the goal is fixed - if ( - !goal_planner_utils::isAllowedGoalModification( - planner_data_->route_handler, left_side_parking_) && - hasFinishedGoalPlanner()) { - return ModuleStatus::SUCCESS; - } - // start_planner module will be run when setting new goal, so not need finishing pull_over module. // Finishing it causes wrong lane_following path generation. return current_state_; @@ -421,18 +447,23 @@ ModuleStatus GoalPlannerModule::updateState() bool GoalPlannerModule::planFreespacePath() { - mutex_.lock(); - goal_searcher_->update(goal_candidates_); - const auto goal_candidates = goal_candidates_; - debug_data_.freespace_planner.num_goal_candidates = goal_candidates.size(); - debug_data_.freespace_planner.is_planning = true; - mutex_.unlock(); + goal_searcher_->setPlannerData(planner_data_); + auto goal_candidates = status_.get_goal_candidates(); + goal_searcher_->update(goal_candidates); + status_.set_goal_candidates(goal_candidates); + + { + const std::lock_guard lock(mutex_); + debug_data_.freespace_planner.num_goal_candidates = goal_candidates.size(); + debug_data_.freespace_planner.is_planning = true; + } for (size_t i = 0; i < goal_candidates.size(); i++) { const auto goal_candidate = goal_candidates.at(i); - mutex_.lock(); - debug_data_.freespace_planner.current_goal_idx = i; - mutex_.unlock(); + { + const std::lock_guard lock(mutex_); + debug_data_.freespace_planner.current_goal_idx = i; + } if (!goal_candidate.is_safe) { continue; @@ -443,17 +474,21 @@ bool GoalPlannerModule::planFreespacePath() if (!freespace_path) { continue; } - mutex_.lock(); - status_.pull_over_path = std::make_shared(*freespace_path); - status_.current_path_idx = 0; - status_.is_safe = true; - modified_goal_pose_ = goal_candidate; - last_path_update_time_ = std::make_unique(clock_->now()); - debug_data_.freespace_planner.is_planning = false; - mutex_.unlock(); + + { + const std::lock_guard lock(mutex_); + status_.set_pull_over_path(std::make_shared(*freespace_path)); + status_.set_current_path_idx(0); + status_.set_is_safe_static_objects(true); + status_.set_modified_goal_pose(goal_candidate); + status_.set_last_path_update_time(std::make_shared(clock_->now())); + debug_data_.freespace_planner.is_planning = false; + } + return true; } + const std::lock_guard lock(mutex_); debug_data_.freespace_planner.is_planning = false; return false; } @@ -465,11 +500,11 @@ void GoalPlannerModule::returnToLaneParking() return; } - if (!status_.lane_parking_pull_over_path) { + if (!status_.get_lane_parking_pull_over_path()) { return; } - const PathWithLaneId path = status_.lane_parking_pull_over_path->getFullPath(); + const PathWithLaneId path = status_.get_lane_parking_pull_over_path()->getFullPath(); if (checkCollision(path)) { return; } @@ -482,13 +517,14 @@ void GoalPlannerModule::returnToLaneParking() return; } - mutex_.lock(); - status_.is_safe = true; - status_.has_decided_path = false; - status_.pull_over_path = status_.lane_parking_pull_over_path; - status_.current_path_idx = 0; - last_path_update_time_ = std::make_unique(clock_->now()); - mutex_.unlock(); + { + const std::lock_guard lock(mutex_); + status_.set_is_safe_static_objects(true); + status_.set_has_decided_path(false); + status_.set_pull_over_path(status_.get_lane_parking_pull_over_path()); + status_.set_current_path_idx(0); + status_.set_last_path_update_time(std::make_shared(clock_->now())); + } RCLCPP_INFO(getLogger(), "return to lane parking"); } @@ -500,30 +536,37 @@ void GoalPlannerModule::generateGoalCandidates() // with old architecture, module instance is not cleared when new route is received // so need to reset status here. // todo: move this check out of this function after old architecture is removed - if (!goal_candidates_.empty()) { + if (!status_.get_goal_candidates().empty()) { return; } // calculate goal candidates - const Pose goal_pose = route_handler->getGoalPose(); - refined_goal_pose_ = calcRefinedGoal(goal_pose); - if (goal_planner_utils::isAllowedGoalModification(route_handler, left_side_parking_)) { + const Pose goal_pose = route_handler->getOriginalGoalPose(); + status_.set_refined_goal_pose(calcRefinedGoal(goal_pose)); + if (goal_planner_utils::isAllowedGoalModification(route_handler)) { goal_searcher_->setPlannerData(planner_data_); - goal_candidates_ = goal_searcher_->search(refined_goal_pose_); + goal_searcher_->setReferenceGoal(status_.get_refined_goal_pose()); + status_.set_goal_candidates(goal_searcher_->search()); } else { GoalCandidate goal_candidate{}; goal_candidate.goal_pose = goal_pose; goal_candidate.distance_from_original_goal = 0.0; - goal_candidates_.push_back(goal_candidate); + GoalCandidates goal_candidates{}; + goal_candidates.push_back(goal_candidate); + status_.set_goal_candidates(goal_candidates); } } BehaviorModuleOutput GoalPlannerModule::plan() { + resetPathCandidate(); + resetPathReference(); + generateGoalCandidates(); - if (goal_planner_utils::isAllowedGoalModification( - planner_data_->route_handler, left_side_parking_)) { + path_reference_ = getPreviousModuleOutput().reference_path; + + if (goal_planner_utils::isAllowedGoalModification(planner_data_->route_handler)) { return planWithGoalModification(); } else { fixed_goal_planner_->setPreviousModuleOutput(getPreviousModuleOutput()); @@ -531,16 +574,57 @@ BehaviorModuleOutput GoalPlannerModule::plan() } } +std::vector GoalPlannerModule::sortPullOverPathCandidatesByGoalPriority( + const std::vector & pull_over_path_candidates, + const GoalCandidates & goal_candidates) const +{ + auto sorted_pull_over_path_candidates = pull_over_path_candidates; + + // Create a map of goal_id to its index in goal_candidates + std::map goal_id_to_index; + for (size_t i = 0; i < goal_candidates.size(); ++i) { + goal_id_to_index[goal_candidates[i].id] = i; + } + + // Sort pull_over_path_candidates based on the order in goal_candidates + std::stable_sort( + sorted_pull_over_path_candidates.begin(), sorted_pull_over_path_candidates.end(), + [&goal_id_to_index](const auto & a, const auto & b) { + return goal_id_to_index[a.goal_id] < goal_id_to_index[b.goal_id]; + }); + + // Sort pull_over_path_candidates based on the order in efficient_path_order + if (parameters_->path_priority == "efficient_path") { + std::stable_sort( + sorted_pull_over_path_candidates.begin(), sorted_pull_over_path_candidates.end(), + [this](const auto & a, const auto & b) { + const auto & order = parameters_->efficient_path_order; + const auto a_pos = std::find(order.begin(), order.end(), magic_enum::enum_name(a.type)); + const auto b_pos = std::find(order.begin(), order.end(), magic_enum::enum_name(b.type)); + return a_pos < b_pos; + }); + } + + return sorted_pull_over_path_candidates; +} + void GoalPlannerModule::selectSafePullOverPath() { // select safe lane pull over path from candidates - mutex_.lock(); - goal_searcher_->setPlannerData(planner_data_); - goal_searcher_->update(goal_candidates_); - const auto pull_over_path_candidates = pull_over_path_candidates_; - const auto goal_candidates = goal_candidates_; - mutex_.unlock(); - status_.is_safe = false; + std::vector pull_over_path_candidates{}; + GoalCandidates goal_candidates{}; + { + const std::lock_guard lock(mutex_); + goal_searcher_->setPlannerData(planner_data_); + goal_candidates = status_.get_goal_candidates(); + goal_searcher_->update(goal_candidates); + status_.set_goal_candidates(goal_candidates); + status_.set_pull_over_path_candidates(sortPullOverPathCandidatesByGoalPriority( + status_.get_pull_over_path_candidates(), status_.get_goal_candidates())); + pull_over_path_candidates = status_.get_pull_over_path_candidates(); + status_.set_is_safe_static_objects(false); + } + for (const auto & pull_over_path : pull_over_path_candidates) { // check if goal is safe const auto goal_candidate_it = std::find_if( @@ -559,68 +643,79 @@ void GoalPlannerModule::selectSafePullOverPath() continue; } - status_.is_safe = true; - mutex_.lock(); - status_.pull_over_path = std::make_shared(pull_over_path); - status_.current_path_idx = 0; - status_.lane_parking_pull_over_path = status_.pull_over_path; - modified_goal_pose_ = *goal_candidate_it; - last_path_update_time_ = std::make_unique(clock_->now()); - mutex_.unlock(); + // found safe pull over path + { + const std::lock_guard lock(mutex_); + status_.set_is_safe_static_objects(true); + status_.set_pull_over_path(std::make_shared(pull_over_path)); + status_.set_current_path_idx(0); + status_.set_lane_parking_pull_over_path(status_.get_pull_over_path()); + status_.set_modified_goal_pose(*goal_candidate_it); + status_.set_last_path_update_time(std::make_shared(clock_->now())); + } break; } + if (!status_.get_is_safe_static_objects()) { + return; + } + // decelerate before the search area start - if (status_.is_safe) { - const auto search_start_offset_pose = calcLongitudinalOffsetPose( - status_.pull_over_path->getFullPath().points, refined_goal_pose_.position, - -parameters_->backward_goal_search_length - planner_data_->parameters.base_link2front - - approximate_pull_over_distance_); - auto & first_path = status_.pull_over_path->partial_paths.front(); - - if (search_start_offset_pose) { - decelerateBeforeSearchStart(*search_start_offset_pose, first_path); - } else { - // if already passed the search start offset pose, set pull_over_velocity to first_path. - const auto min_decel_distance = calcFeasibleDecelDistance(parameters_->pull_over_velocity); - for (auto & p : first_path.points) { - const double distance_from_ego = calcSignedArcLengthFromEgo(first_path, p.point.pose); - if (min_decel_distance && distance_from_ego < *min_decel_distance) { - continue; - } - p.point.longitudinal_velocity_mps = std::min( - p.point.longitudinal_velocity_mps, static_cast(parameters_->pull_over_velocity)); + const auto search_start_offset_pose = calcLongitudinalOffsetPose( + status_.get_pull_over_path()->getFullPath().points, status_.get_refined_goal_pose().position, + -parameters_->backward_goal_search_length - planner_data_->parameters.base_link2front - + approximate_pull_over_distance_); + auto & first_path = status_.get_pull_over_path()->partial_paths.front(); + if (search_start_offset_pose) { + decelerateBeforeSearchStart(*search_start_offset_pose, first_path); + } else { + // if already passed the search start offset pose, set pull_over_velocity to first_path. + const auto min_decel_distance = calcFeasibleDecelDistance( + planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk, + parameters_->pull_over_velocity); + for (auto & p : first_path.points) { + const double distance_from_ego = calcSignedArcLengthFromEgo(first_path, p.point.pose); + if (min_decel_distance && distance_from_ego < *min_decel_distance) { + continue; } + p.point.longitudinal_velocity_mps = std::min( + p.point.longitudinal_velocity_mps, static_cast(parameters_->pull_over_velocity)); } } - - // generate drivable area for each partial path - for (auto & path : status_.pull_over_path->partial_paths) { - const size_t ego_idx = planner_data_->findEgoIndex(path.points); - utils::clipPathLength(path, ego_idx, planner_data_->parameters); - } } void GoalPlannerModule::setLanes() { - status_.current_lanes = utils::getExtendedCurrentLanes( + const std::lock_guard lock(mutex_); + status_.set_current_lanes(utils::getExtendedCurrentLanes( planner_data_, parameters_->backward_goal_search_length, parameters_->forward_goal_search_length, - /*forward_only_in_route*/ false); - status_.pull_over_lanes = - goal_planner_utils::getPullOverLanes(*(planner_data_->route_handler), left_side_parking_); - status_.lanes = - utils::generateDrivableLanesWithShoulderLanes(status_.current_lanes, status_.pull_over_lanes); + /*forward_only_in_route*/ false)); + status_.set_pull_over_lanes(goal_planner_utils::getPullOverLanes( + *(planner_data_->route_handler), left_side_parking_, parameters_->backward_goal_search_length, + parameters_->forward_goal_search_length)); + status_.set_lanes(utils::generateDrivableLanesWithShoulderLanes( + status_.get_current_lanes(), status_.get_pull_over_lanes())); } void GoalPlannerModule::setOutput(BehaviorModuleOutput & output) { - if (status_.is_safe) { - // clear stop pose when the path is safe and activated + if (!status_.get_is_safe_static_objects()) { + // situation : not safe against static objects use stop_path + setStopPath(output); + } else if ( + parameters_->safety_check_params.enable_safety_check && !isSafePath() && + status_.get_has_decided_path() && isActivated()) { + // situation : not safe against dynamic objects after approval + // insert stop point in current path if ego is able to stop with acceleration and jerk + // constraints + setStopPathFromCurrentPath(output); + } else { + // situation : (safe against static and dynamic objects) or (safe against static objects and + // before approval) don't stop if (isActivated()) { resetWallPoses(); } - // keep stop if not enough time passed, // because it takes time for the trajectory to be reflected auto current_path = getCurrentPath(); @@ -628,9 +723,6 @@ void GoalPlannerModule::setOutput(BehaviorModuleOutput & output) output.path = std::make_shared(current_path); output.reference_path = getPreviousModuleOutput().reference_path; - } else { - // not safe: use stop_path - setStopPath(output); } setDrivableAreaInfo(output); @@ -638,50 +730,80 @@ void GoalPlannerModule::setOutput(BehaviorModuleOutput & output) setModifiedGoal(output); // set hazard and turn signal - if (status_.has_decided_path) { + if (status_.get_has_decided_path()) { setTurnSignalInfo(output); } // for the next loop setOutput(). // this is used to determine whether to generate a new stop path or keep the current stop path. - status_.prev_is_safe = status_.is_safe; + const std::lock_guard lock(mutex_); + status_.set_prev_is_safe(status_.get_is_safe_static_objects()); + status_.set_prev_is_safe_dynamic_objects( + parameters_->safety_check_params.enable_safety_check ? isSafePath() : true); } void GoalPlannerModule::setStopPath(BehaviorModuleOutput & output) { - if (status_.prev_is_safe || status_.prev_stop_path == nullptr) { + if (status_.get_prev_is_safe() || !status_.get_prev_stop_path()) { // safe -> not_safe or no prev_stop_path: generate new stop_path output.path = std::make_shared(generateStopPath()); - output.reference_path = getPreviousModuleOutput().reference_path; - status_.prev_stop_path = output.path; + const std::lock_guard lock(mutex_); + status_.set_prev_stop_path(output.path); // set stop path as pull over path - mutex_.lock(); - PullOverPath pull_over_path{}; - status_.pull_over_path = std::make_shared(pull_over_path); - status_.current_path_idx = 0; - status_.pull_over_path->partial_paths.push_back(*output.path); - last_path_update_time_ = std::make_unique(clock_->now()); - mutex_.unlock(); + auto stop_pull_over_path = std::make_shared(); + stop_pull_over_path->partial_paths.push_back(*output.path); + status_.set_pull_over_path(stop_pull_over_path); + status_.set_current_path_idx(0); + status_.set_last_path_update_time(std::make_shared(clock_->now())); + RCLCPP_WARN_THROTTLE( getLogger(), *clock_, 5000, "Not found safe pull_over path, generate stop path"); } else { // not_safe -> not_safe: use previous stop path - output.path = status_.prev_stop_path; - output.reference_path = getPreviousModuleOutput().reference_path; + output.path = status_.get_prev_stop_path(); RCLCPP_WARN_THROTTLE( getLogger(), *clock_, 5000, "Not found safe pull_over path, use previous stop path"); } + output.reference_path = getPreviousModuleOutput().reference_path; +} + +void GoalPlannerModule::setStopPathFromCurrentPath(BehaviorModuleOutput & output) +{ + // safe or not safe(no feasible stop_path found) -> not_safe: try to find new feasible stop_path + if (status_.get_prev_is_safe_dynamic_objects() || !status_.get_prev_stop_path_after_approval()) { + auto current_path = getCurrentPath(); + const auto stop_path = + behavior_path_planner::utils::start_goal_planner_common::generateFeasibleStopPath( + current_path, planner_data_, *stop_pose_, parameters_->maximum_deceleration_for_stop, + parameters_->maximum_jerk_for_stop); + if (stop_path) { + output.path = std::make_shared(*stop_path); + status_.set_prev_stop_path_after_approval(output.path); + RCLCPP_WARN_THROTTLE(getLogger(), *clock_, 5000, "Collision detected, generate stop path"); + } else { + output.path = std::make_shared(getCurrentPath()); + RCLCPP_WARN_THROTTLE( + getLogger(), *clock_, 5000, + "Collision detected, no feasible stop path found, cannot stop."); + } + status_.set_last_path_update_time(std::make_shared(clock_->now())); + } else { + // not_safe safe(no feasible stop path found) -> not_safe: use previous stop path + output.path = status_.get_prev_stop_path_after_approval(); + RCLCPP_WARN_THROTTLE(getLogger(), *clock_, 5000, "Collision detected, use previous stop path"); + } + output.reference_path = getPreviousModuleOutput().reference_path; } void GoalPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) const { - if (status_.pull_over_path->type == PullOverPlannerType::FREESPACE) { + if (status_.get_pull_over_path()->type == PullOverPlannerType::FREESPACE) { const double drivable_area_margin = planner_data_->parameters.vehicle_width; output.drivable_area_info.drivable_margin = planner_data_->parameters.vehicle_width / 2.0 + drivable_area_margin; } else { const auto target_drivable_lanes = utils::getNonOverlappingExpandedLanes( - *output.path, status_.lanes, planner_data_->drivable_area_expansion_parameters); + *output.path, status_.get_lanes(), planner_data_->drivable_area_expansion_parameters); DrivableAreaInfo current_drivable_area_info; current_drivable_area_info.drivable_lanes = target_drivable_lanes; @@ -693,10 +815,10 @@ void GoalPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) const void GoalPlannerModule::setModifiedGoal(BehaviorModuleOutput & output) const { const auto & route_handler = planner_data_->route_handler; - if (status_.is_safe) { + if (status_.get_is_safe_static_objects()) { PoseWithUuidStamped modified_goal{}; modified_goal.uuid = route_handler->getRouteUuid(); - modified_goal.pose = modified_goal_pose_->goal_pose; + modified_goal.pose = status_.get_modified_goal_pose()->goal_pose; modified_goal.header = route_handler->getRouteHeader(); output.modified_goal = modified_goal; } else { @@ -736,12 +858,12 @@ void GoalPlannerModule::updateSteeringFactor( bool GoalPlannerModule::hasDecidedPath() const { // once decided, keep the decision - if (status_.has_decided_path) { + if (status_.get_has_decided_path()) { return true; } // if path is not safe, not decided - if (!status_.is_safe) { + if (!status_.get_is_safe_static_objects()) { return false; } @@ -753,10 +875,10 @@ bool GoalPlannerModule::hasDecidedPath() const return false; } const size_t start_pose_segment_idx = motion_utils::findNearestSegmentIndex( - getCurrentPath().points, status_.pull_over_path->start_pose.position); + getCurrentPath().points, status_.get_pull_over_path()->start_pose.position); const double dist_to_parking_start_pose = calcSignedArcLength( getCurrentPath().points, current_pose.position, *ego_segment_idx, - status_.pull_over_path->start_pose.position, start_pose_segment_idx); + status_.get_pull_over_path()->start_pose.position, start_pose_segment_idx); return dist_to_parking_start_pose < parameters_->decide_path_distance; } @@ -765,15 +887,15 @@ void GoalPlannerModule::decideVelocity() const double current_vel = planner_data_->self_odometry->twist.twist.linear.x; // decide velocity to guarantee turn signal lighting time - if (!status_.has_decided_velocity) { - auto & first_path = status_.pull_over_path->partial_paths.front(); + if (!status_.get_has_decided_velocity()) { + auto & first_path = status_.get_pull_over_path()->partial_paths.front(); const auto vel = static_cast(std::max(current_vel, parameters_->pull_over_minimum_velocity)); for (auto & p : first_path.points) { p.point.longitudinal_velocity_mps = std::min(p.point.longitudinal_velocity_mps, vel); } } - status_.has_decided_velocity = true; + status_.set_has_decided_velocity(true); } BehaviorModuleOutput GoalPlannerModule::planWithGoalModification() @@ -787,21 +909,26 @@ BehaviorModuleOutput GoalPlannerModule::planWithGoalModification() setLanes(); // Check if it needs to decide path - status_.has_decided_path = hasDecidedPath(); + status_.set_has_decided_path(hasDecidedPath()); // Use decided path - if (status_.has_decided_path) { + if (status_.get_has_decided_path()) { if (isActivated() && isWaitingApproval()) { - last_approved_time_ = std::make_unique(clock_->now()); - last_approved_pose_ = std::make_unique(planner_data_->self_odometry->pose.pose); + { + const std::lock_guard lock(mutex_); + status_.set_last_approved_time(std::make_shared(clock_->now())); + status_.set_last_approved_pose( + std::make_shared(planner_data_->self_odometry->pose.pose)); + } clearWaitingApproval(); decideVelocity(); } transitionToNextPathIfFinishingCurrentPath(); - } else if (!pull_over_path_candidates_.empty() && needPathUpdate(path_update_duration)) { + } else if ( + !status_.get_pull_over_path_candidates().empty() && needPathUpdate(path_update_duration)) { // if the final path is not decided and enough time has passed since last path update, // select safe path from lane parking pull over path candidates - // and set it to status_.pull_over_path + // and set it to status_.get_pull_over_path() selectSafePullOverPath(); } // else: stop path is generated and set by setOutput() @@ -809,21 +936,21 @@ BehaviorModuleOutput GoalPlannerModule::planWithGoalModification() // set output and status BehaviorModuleOutput output{}; setOutput(output); - path_candidate_ = std::make_shared(status_.pull_over_path->getFullPath()); + path_candidate_ = std::make_shared(status_.get_pull_over_path()->getFullPath()); path_reference_ = getPreviousModuleOutput().reference_path; // return to lane parking if it is possible - if (status_.pull_over_path->type == PullOverPlannerType::FREESPACE) { + if (status_.get_pull_over_path()->type == PullOverPlannerType::FREESPACE) { returnToLaneParking(); } const auto distance_to_path_change = calcDistanceToPathChange(); - if (status_.has_decided_path) { + if (status_.get_has_decided_path()) { updateRTCStatus(distance_to_path_change.first, distance_to_path_change.second); } // TODO(tkhmy) add handle status TRYING updateSteeringFactor( - {status_.pull_over_path->start_pose, modified_goal_pose_->goal_pose}, + {status_.get_pull_over_path()->start_pose, status_.get_modified_goal_pose()->goal_pose}, {distance_to_path_change.first, distance_to_path_change.second}, SteeringFactor::TURNING); // For debug @@ -833,15 +960,19 @@ BehaviorModuleOutput GoalPlannerModule::planWithGoalModification() printParkingPositionError(); } - setStopReason(StopReason::GOAL_PLANNER, status_.pull_over_path->getFullPath()); + setStopReason(StopReason::GOAL_PLANNER, status_.get_pull_over_path()->getFullPath()); return output; } BehaviorModuleOutput GoalPlannerModule::planWaitingApproval() { - if (goal_planner_utils::isAllowedGoalModification( - planner_data_->route_handler, left_side_parking_)) { + resetPathCandidate(); + resetPathReference(); + + path_reference_ = getPreviousModuleOutput().reference_path; + + if (goal_planner_utils::isAllowedGoalModification(planner_data_->route_handler)) { return planWaitingApprovalWithGoalModification(); } else { fixed_goal_planner_->setPreviousModuleOutput(getPreviousModuleOutput()); @@ -858,20 +989,21 @@ BehaviorModuleOutput GoalPlannerModule::planWaitingApprovalWithGoalModification( out.modified_goal = plan().modified_goal; // update status_ out.path = std::make_shared(generateStopPath()); out.reference_path = getPreviousModuleOutput().reference_path; - path_candidate_ = status_.is_safe - ? std::make_shared(status_.pull_over_path->getFullPath()) - : out.path; + path_candidate_ = + status_.get_is_safe_static_objects() + ? std::make_shared(status_.get_pull_over_path()->getFullPath()) + : out.path; path_reference_ = getPreviousModuleOutput().reference_path; const auto distance_to_path_change = calcDistanceToPathChange(); // generate drivable area info for new architecture - if (status_.pull_over_path->type == PullOverPlannerType::FREESPACE) { + if (status_.get_pull_over_path()->type == PullOverPlannerType::FREESPACE) { const double drivable_area_margin = planner_data_->parameters.vehicle_width; out.drivable_area_info.drivable_margin = planner_data_->parameters.vehicle_width / 2.0 + drivable_area_margin; } else { const auto target_drivable_lanes = utils::getNonOverlappingExpandedLanes( - *out.path, status_.lanes, planner_data_->drivable_area_expansion_parameters); + *out.path, status_.get_lanes(), planner_data_->drivable_area_expansion_parameters); DrivableAreaInfo current_drivable_area_info; current_drivable_area_info.drivable_lanes = target_drivable_lanes; @@ -879,21 +1011,21 @@ BehaviorModuleOutput GoalPlannerModule::planWaitingApprovalWithGoalModification( current_drivable_area_info, getPreviousModuleOutput().drivable_area_info); } - if (status_.has_decided_path) { + if (status_.get_has_decided_path()) { updateRTCStatus(distance_to_path_change.first, distance_to_path_change.second); } updateSteeringFactor( - {status_.pull_over_path->start_pose, modified_goal_pose_->goal_pose}, + {status_.get_pull_over_path()->start_pose, status_.get_modified_goal_pose()->goal_pose}, {distance_to_path_change.first, distance_to_path_change.second}, SteeringFactor::APPROACHING); - setStopReason(StopReason::GOAL_PLANNER, status_.pull_over_path->getFullPath()); + setStopReason(StopReason::GOAL_PLANNER, status_.get_pull_over_path()->getFullPath()); return out; } std::pair GoalPlannerModule::calcDistanceToPathChange() const { - const auto & full_path = status_.pull_over_path->getFullPath(); + const auto & full_path = status_.get_pull_over_path()->getFullPath(); const auto ego_segment_idx = motion_utils::findNearestSegmentIndex( full_path.points, planner_data_->self_odometry->pose.pose, std::numeric_limits::max(), @@ -903,15 +1035,15 @@ std::pair GoalPlannerModule::calcDistanceToPathChange() const } const size_t start_pose_segment_idx = motion_utils::findNearestSegmentIndex( - full_path.points, status_.pull_over_path->start_pose.position); + full_path.points, status_.get_pull_over_path()->start_pose.position); const double dist_to_parking_start_pose = calcSignedArcLength( full_path.points, planner_data_->self_odometry->pose.pose.position, *ego_segment_idx, - status_.pull_over_path->start_pose.position, start_pose_segment_idx); + status_.get_pull_over_path()->start_pose.position, start_pose_segment_idx); const size_t goal_pose_segment_idx = motion_utils::findNearestSegmentIndex( - full_path.points, modified_goal_pose_->goal_pose.position); + full_path.points, status_.get_modified_goal_pose()->goal_pose.position); const double dist_to_parking_finish_pose = calcSignedArcLength( full_path.points, planner_data_->self_odometry->pose.pose.position, *ego_segment_idx, - modified_goal_pose_->goal_pose.position, goal_pose_segment_idx); + status_.get_modified_goal_pose()->goal_pose.position, goal_pose_segment_idx); return {dist_to_parking_start_pose, dist_to_parking_finish_pose}; } @@ -929,17 +1061,17 @@ PathWithLaneId GoalPlannerModule::generateStopPath() const double current_vel = planner_data_->self_odometry->twist.twist.linear.x; const double pull_over_velocity = parameters_->pull_over_velocity; - if (status_.current_lanes.empty()) { + if (status_.get_current_lanes().empty()) { return PathWithLaneId{}; } // generate reference path const auto s_current = - lanelet::utils::getArcCoordinates(status_.current_lanes, current_pose).length; + lanelet::utils::getArcCoordinates(status_.get_current_lanes(), current_pose).length; const double s_start = std::max(0.0, s_current - common_parameters.backward_path_length); const double s_end = s_current + common_parameters.forward_path_length; auto reference_path = - route_handler->getCenterLinePath(status_.current_lanes, s_start, s_end, true); + route_handler->getCenterLinePath(status_.get_current_lanes(), s_start, s_end, true); // if not approved stop road lane. // stop point priority is @@ -950,20 +1082,25 @@ PathWithLaneId GoalPlannerModule::generateStopPath() // difference between the outer and inner sides) // 4. feasible stop const auto search_start_offset_pose = calcLongitudinalOffsetPose( - reference_path.points, refined_goal_pose_.position, + reference_path.points, status_.get_refined_goal_pose().position, -parameters_->backward_goal_search_length - common_parameters.base_link2front - approximate_pull_over_distance_); - if (!status_.is_safe && !closest_start_pose_ && !search_start_offset_pose) { + if ( + !status_.get_is_safe_static_objects() && !status_.get_closest_start_pose() && + !search_start_offset_pose) { return generateFeasibleStopPath(); } - const Pose stop_pose = status_.is_safe ? status_.pull_over_path->start_pose - : (closest_start_pose_ ? closest_start_pose_.value() - : *search_start_offset_pose); + const Pose stop_pose = + status_.get_is_safe_static_objects() + ? status_.get_pull_over_path()->start_pose + : (status_.get_closest_start_pose() ? status_.get_closest_start_pose().value() + : *search_start_offset_pose); // if stop pose is closer than min_stop_distance, stop as soon as possible const double ego_to_stop_distance = calcSignedArcLengthFromEgo(reference_path, stop_pose); - const auto min_stop_distance = calcFeasibleDecelDistance(0.0); + const auto min_stop_distance = calcFeasibleDecelDistance( + planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk, 0.0); const double eps_vel = 0.01; const bool is_stopped = std::abs(current_vel) < eps_vel; const double buffer = is_stopped ? stop_distance_buffer_ : 0.0; @@ -980,7 +1117,9 @@ PathWithLaneId GoalPlannerModule::generateStopPath() decelerateBeforeSearchStart(*search_start_offset_pose, reference_path); } else { // if already passed the search start offset pose, set pull_over_velocity to reference_path. - const auto min_decel_distance = calcFeasibleDecelDistance(pull_over_velocity); + const auto min_decel_distance = calcFeasibleDecelDistance( + planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk, + pull_over_velocity); for (auto & p : reference_path.points) { const double distance_from_ego = calcSignedArcLengthFromEgo(reference_path, p.point.pose); if (min_decel_distance && distance_from_ego < *min_decel_distance) { @@ -1002,13 +1141,15 @@ PathWithLaneId GoalPlannerModule::generateFeasibleStopPath() // generate stop reference path const auto s_current = - lanelet::utils::getArcCoordinates(status_.current_lanes, current_pose).length; + lanelet::utils::getArcCoordinates(status_.get_current_lanes(), current_pose).length; const double s_start = std::max(0.0, s_current - common_parameters.backward_path_length); const double s_end = s_current + common_parameters.forward_path_length; - auto stop_path = route_handler->getCenterLinePath(status_.current_lanes, s_start, s_end, true); + auto stop_path = + route_handler->getCenterLinePath(status_.get_current_lanes(), s_start, s_end, true); // calc minimum stop distance under maximum deceleration - const auto min_stop_distance = calcFeasibleDecelDistance(0.0); + const auto min_stop_distance = calcFeasibleDecelDistance( + planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk, 0.0); if (!min_stop_distance) { return stop_path; } @@ -1025,15 +1166,16 @@ PathWithLaneId GoalPlannerModule::generateFeasibleStopPath() void GoalPlannerModule::transitionToNextPathIfFinishingCurrentPath() { - if (isActivated() && last_approved_time_ != nullptr) { + if (isActivated() && status_.get_last_approved_time()) { // if using arc_path and finishing current_path, get next path // enough time for turn signal - const bool has_passed_enough_time = (clock_->now() - *last_approved_time_).seconds() > - planner_data_->parameters.turn_signal_search_time; + const bool has_passed_enough_time = + (clock_->now() - *status_.get_last_approved_time()).seconds() > + planner_data_->parameters.turn_signal_search_time; - if (hasFinishedCurrentPath() && has_passed_enough_time && status_.require_increment_) { + if (hasFinishedCurrentPath() && has_passed_enough_time && status_.get_require_increment()) { if (incrementPathIndex()) { - last_increment_time_ = std::make_unique(clock_->now()); + status_.set_last_increment_time(std::make_shared(clock_->now())); } } } @@ -1041,19 +1183,23 @@ void GoalPlannerModule::transitionToNextPathIfFinishingCurrentPath() bool GoalPlannerModule::incrementPathIndex() { - if (status_.current_path_idx == status_.pull_over_path->partial_paths.size() - 1) { + if (status_.get_current_path_idx() == status_.get_pull_over_path()->partial_paths.size() - 1) { return false; } - status_.current_path_idx = status_.current_path_idx + 1; + status_.set_current_path_idx(status_.get_current_path_idx() + 1); return true; } PathWithLaneId GoalPlannerModule::getCurrentPath() const { - if (status_.pull_over_path->partial_paths.size() <= status_.current_path_idx) { + if (status_.get_pull_over_path() == nullptr) { return PathWithLaneId{}; } - return status_.pull_over_path->partial_paths.at(status_.current_path_idx); + + if (status_.get_pull_over_path()->partial_paths.size() <= status_.get_current_path_idx()) { + return PathWithLaneId{}; + } + return status_.get_pull_over_path()->partial_paths.at(status_.get_current_path_idx()); } bool GoalPlannerModule::isStopped( @@ -1082,23 +1228,28 @@ bool GoalPlannerModule::isStopped( bool GoalPlannerModule::isStopped() { + const std::lock_guard lock(mutex_); return isStopped(odometry_buffer_stopped_, parameters_->th_stopped_time); } bool GoalPlannerModule::isStuck() { + if (isOnModifiedGoal()) { + return false; + } + constexpr double stuck_time = 5.0; if (!isStopped(odometry_buffer_stuck_, stuck_time)) { return false; } // not found safe path - if (!status_.is_safe) { + if (!status_.get_is_safe_static_objects()) { return true; } // any path has never been found - if (!status_.pull_over_path) { + if (!status_.get_pull_over_path()) { return false; } @@ -1107,7 +1258,7 @@ bool GoalPlannerModule::isStuck() bool GoalPlannerModule::hasFinishedCurrentPath() { - const auto & current_path_end = getCurrentPath().points.back(); + const auto current_path_end = getCurrentPath().points.back(); const auto & self_pose = planner_data_->self_odometry->pose.pose; const bool is_near_target = tier4_autoware_utils::calcDistance2d(current_path_end, self_pose) < parameters_->th_arrived_distance; @@ -1117,28 +1268,23 @@ bool GoalPlannerModule::hasFinishedCurrentPath() bool GoalPlannerModule::isOnModifiedGoal() const { - if (!modified_goal_pose_) { + if (!status_.get_modified_goal_pose()) { return false; } const Pose current_pose = planner_data_->self_odometry->pose.pose; - return calcDistance2d(current_pose, modified_goal_pose_->goal_pose) < + return calcDistance2d(current_pose, status_.get_modified_goal_pose()->goal_pose) < parameters_->th_arrived_distance; } -bool GoalPlannerModule::hasFinishedGoalPlanner() -{ - return isOnModifiedGoal() && isStopped(); -} - TurnSignalInfo GoalPlannerModule::calcTurnSignalInfo() const { TurnSignalInfo turn_signal{}; // output const auto & current_pose = planner_data_->self_odometry->pose.pose; - const auto & start_pose = status_.pull_over_path->start_pose; - const auto & end_pose = status_.pull_over_path->end_pose; - const auto full_path = status_.pull_over_path->getFullPath(); + const auto & start_pose = status_.get_pull_over_path()->start_pose; + const auto & end_pose = status_.get_pull_over_path()->end_pose; + const auto full_path = status_.get_pull_over_path()->getFullPath(); // calc TurnIndicatorsCommand { @@ -1161,7 +1307,9 @@ TurnSignalInfo GoalPlannerModule::calcTurnSignalInfo() const // ego decelerates so that current pose is the point `turn_light_on_threshold_time` seconds // before starting pull_over turn_signal.desired_start_point = - last_approved_pose_ && status_.has_decided_path ? *last_approved_pose_ : current_pose; + status_.get_last_approved_pose() && status_.get_has_decided_path() + ? *status_.get_last_approved_pose() + : current_pose; turn_signal.desired_end_point = end_pose; turn_signal.required_start_point = start_pose; turn_signal.required_end_point = end_pose; @@ -1172,27 +1320,60 @@ TurnSignalInfo GoalPlannerModule::calcTurnSignalInfo() const bool GoalPlannerModule::checkCollision(const PathWithLaneId & path) const { - if (parameters_->use_occupancy_grid || !occupancy_grid_map_) { + if (parameters_->use_occupancy_grid_for_path_collision_check && occupancy_grid_map_) { const bool check_out_of_range = false; if (occupancy_grid_map_->hasObstacleOnPath(path, check_out_of_range)) { return true; } } - - if (parameters_->use_object_recognition) { - const auto common_parameters = planner_data_->parameters; - const double base_link2front = common_parameters.base_link2front; - const double base_link2rear = common_parameters.base_link2rear; - const double vehicle_width = common_parameters.vehicle_width; - if (utils::path_safety_checker::checkCollisionWithExtraStoppingMargin( - path, *(planner_data_->dynamic_object), base_link2front, base_link2rear, vehicle_width, - parameters_->maximum_deceleration, parameters_->object_recognition_collision_check_margin, - parameters_->object_recognition_collision_check_max_extra_stopping_margin)) { - return true; - } + if (!parameters_->use_object_recognition) { + return false; } - return false; + const auto pull_over_lanes = goal_planner_utils::getPullOverLanes( + *(planner_data_->route_handler), left_side_parking_, parameters_->backward_goal_search_length, + parameters_->forward_goal_search_length); + const auto [pull_over_lane_objects, others] = + utils::path_safety_checker::separateObjectsByLanelets( + *(planner_data_->dynamic_object), pull_over_lanes, + utils::path_safety_checker::isPolygonOverlapLanelet); + const auto pull_over_lane_stop_objects = utils::path_safety_checker::filterObjectsByVelocity( + pull_over_lane_objects, parameters_->th_moving_object_velocity); + std::vector obj_polygons; + for (const auto & object : pull_over_lane_stop_objects.objects) { + obj_polygons.push_back(tier4_autoware_utils::toPolygon2d(object)); + } + + std::vector ego_polygons_expanded; + const auto curvatures = motion_utils::calcCurvature(path.points); + for (size_t i = 0; i < path.points.size(); ++i) { + const auto p = path.points.at(i); + + const double extra_stopping_margin = std::min( + std::pow(p.point.longitudinal_velocity_mps, 2) * 0.5 / parameters_->maximum_deceleration, + parameters_->object_recognition_collision_check_max_extra_stopping_margin); + + double extra_lateral_margin = (-1) * curvatures.at(i) * p.point.longitudinal_velocity_mps * + std::abs(p.point.longitudinal_velocity_mps); + extra_lateral_margin = + std::clamp(extra_lateral_margin, -extra_stopping_margin, extra_stopping_margin); + + const auto lateral_offset_pose = + tier4_autoware_utils::calcOffsetPose(p.point.pose, 0.0, extra_lateral_margin / 2.0, 0.0); + const auto ego_polygon = tier4_autoware_utils::toFootprint( + lateral_offset_pose, + planner_data_->parameters.base_link2front + + parameters_->object_recognition_collision_check_margin + extra_stopping_margin, + planner_data_->parameters.base_link2rear + + parameters_->object_recognition_collision_check_margin, + planner_data_->parameters.vehicle_width + + parameters_->object_recognition_collision_check_margin * 2.0 + + std::abs(extra_lateral_margin)); + ego_polygons_expanded.push_back(ego_polygon); + } + debug_data_.ego_polygons_expanded = ego_polygons_expanded; + + return utils::path_safety_checker::checkPolygonsIntersects(ego_polygons_expanded, obj_polygons); } bool GoalPlannerModule::hasEnoughDistance(const PullOverPath & pull_over_path) const @@ -1205,7 +1386,7 @@ bool GoalPlannerModule::hasEnoughDistance(const PullOverPath & pull_over_path) c // so need enough distance to restart. // distance to restart should be less than decide_path_distance. // otherwise, the goal would change immediately after departure. - const bool is_separated_path = status_.pull_over_path->partial_paths.size() > 1; + const bool is_separated_path = status_.get_pull_over_path()->partial_paths.size() > 1; const double distance_to_start = calcSignedArcLength( pull_over_path.getFullPath().points, current_pose.position, pull_over_path.start_pose.position); const double distance_to_restart = parameters_->decide_path_distance / 2; @@ -1215,7 +1396,8 @@ bool GoalPlannerModule::hasEnoughDistance(const PullOverPath & pull_over_path) c return false; } - const auto current_to_stop_distance = calcFeasibleDecelDistance(0.0); + const auto current_to_stop_distance = calcFeasibleDecelDistance( + planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk, 0.0); if (!current_to_stop_distance) { return false; } @@ -1235,10 +1417,10 @@ void GoalPlannerModule::keepStoppedWithCurrentPath(PathWithLaneId & path) { constexpr double keep_stop_time = 2.0; constexpr double keep_current_idx_buffer_time = 2.0; - if (last_increment_time_) { - const auto time_diff = (clock_->now() - *last_increment_time_).seconds(); + if (status_.get_last_increment_time()) { + const auto time_diff = (clock_->now() - *status_.get_last_increment_time()).seconds(); if (time_diff < keep_stop_time) { - status_.require_increment_ = false; + status_.set_require_increment(false); for (auto & p : path.points) { p.point.longitudinal_velocity_mps = 0.0; } @@ -1246,35 +1428,11 @@ void GoalPlannerModule::keepStoppedWithCurrentPath(PathWithLaneId & path) // require increment only when the time passed is enough // to prevent increment before driving // when the end of the current path is close to the current pose - status_.require_increment_ = true; + status_.set_require_increment(true); } } } -boost::optional GoalPlannerModule::calcFeasibleDecelDistance( - const double target_velocity) const -{ - const auto v_now = planner_data_->self_odometry->twist.twist.linear.x; - const auto a_now = planner_data_->self_acceleration->accel.accel.linear.x; - const auto a_lim = parameters_->maximum_deceleration; // positive value - const auto j_lim = parameters_->maximum_jerk; - - if (v_now < target_velocity) { - return 0.0; - } - - auto min_stop_distance = calcDecelDistWithJerkAndAccConstraints( - v_now, target_velocity, a_now, -a_lim, j_lim, -1.0 * j_lim); - - if (!min_stop_distance) { - return {}; - } - - min_stop_distance = std::max(*min_stop_distance, 0.0); - - return min_stop_distance; -} - double GoalPlannerModule::calcSignedArcLengthFromEgo( const PathWithLaneId & path, const Pose & pose) const { @@ -1299,7 +1457,8 @@ void GoalPlannerModule::decelerateForTurnSignal(const Pose & stop_pose, PathWith const float decel_vel = std::min(point.point.longitudinal_velocity_mps, static_cast(distance_to_stop / time)); const double distance_from_ego = calcSignedArcLengthFromEgo(path, point.point.pose); - const auto min_decel_distance = calcFeasibleDecelDistance(decel_vel); + const auto min_decel_distance = calcFeasibleDecelDistance( + planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk, decel_vel); // when current velocity already lower than decel_vel, min_decel_distance will be 0.0, // and do not need to decelerate. @@ -1317,7 +1476,8 @@ void GoalPlannerModule::decelerateForTurnSignal(const Pose & stop_pose, PathWith } const double stop_point_length = calcSignedArcLength(path.points, 0, stop_pose.position); - const auto min_stop_distance = calcFeasibleDecelDistance(0.0); + const auto min_stop_distance = calcFeasibleDecelDistance( + planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk, 0.0); if (min_stop_distance && *min_stop_distance < stop_point_length) { const auto stop_point = utils::insertStopPoint(stop_point_length, path); @@ -1331,7 +1491,9 @@ void GoalPlannerModule::decelerateBeforeSearchStart( const Pose & current_pose = planner_data_->self_odometry->pose.pose; // slow down before the search area. - const auto min_decel_distance = calcFeasibleDecelDistance(pull_over_velocity); + const auto min_decel_distance = calcFeasibleDecelDistance( + planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk, + pull_over_velocity); if (min_decel_distance) { const double distance_to_search_start = calcSignedArcLengthFromEgo(path, search_start_offset_pose); @@ -1432,19 +1594,114 @@ bool GoalPlannerModule::isCrossingPossible( bool GoalPlannerModule::isCrossingPossible(const PullOverPath & pull_over_path) const { - const lanelet::ConstLanelets lanes = utils::transformToLanelets(status_.lanes); + const lanelet::ConstLanelets lanes = utils::transformToLanelets(status_.get_lanes()); const Pose & start_pose = pull_over_path.start_pose; const Pose & end_pose = pull_over_path.end_pose; return isCrossingPossible(start_pose, end_pose, lanes); } +void GoalPlannerModule::updateSafetyCheckTargetObjectsData( + const PredictedObjects & filtered_objects, const TargetObjectsOnLane & target_objects_on_lane, + const std::vector & ego_predicted_path) const +{ + goal_planner_data_.filtered_objects = filtered_objects; + goal_planner_data_.target_objects_on_lane = target_objects_on_lane; + goal_planner_data_.ego_predicted_path = ego_predicted_path; +} + +bool GoalPlannerModule::isSafePath() const +{ + const auto pull_over_path = getCurrentPath(); + const auto & current_pose = planner_data_->self_odometry->pose.pose; + const double current_velocity = std::hypot( + planner_data_->self_odometry->twist.twist.linear.x, + planner_data_->self_odometry->twist.twist.linear.y); + const auto & dynamic_object = planner_data_->dynamic_object; + const auto & route_handler = planner_data_->route_handler; + const double backward_path_length = planner_data_->parameters.backward_path_length; + const auto current_lanes = utils::getExtendedCurrentLanes( + planner_data_, backward_path_length, std::numeric_limits::max(), + /*forward_only_in_route*/ true); + const auto pull_over_lanes = goal_planner_utils::getPullOverLanes( + *route_handler, left_side_parking_, parameters_->backward_goal_search_length, + parameters_->forward_goal_search_length); + const size_t ego_seg_idx = planner_data_->findEgoSegmentIndex(pull_over_path.points); + const std::pair terminal_velocity_and_accel = + utils::start_goal_planner_common::getPairsTerminalVelocityAndAccel( + status_.get_pull_over_path()->pairs_terminal_velocity_and_accel, + status_.get_current_path_idx()); + RCLCPP_DEBUG( + getLogger(), "pairs_terminal_velocity_and_accel for goal_planner: %f, %f", + terminal_velocity_and_accel.first, terminal_velocity_and_accel.second); + RCLCPP_DEBUG(getLogger(), "current_path_idx %ld", status_.get_current_path_idx()); + utils::start_goal_planner_common::updatePathProperty( + ego_predicted_path_params_, terminal_velocity_and_accel); + // TODO(Sugahara): shoule judge is_object_front properly + const bool is_object_front = true; + const bool limit_to_max_velocity = true; + const auto ego_predicted_path = + behavior_path_planner::utils::path_safety_checker::createPredictedPath( + ego_predicted_path_params_, pull_over_path.points, current_pose, current_velocity, + ego_seg_idx, is_object_front, limit_to_max_velocity); + + // filtering objects with velocity, position and class + const auto & filtered_objects = utils::path_safety_checker::filterObjects( + dynamic_object, route_handler, pull_over_lanes, current_pose.position, + objects_filtering_params_); + + // filtering objects based on the current position's lane + const auto & target_objects_on_lane = utils::path_safety_checker::createTargetObjectsOnLane( + pull_over_lanes, route_handler, filtered_objects, objects_filtering_params_); + + const double hysteresis_factor = + status_.get_prev_is_safe_dynamic_objects() ? 1.0 : parameters_->hysteresis_factor_expand_rate; + + utils::start_goal_planner_common::updateSafetyCheckTargetObjectsData( + goal_planner_data_, filtered_objects, target_objects_on_lane, ego_predicted_path); + + bool is_safe_dynamic_objects = true; + // Check for collisions with each predicted path of the object + for (const auto & object : target_objects_on_lane.on_current_lane) { + auto current_debug_data = marker_utils::createObjectDebug(object); + + bool is_safe_dynamic_object = true; + + const auto obj_predicted_paths = utils::path_safety_checker::getPredictedPathFromObj( + object, objects_filtering_params_->check_all_predicted_path); + + // If a collision is detected, mark the object as unsafe and break the loop + for (const auto & obj_path : obj_predicted_paths) { + if (!utils::path_safety_checker::checkCollision( + pull_over_path, ego_predicted_path, object, obj_path, planner_data_->parameters, + safety_check_params_->rss_params, hysteresis_factor, current_debug_data.second)) { + marker_utils::updateCollisionCheckDebugMap( + goal_planner_data_.collision_check, current_debug_data, false); + is_safe_dynamic_objects = false; + is_safe_dynamic_object = false; + break; + } + } + if (is_safe_dynamic_object) { + marker_utils::updateCollisionCheckDebugMap( + goal_planner_data_.collision_check, current_debug_data, is_safe_dynamic_object); + } + } + + return is_safe_dynamic_objects; +} + void GoalPlannerModule::setDebugData() { debug_marker_.markers.clear(); + using marker_utils::createObjectsMarkerArray; using marker_utils::createPathMarkerArray; using marker_utils::createPoseMarkerArray; + using marker_utils::createPredictedPathMarkerArray; + using marker_utils::showPolygon; + using marker_utils::showPredictedPath; + using marker_utils::showSafetyCheckInfo; using motion_utils::createStopVirtualWallMarker; using tier4_autoware_utils::createDefaultMarker; using tier4_autoware_utils::createMarkerColor; @@ -1458,51 +1715,89 @@ void GoalPlannerModule::setDebugData() } tier4_autoware_utils::appendMarkerArray(added, &debug_marker_); }; - - if (goal_planner_utils::isAllowedGoalModification( - planner_data_->route_handler, left_side_parking_)) { + if (goal_planner_utils::isAllowedGoalModification(planner_data_->route_handler)) { // Visualize pull over areas - const auto color = status_.has_decided_path ? createMarkerColor(1.0, 1.0, 0.0, 0.999) // yellow - : createMarkerColor(0.0, 1.0, 0.0, 0.999); // green - const double z = refined_goal_pose_.position.z; + const auto color = status_.get_has_decided_path() + ? createMarkerColor(1.0, 1.0, 0.0, 0.999) // yellow + : createMarkerColor(0.0, 1.0, 0.0, 0.999); // green + const double z = status_.get_refined_goal_pose().position.z; add(goal_planner_utils::createPullOverAreaMarkerArray( goal_searcher_->getAreaPolygons(), header, color, z)); // Visualize goal candidates - add(goal_planner_utils::createGoalCandidatesMarkerArray(goal_candidates_, color)); + const auto goal_candidates = status_.get_goal_candidates(); + add(goal_planner_utils::createGoalCandidatesMarkerArray(goal_candidates, color)); } // Visualize path and related pose - if (status_.is_safe) { + if (status_.get_is_safe_static_objects()) { add(createPoseMarkerArray( - status_.pull_over_path->start_pose, "pull_over_start_pose", 0, 0.3, 0.3, 0.9)); + status_.get_pull_over_path()->start_pose, "pull_over_start_pose", 0, 0.3, 0.3, 0.9)); add(createPoseMarkerArray( - status_.pull_over_path->end_pose, "pull_over_end_pose", 0, 0.3, 0.3, 0.9)); - add( - createPathMarkerArray(status_.pull_over_path->getFullPath(), "full_path", 0, 0.0, 0.5, 0.9)); + status_.get_pull_over_path()->end_pose, "pull_over_end_pose", 0, 0.3, 0.3, 0.9)); + add(createPathMarkerArray( + status_.get_pull_over_path()->getFullPath(), "full_path", 0, 0.0, 0.5, 0.9)); add(createPathMarkerArray(getCurrentPath(), "current_path", 0, 0.9, 0.5, 0.0)); // visualize each partial path - for (size_t i = 0; i < status_.pull_over_path->partial_paths.size(); ++i) { - const auto & partial_path = status_.pull_over_path->partial_paths.at(i); + for (size_t i = 0; i < status_.get_pull_over_path()->partial_paths.size(); ++i) { + const auto & partial_path = status_.get_pull_over_path()->partial_paths.at(i); add( createPathMarkerArray(partial_path, "partial_path_" + std::to_string(i), 0, 0.9, 0.5, 0.9)); } + + auto marker = tier4_autoware_utils::createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "detection_polygons", 0, Marker::LINE_LIST, + tier4_autoware_utils::createMarkerScale(0.01, 0.0, 0.0), + tier4_autoware_utils::createMarkerColor(0.0, 0.0, 1.0, 0.999)); + + for (const auto & ego_polygon : debug_data_.ego_polygons_expanded) { + for (size_t ep_idx = 0; ep_idx < ego_polygon.outer().size(); ++ep_idx) { + const auto & current_point = ego_polygon.outer().at(ep_idx); + const auto & next_point = ego_polygon.outer().at((ep_idx + 1) % ego_polygon.outer().size()); + + marker.points.push_back( + tier4_autoware_utils::createPoint(current_point.x(), current_point.y(), 0.0)); + marker.points.push_back( + tier4_autoware_utils::createPoint(next_point.x(), next_point.y(), 0.0)); + } + } + debug_marker_.markers.push_back(marker); + } + // safety check + if (parameters_->safety_check_params.enable_safety_check) { + if (goal_planner_data_.ego_predicted_path.size() > 0) { + const auto & ego_predicted_path = utils::path_safety_checker::convertToPredictedPath( + goal_planner_data_.ego_predicted_path, ego_predicted_path_params_->time_resolution); + add(createPredictedPathMarkerArray( + ego_predicted_path, vehicle_info_, "ego_predicted_path_goal_planner", 0, 0.0, 0.5, 0.9)); + } + if (goal_planner_data_.filtered_objects.objects.size() > 0) { + add(createObjectsMarkerArray( + goal_planner_data_.filtered_objects, "filtered_objects", 0, 0.0, 0.5, 0.9)); + } + + add(showSafetyCheckInfo(goal_planner_data_.collision_check, "object_debug_info")); + add(showPredictedPath(goal_planner_data_.collision_check, "ego_predicted_path")); + add(showPolygon(goal_planner_data_.collision_check, "ego_and_target_polygon_relation")); + utils::start_goal_planner_common::initializeCollisionCheckDebugMap( + goal_planner_data_.collision_check); } // Visualize planner type text { visualization_msgs::msg::MarkerArray planner_type_marker_array{}; - const auto color = status_.is_safe ? createMarkerColor(1.0, 1.0, 1.0, 0.99) - : createMarkerColor(1.0, 0.0, 0.0, 0.99); + const auto color = status_.get_is_safe_static_objects() + ? createMarkerColor(1.0, 1.0, 1.0, 0.99) + : createMarkerColor(1.0, 0.0, 0.0, 0.99); auto marker = createDefaultMarker( header.frame_id, header.stamp, "planner_type", 0, visualization_msgs::msg::Marker::TEXT_VIEW_FACING, createMarkerScale(0.0, 0.0, 1.0), color); - marker.pose = modified_goal_pose_ ? modified_goal_pose_->goal_pose - : planner_data_->self_odometry->pose.pose; - marker.text = magic_enum::enum_name(status_.pull_over_path->type); - marker.text += " " + std::to_string(status_.current_path_idx) + "/" + - std::to_string(status_.pull_over_path->partial_paths.size() - 1); + marker.pose = status_.get_modified_goal_pose() ? status_.get_modified_goal_pose()->goal_pose + : planner_data_->self_odometry->pose.pose; + marker.text = magic_enum::enum_name(status_.get_pull_over_path()->type); + marker.text += " " + std::to_string(status_.get_current_path_idx()) + "/" + + std::to_string(status_.get_pull_over_path()->partial_paths.size() - 1); if (isStuck()) { marker.text += " stuck"; } else if (isStopped()) { @@ -1520,7 +1815,7 @@ void GoalPlannerModule::setDebugData() } // Visualize debug poses - const auto & debug_poses = status_.pull_over_path->debug_poses; + const auto & debug_poses = status_.get_pull_over_path()->debug_poses; for (size_t i = 0; i < debug_poses.size(); ++i) { add(createPoseMarkerArray( debug_poses.at(i), "debug_pose_" + std::to_string(i), 0, 0.3, 0.3, 0.3)); @@ -1532,7 +1827,8 @@ void GoalPlannerModule::printParkingPositionError() const const auto current_pose = planner_data_->self_odometry->pose.pose; const double real_shoulder_to_map_shoulder = 0.0; - const Pose goal_to_ego = inverseTransformPose(current_pose, modified_goal_pose_->goal_pose); + const Pose goal_to_ego = + inverseTransformPose(current_pose, status_.get_modified_goal_pose()->goal_pose); const double dx = goal_to_ego.position.x; const double dy = goal_to_ego.position.y; const double distance_from_real_shoulder = @@ -1541,17 +1837,18 @@ void GoalPlannerModule::printParkingPositionError() const getLogger(), "current pose to goal, dx:%f dy:%f dyaw:%f from_real_shoulder:%f", dx, dy, tier4_autoware_utils::rad2deg( tf2::getYaw(current_pose.orientation) - - tf2::getYaw(modified_goal_pose_->goal_pose.orientation)), + tf2::getYaw(status_.get_modified_goal_pose()->goal_pose.orientation)), distance_from_real_shoulder); } bool GoalPlannerModule::checkOriginalGoalIsInShoulder() const { const auto & route_handler = planner_data_->route_handler; - const Pose & goal_pose = route_handler->getGoalPose(); + const Pose & goal_pose = route_handler->getOriginalGoalPose(); - const lanelet::ConstLanelets pull_over_lanes = - goal_planner_utils::getPullOverLanes(*(route_handler), left_side_parking_); + const lanelet::ConstLanelets pull_over_lanes = goal_planner_utils::getPullOverLanes( + *(route_handler), left_side_parking_, parameters_->backward_goal_search_length, + parameters_->forward_goal_search_length); lanelet::ConstLanelet target_lane{}; lanelet::utils::query::getClosestLanelet(pull_over_lanes, goal_pose, &target_lane); @@ -1566,10 +1863,10 @@ bool GoalPlannerModule::needPathUpdate(const double path_update_duration) const bool GoalPlannerModule::hasEnoughTimePassedSincePathUpdate(const double duration) const { - if (!last_path_update_time_) { + if (!status_.get_last_path_update_time()) { return true; } - return (clock_->now() - *last_path_update_time_).seconds() > duration; + return (clock_->now() - *status_.get_last_path_update_time()).seconds() > duration; } } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp index 7df1749a0fe82..41d0299bca427 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp @@ -15,8 +15,8 @@ #include "behavior_path_planner/scene_module/goal_planner/manager.hpp" #include "behavior_path_planner/utils/goal_planner/util.hpp" +#include "tier4_autoware_utils/ros/update_param.hpp" -#include #include #include @@ -32,19 +32,24 @@ GoalPlannerModuleManager::GoalPlannerModuleManager( { GoalPlannerParameters p; + const std::string base_ns = "goal_planner."; // general params { - std::string ns = "goal_planner."; - p.minimum_request_length = node->declare_parameter(ns + "minimum_request_length"); - p.th_stopped_velocity = node->declare_parameter(ns + "th_stopped_velocity"); - p.th_arrived_distance = node->declare_parameter(ns + "th_arrived_distance"); - p.th_stopped_time = node->declare_parameter(ns + "th_stopped_time"); + p.th_stopped_velocity = node->declare_parameter(base_ns + "th_stopped_velocity"); + p.th_arrived_distance = node->declare_parameter(base_ns + "th_arrived_distance"); + p.th_stopped_time = node->declare_parameter(base_ns + "th_stopped_time"); + p.center_line_path_interval = + node->declare_parameter(base_ns + "center_line_path_interval"); } // goal search { - std::string ns = "goal_planner.goal_search."; - p.search_priority = node->declare_parameter(ns + "search_priority"); + const std::string ns = base_ns + "goal_search."; + p.goal_priority = node->declare_parameter(ns + "goal_priority"); + p.minimum_weighted_distance_lateral_weight = + node->declare_parameter(ns + "minimum_weighted_distance.lateral_weight"); + p.prioritize_goals_before_objects = + node->declare_parameter(ns + "prioritize_goals_before_objects"); p.forward_goal_search_length = node->declare_parameter(ns + "forward_goal_search_length"); p.backward_goal_search_length = @@ -72,10 +77,13 @@ GoalPlannerModuleManager::GoalPlannerModuleManager( // occupancy grid map { - std::string ns = "goal_planner.occupancy_grid."; - p.use_occupancy_grid = node->declare_parameter(ns + "use_occupancy_grid"); - p.use_occupancy_grid_for_longitudinal_margin = - node->declare_parameter(ns + "use_occupancy_grid_for_longitudinal_margin"); + const std::string ns = base_ns + "occupancy_grid."; + p.use_occupancy_grid_for_goal_search = + node->declare_parameter(ns + "use_occupancy_grid_for_goal_search"); + p.use_occupancy_grid_for_path_collision_check = + node->declare_parameter(ns + "use_occupancy_grid_for_path_collision_check"); + p.use_occupancy_grid_for_goal_longitudinal_margin = + node->declare_parameter(ns + "use_occupancy_grid_for_goal_longitudinal_margin"); p.occupancy_grid_collision_check_margin = node->declare_parameter(ns + "occupancy_grid_collision_check_margin"); p.theta_size = node->declare_parameter(ns + "theta_size"); @@ -84,29 +92,35 @@ GoalPlannerModuleManager::GoalPlannerModuleManager( // object recognition { - std::string ns = "goal_planner.object_recognition."; + const std::string ns = base_ns + "object_recognition."; p.use_object_recognition = node->declare_parameter(ns + "use_object_recognition"); p.object_recognition_collision_check_margin = node->declare_parameter(ns + "object_recognition_collision_check_margin"); p.object_recognition_collision_check_max_extra_stopping_margin = node->declare_parameter( ns + "object_recognition_collision_check_max_extra_stopping_margin"); + p.th_moving_object_velocity = node->declare_parameter(ns + "th_moving_object_velocity"); } // pull over general params { - std::string ns = "goal_planner.pull_over."; + const std::string ns = base_ns + "pull_over."; + p.pull_over_minimum_request_length = + node->declare_parameter(ns + "minimum_request_length"); p.pull_over_velocity = node->declare_parameter(ns + "pull_over_velocity"); p.pull_over_minimum_velocity = node->declare_parameter(ns + "pull_over_minimum_velocity"); p.decide_path_distance = node->declare_parameter(ns + "decide_path_distance"); p.maximum_deceleration = node->declare_parameter(ns + "maximum_deceleration"); p.maximum_jerk = node->declare_parameter(ns + "maximum_jerk"); + p.path_priority = node->declare_parameter(ns + "path_priority"); + p.efficient_path_order = + node->declare_parameter>(ns + "efficient_path_order"); } // shift parking { - std::string ns = "goal_planner.pull_over.shift_parking."; + const std::string ns = base_ns + "pull_over.shift_parking."; p.enable_shift_parking = node->declare_parameter(ns + "enable_shift_parking"); p.shift_sampling_num = node->declare_parameter(ns + "shift_sampling_num"); p.maximum_lateral_jerk = node->declare_parameter(ns + "maximum_lateral_jerk"); @@ -116,9 +130,16 @@ GoalPlannerModuleManager::GoalPlannerModuleManager( node->declare_parameter(ns + "after_shift_straight_distance"); } + // parallel parking common + { + const std::string ns = base_ns + "pull_over.parallel_parking."; + p.parallel_parking_parameters.center_line_path_interval = + p.center_line_path_interval; // for geometric parallel parking + } + // forward parallel parking forward { - std::string ns = "goal_planner.pull_over.parallel_parking.forward."; + const std::string ns = base_ns + "pull_over.parallel_parking.forward."; p.enable_arc_forward_parking = node->declare_parameter(ns + "enable_arc_forward_parking"); p.parallel_parking_parameters.after_forward_parking_straight_distance = node->declare_parameter(ns + "after_forward_parking_straight_distance"); @@ -134,7 +155,7 @@ GoalPlannerModuleManager::GoalPlannerModuleManager( // forward parallel parking backward { - std::string ns = "goal_planner.pull_over.parallel_parking.backward."; + const std::string ns = base_ns + "pull_over.parallel_parking.backward."; p.enable_arc_backward_parking = node->declare_parameter(ns + "enable_arc_backward_parking"); p.parallel_parking_parameters.after_backward_parking_straight_distance = @@ -151,7 +172,7 @@ GoalPlannerModuleManager::GoalPlannerModuleManager( // freespace parking general params { - std::string ns = "goal_planner.pull_over.freespace_parking."; + const std::string ns = base_ns + "pull_over.freespace_parking."; p.enable_freespace_parking = node->declare_parameter(ns + "enable_freespace_parking"); p.freespace_parking_algorithm = node->declare_parameter(ns + "freespace_parking_algorithm"); @@ -174,7 +195,7 @@ GoalPlannerModuleManager::GoalPlannerModuleManager( // freespace parking search config { - std::string ns = "goal_planner.pull_over.freespace_parking.search_configs."; + const std::string ns = base_ns + "pull_over.freespace_parking.search_configs."; p.freespace_parking_common_parameters.theta_size = node->declare_parameter(ns + "theta_size"); p.freespace_parking_common_parameters.angle_goal_range = @@ -191,14 +212,14 @@ GoalPlannerModuleManager::GoalPlannerModuleManager( // freespace parking costmap configs { - std::string ns = "goal_planner.pull_over.freespace_parking.costmap_configs."; + const std::string ns = base_ns + "pull_over.freespace_parking.costmap_configs."; p.freespace_parking_common_parameters.obstacle_threshold = node->declare_parameter(ns + "obstacle_threshold"); } // freespace parking astar { - std::string ns = "goal_planner.pull_over.freespace_parking.astar."; + const std::string ns = base_ns + "pull_over.freespace_parking.astar."; p.astar_parameters.only_behind_solutions = node->declare_parameter(ns + "only_behind_solutions"); p.astar_parameters.use_back = node->declare_parameter(ns + "use_back"); @@ -208,7 +229,7 @@ GoalPlannerModuleManager::GoalPlannerModuleManager( // freespace parking rrtstar { - std::string ns = "goal_planner.pull_over.freespace_parking.rrtstar."; + const std::string ns = base_ns + "pull_over.freespace_parking.rrtstar."; p.rrt_star_parameters.enable_update = node->declare_parameter(ns + "enable_update"); p.rrt_star_parameters.use_informed_sampling = node->declare_parameter(ns + "use_informed_sampling"); @@ -218,9 +239,129 @@ GoalPlannerModuleManager::GoalPlannerModuleManager( p.rrt_star_parameters.margin = node->declare_parameter(ns + "margin"); } + // stop condition + { + p.maximum_deceleration_for_stop = + node->declare_parameter(base_ns + "stop_condition.maximum_deceleration_for_stop"); + p.maximum_jerk_for_stop = + node->declare_parameter(base_ns + "stop_condition.maximum_jerk_for_stop"); + } + + std::string path_safety_check_ns = "goal_planner.path_safety_check."; + + // EgoPredictedPath + std::string ego_path_ns = path_safety_check_ns + "ego_predicted_path."; + { + p.ego_predicted_path_params.min_velocity = + node->declare_parameter(ego_path_ns + "min_velocity"); + p.ego_predicted_path_params.acceleration = + node->declare_parameter(ego_path_ns + "acceleration"); + p.ego_predicted_path_params.max_velocity = + node->declare_parameter(ego_path_ns + "max_velocity"); + p.ego_predicted_path_params.time_horizon_for_front_object = + node->declare_parameter(ego_path_ns + "time_horizon_for_front_object"); + p.ego_predicted_path_params.time_horizon_for_rear_object = + node->declare_parameter(ego_path_ns + "time_horizon_for_rear_object"); + p.ego_predicted_path_params.time_resolution = + node->declare_parameter(ego_path_ns + "time_resolution"); + p.ego_predicted_path_params.delay_until_departure = + node->declare_parameter(ego_path_ns + "delay_until_departure"); + } + + // ObjectFilteringParams + std::string obj_filter_ns = path_safety_check_ns + "target_filtering."; + { + p.objects_filtering_params.safety_check_time_horizon = + node->declare_parameter(obj_filter_ns + "safety_check_time_horizon"); + p.objects_filtering_params.safety_check_time_resolution = + node->declare_parameter(obj_filter_ns + "safety_check_time_resolution"); + p.objects_filtering_params.object_check_forward_distance = + node->declare_parameter(obj_filter_ns + "object_check_forward_distance"); + p.objects_filtering_params.object_check_backward_distance = + node->declare_parameter(obj_filter_ns + "object_check_backward_distance"); + p.objects_filtering_params.ignore_object_velocity_threshold = + node->declare_parameter(obj_filter_ns + "ignore_object_velocity_threshold"); + p.objects_filtering_params.include_opposite_lane = + node->declare_parameter(obj_filter_ns + "include_opposite_lane"); + p.objects_filtering_params.invert_opposite_lane = + node->declare_parameter(obj_filter_ns + "invert_opposite_lane"); + p.objects_filtering_params.check_all_predicted_path = + node->declare_parameter(obj_filter_ns + "check_all_predicted_path"); + p.objects_filtering_params.use_all_predicted_path = + node->declare_parameter(obj_filter_ns + "use_all_predicted_path"); + p.objects_filtering_params.use_predicted_path_outside_lanelet = + node->declare_parameter(obj_filter_ns + "use_predicted_path_outside_lanelet"); + } + + // ObjectTypesToCheck + std::string obj_types_ns = obj_filter_ns + "object_types_to_check."; + { + p.objects_filtering_params.object_types_to_check.check_car = + node->declare_parameter(obj_types_ns + "check_car"); + p.objects_filtering_params.object_types_to_check.check_truck = + node->declare_parameter(obj_types_ns + "check_truck"); + p.objects_filtering_params.object_types_to_check.check_bus = + node->declare_parameter(obj_types_ns + "check_bus"); + p.objects_filtering_params.object_types_to_check.check_trailer = + node->declare_parameter(obj_types_ns + "check_trailer"); + p.objects_filtering_params.object_types_to_check.check_unknown = + node->declare_parameter(obj_types_ns + "check_unknown"); + p.objects_filtering_params.object_types_to_check.check_bicycle = + node->declare_parameter(obj_types_ns + "check_bicycle"); + p.objects_filtering_params.object_types_to_check.check_motorcycle = + node->declare_parameter(obj_types_ns + "check_motorcycle"); + p.objects_filtering_params.object_types_to_check.check_pedestrian = + node->declare_parameter(obj_types_ns + "check_pedestrian"); + } + + // ObjectLaneConfiguration + std::string obj_lane_ns = obj_filter_ns + "object_lane_configuration."; + { + p.objects_filtering_params.object_lane_configuration.check_current_lane = + node->declare_parameter(obj_lane_ns + "check_current_lane"); + p.objects_filtering_params.object_lane_configuration.check_right_lane = + node->declare_parameter(obj_lane_ns + "check_right_side_lane"); + p.objects_filtering_params.object_lane_configuration.check_left_lane = + node->declare_parameter(obj_lane_ns + "check_left_side_lane"); + p.objects_filtering_params.object_lane_configuration.check_shoulder_lane = + node->declare_parameter(obj_lane_ns + "check_shoulder_lane"); + p.objects_filtering_params.object_lane_configuration.check_other_lane = + node->declare_parameter(obj_lane_ns + "check_other_lane"); + } + + // SafetyCheckParams + std::string safety_check_ns = path_safety_check_ns + "safety_check_params."; + { + p.safety_check_params.enable_safety_check = + node->declare_parameter(safety_check_ns + "enable_safety_check"); + p.safety_check_params.hysteresis_factor_expand_rate = + node->declare_parameter(safety_check_ns + "hysteresis_factor_expand_rate"); + p.safety_check_params.backward_path_length = + node->declare_parameter(safety_check_ns + "backward_path_length"); + p.safety_check_params.forward_path_length = + node->declare_parameter(safety_check_ns + "forward_path_length"); + p.safety_check_params.publish_debug_marker = + node->declare_parameter(safety_check_ns + "publish_debug_marker"); + } + + // RSSparams + std::string rss_ns = safety_check_ns + "rss_params."; + { + p.safety_check_params.rss_params.rear_vehicle_reaction_time = + node->declare_parameter(rss_ns + "rear_vehicle_reaction_time"); + p.safety_check_params.rss_params.rear_vehicle_safety_time_margin = + node->declare_parameter(rss_ns + "rear_vehicle_safety_time_margin"); + p.safety_check_params.rss_params.lateral_distance_max_threshold = + node->declare_parameter(rss_ns + "lateral_distance_max_threshold"); + p.safety_check_params.rss_params.longitudinal_distance_min_threshold = + node->declare_parameter(rss_ns + "longitudinal_distance_min_threshold"); + p.safety_check_params.rss_params.longitudinal_velocity_delta_time = + node->declare_parameter(rss_ns + "longitudinal_velocity_delta_time"); + } + // debug { - std::string ns = "goal_planner.debug."; + const std::string ns = base_ns + "debug."; p.print_debug_info = node->declare_parameter(ns + "print_debug_info"); } @@ -241,8 +382,6 @@ GoalPlannerModuleManager::GoalPlannerModuleManager( } parameters_ = std::make_shared(p); - - left_side_parking_ = parameters_->parking_policy == ParkingPolicy::LEFT_SIDE; } void GoalPlannerModuleManager::updateModuleParams( @@ -259,12 +398,26 @@ void GoalPlannerModuleManager::updateModuleParams( }); } +bool GoalPlannerModuleManager::isAlwaysExecutableModule() const +{ + // enable AlwaysExecutable whenever goal modification is not allowed + // because only minor path refinements are made for fixed goals + if (!goal_planner_utils::isAllowedGoalModification(planner_data_->route_handler)) { + return true; + } + + return false; +} + bool GoalPlannerModuleManager::isSimultaneousExecutableAsApprovedModule() const { + if (isAlwaysExecutableModule()) { + return true; + } + // enable SimultaneousExecutable whenever goal modification is not allowed // because only minor path refinements are made for fixed goals - if (!goal_planner_utils::isAllowedGoalModification( - planner_data_->route_handler, left_side_parking_)) { + if (!goal_planner_utils::isAllowedGoalModification(planner_data_->route_handler)) { return true; } @@ -273,10 +426,13 @@ bool GoalPlannerModuleManager::isSimultaneousExecutableAsApprovedModule() const bool GoalPlannerModuleManager::isSimultaneousExecutableAsCandidateModule() const { + if (isAlwaysExecutableModule()) { + return true; + } + // enable SimultaneousExecutable whenever goal modification is not allowed // because only minor path refinements are made for fixed goals - if (!goal_planner_utils::isAllowedGoalModification( - planner_data_->route_handler, left_side_parking_)) { + if (!goal_planner_utils::isAllowedGoalModification(planner_data_->route_handler)) { return true; } diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/avoidance_by_lane_change.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/avoidance_by_lane_change.cpp index 841fc9092e279..daba02e1cee89 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/avoidance_by_lane_change.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/avoidance_by_lane_change.cpp @@ -19,6 +19,10 @@ #include "behavior_path_planner/utils/path_utils.hpp" #include +#include + +#include +#include #include @@ -138,7 +142,8 @@ void AvoidanceByLaneChange::fillAvoidanceTargetObjects( const auto [object_within_target_lane, object_outside_target_lane] = utils::path_safety_checker::separateObjectsByLanelets( - *planner_data_->dynamic_object, data.current_lanelets); + *planner_data_->dynamic_object, data.current_lanelets, + utils::path_safety_checker::isPolygonOverlapLanelet); // Assume that the maximum allocation for data.other object is the sum of // objects_within_target_lane and object_outside_target_lane. The maximum allocation for @@ -216,7 +221,7 @@ ObjectData AvoidanceByLaneChange::createObjectData( // Find the footprint point closest to the path, set to object_data.overhang_distance. object_data.overhang_dist = utils::avoidance::calcEnvelopeOverhangDistance( - object_data, object_closest_pose, object_data.overhang_pose.position); + object_data, data.reference_path, object_data.overhang_pose.position); // Check whether the the ego should avoid the object. const auto & vehicle_width = planner_data_->parameters.vehicle_width; diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/external_request.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/external_request.cpp index a74a51b7e2cdb..3a304a9b5bb53 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/external_request.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/external_request.cpp @@ -14,18 +14,9 @@ #include "behavior_path_planner/scene_module/lane_change/external_request.hpp" -#include "behavior_path_planner/scene_module/scene_module_visitor.hpp" -#include "behavior_path_planner/utils/lane_change/utils.hpp" -#include "behavior_path_planner/utils/path_utils.hpp" - #include -#include -#include #include -#include -#include -#include namespace behavior_path_planner { diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp index 270e2064228af..22c8ac022ad1a 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp @@ -15,15 +15,14 @@ #include "behavior_path_planner/scene_module/lane_change/interface.hpp" #include "behavior_path_planner/marker_utils/lane_change/debug.hpp" +#include "behavior_path_planner/marker_utils/utils.hpp" #include "behavior_path_planner/scene_module/scene_module_interface.hpp" #include "behavior_path_planner/scene_module/scene_module_visitor.hpp" #include "behavior_path_planner/utils/lane_change/utils.hpp" -#include "behavior_path_planner/utils/path_utils.hpp" #include #include -#include #include #include #include @@ -31,6 +30,7 @@ namespace behavior_path_planner { +using tier4_autoware_utils::appendMarkerArray; using utils::lane_change::assignToCandidate; LaneChangeInterface::LaneChangeInterface( @@ -87,14 +87,11 @@ ModuleStatus LaneChangeInterface::updateState() } if (!module_type_->isValidPath()) { - return ModuleStatus::RUNNING; + return ModuleStatus::SUCCESS; } if (module_type_->isAbortState()) { - if (module_type_->hasFinishedAbort()) { - resetLaneChangeModule(); - } - return ModuleStatus::RUNNING; + return module_type_->hasFinishedAbort() ? ModuleStatus::SUCCESS : ModuleStatus::RUNNING; } if (module_type_->hasFinishedLaneChange()) { @@ -148,15 +145,12 @@ ModuleStatus LaneChangeInterface::updateState() return ModuleStatus::RUNNING; } - if (module_type_->isEgoOnPreparePhase()) { + if (module_type_->isEgoOnPreparePhase() && module_type_->isAbleToReturnCurrentLane()) { RCLCPP_WARN_STREAM_THROTTLE( getLogger().get_child(module_type_->getModuleTypeStr()), *clock_, 5000, "Lane change path is unsafe. Cancel lane change."); module_type_->toCancelState(); - if (!isWaitingApproval()) { - resetLaneChangeModule(); - } - return ModuleStatus::RUNNING; + return isWaitingApproval() ? ModuleStatus::RUNNING : ModuleStatus::SUCCESS; } if (!module_type_->isAbortEnabled()) { @@ -191,18 +185,12 @@ ModuleStatus LaneChangeInterface::updateState() return ModuleStatus::RUNNING; } -void LaneChangeInterface::resetLaneChangeModule() -{ - processOnExit(); - removeRTCStatus(); - processOnEntry(); -} - void LaneChangeInterface::updateData() { module_type_->setPreviousModulePaths( getPreviousModuleOutput().reference_path, getPreviousModuleOutput().path); module_type_->updateSpecialData(); + module_type_->resetStopPose(); } BehaviorModuleOutput LaneChangeInterface::plan() @@ -221,6 +209,8 @@ BehaviorModuleOutput LaneChangeInterface::plan() *prev_approved_path_ = *getPreviousModuleOutput().path; module_type_->insertStopPoint(module_type_->getLaneChangeStatus().target_lanes, *output.path); + stop_pose_ = module_type_->getStopPose(); + updateSteeringFactorPtr(output); clearWaitingApproval(); @@ -249,6 +239,8 @@ BehaviorModuleOutput LaneChangeInterface::planWaitingApproval() path_reference_ = getPreviousModuleOutput().reference_path; + stop_pose_ = module_type_->getStopPose(); + if (!module_type_->isValidPath()) { removeRTCStatus(); path_candidate_ = std::make_shared(); @@ -286,6 +278,7 @@ void LaneChangeInterface::updateModuleParams(const std::any & parameters) void LaneChangeInterface::setData(const std::shared_ptr & data) { + planner_data_ = data; module_type_->setData(data); } @@ -295,15 +288,16 @@ void LaneChangeInterface::setObjectDebugVisualization() const if (!parameters_->publish_debug_marker) { return; } + using marker_utils::showPolygon; + using marker_utils::showPredictedPath; + using marker_utils::showSafetyCheckInfo; using marker_utils::lane_change_markers::showAllValidLaneChangePath; - using marker_utils::lane_change_markers::showLerpedPose; - using marker_utils::lane_change_markers::showObjectInfo; - using marker_utils::lane_change_markers::showPolygon; - using marker_utils::lane_change_markers::showPolygonPose; + using marker_utils::lane_change_markers::showFilteredObjects; const auto debug_data = module_type_->getDebugData(); const auto debug_after_approval = module_type_->getAfterApprovalDebugData(); const auto debug_valid_path = module_type_->getDebugValidPath(); + const auto debug_filtered_objects = module_type_->getDebugFilteredObjects(); debug_marker_.markers.clear(); const auto add = [this](const MarkerArray & added) { @@ -311,15 +305,18 @@ void LaneChangeInterface::setObjectDebugVisualization() const }; add(showAllValidLaneChangePath(debug_valid_path, "lane_change_valid_paths")); + add(showFilteredObjects( + debug_filtered_objects.current_lane, debug_filtered_objects.target_lane, + debug_filtered_objects.other_lane, "object_filtered")); if (!debug_data.empty()) { - add(showObjectInfo(debug_data, "object_debug_info")); - add(showLerpedPose(debug_data, "ego_predicted_path")); + add(showSafetyCheckInfo(debug_data, "object_debug_info")); + add(showPredictedPath(debug_data, "ego_predicted_path")); add(showPolygon(debug_data, "ego_and_target_polygon_relation")); } if (!debug_after_approval.empty()) { - add(showObjectInfo(debug_after_approval, "object_debug_info_after_approval")); - add(showLerpedPose(debug_after_approval, "ego_predicted_path_after_approval")); + add(showSafetyCheckInfo(debug_after_approval, "object_debug_info_after_approval")); + add(showPredictedPath(debug_after_approval, "ego_predicted_path_after_approval")); add(showPolygon(debug_after_approval, "ego_and_target_polygon_relation_after_approval")); } } @@ -332,10 +329,9 @@ std::shared_ptr LaneChangeInterface::get_debug_msg_arra for (const auto & [uuid, debug_data] : debug_data) { LaneChangeDebugMsg debug_msg; debug_msg.object_id = uuid; - debug_msg.allow_lane_change = debug_data.allow_lane_change; + debug_msg.allow_lane_change = debug_data.is_safe; debug_msg.is_front = debug_data.is_front; - debug_msg.relative_distance = debug_data.relative_to_ego; - debug_msg.failed_reason = debug_data.failed_reason; + debug_msg.failed_reason = debug_data.unsafe_reason; debug_msg.velocity = std::hypot(debug_data.object_twist.linear.x, debug_data.object_twist.linear.y); debug_msg_array.lane_change_info.push_back(debug_msg); diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp index f392ef6a26e22..78e05c940a814 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp @@ -14,11 +14,13 @@ #include "behavior_path_planner/scene_module/lane_change/manager.hpp" +#include "tier4_autoware_utils/ros/parameter.hpp" +#include "tier4_autoware_utils/ros/update_param.hpp" + #include #include #include -#include #include namespace behavior_path_planner @@ -27,122 +29,151 @@ namespace behavior_path_planner using route_handler::Direction; using utils::convertToSnakeCase; -namespace -{ -template -T get_parameter(rclcpp::Node * node, const std::string & ns) -{ - if (node->has_parameter(ns)) { - return node->get_parameter(ns).get_value(); - } - - return node->declare_parameter(ns); -} -} // namespace - LaneChangeModuleManager::LaneChangeModuleManager( rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config, const Direction direction, const LaneChangeModuleType type) : SceneModuleManagerInterface(node, name, config, {""}), direction_{direction}, type_{type} { + using tier4_autoware_utils::getOrDeclareParameter; + LaneChangeParameters p{}; const auto parameter = [](std::string && name) { return "lane_change." + name; }; // trajectory generation - p.backward_lane_length = get_parameter(node, parameter("backward_lane_length")); + p.backward_lane_length = getOrDeclareParameter(*node, parameter("backward_lane_length")); p.prediction_time_resolution = - get_parameter(node, parameter("prediction_time_resolution")); + getOrDeclareParameter(*node, parameter("prediction_time_resolution")); p.longitudinal_acc_sampling_num = - get_parameter(node, parameter("longitudinal_acceleration_sampling_num")); + getOrDeclareParameter(*node, parameter("longitudinal_acceleration_sampling_num")); p.lateral_acc_sampling_num = - get_parameter(node, parameter("lateral_acceleration_sampling_num")); + getOrDeclareParameter(*node, parameter("lateral_acceleration_sampling_num")); // parked vehicle detection p.object_check_min_road_shoulder_width = - get_parameter(node, parameter("object_check_min_road_shoulder_width")); + getOrDeclareParameter(*node, parameter("object_check_min_road_shoulder_width")); p.object_shiftable_ratio_threshold = - get_parameter(node, parameter("object_shiftable_ratio_threshold")); + getOrDeclareParameter(*node, parameter("object_shiftable_ratio_threshold")); // turn signal p.min_length_for_turn_signal_activation = - get_parameter(node, parameter("min_length_for_turn_signal_activation")); + getOrDeclareParameter(*node, parameter("min_length_for_turn_signal_activation")); p.length_ratio_for_turn_signal_deactivation = - get_parameter(node, parameter("length_ratio_for_turn_signal_deactivation")); + getOrDeclareParameter(*node, parameter("length_ratio_for_turn_signal_deactivation")); // acceleration - p.min_longitudinal_acc = get_parameter(node, parameter("min_longitudinal_acc")); - p.max_longitudinal_acc = get_parameter(node, parameter("max_longitudinal_acc")); + p.min_longitudinal_acc = getOrDeclareParameter(*node, parameter("min_longitudinal_acc")); + p.max_longitudinal_acc = getOrDeclareParameter(*node, parameter("max_longitudinal_acc")); // collision check p.enable_prepare_segment_collision_check = - get_parameter(node, parameter("enable_prepare_segment_collision_check")); - p.prepare_segment_ignore_object_velocity_thresh = - get_parameter(node, parameter("prepare_segment_ignore_object_velocity_thresh")); + getOrDeclareParameter(*node, parameter("enable_prepare_segment_collision_check")); + p.prepare_segment_ignore_object_velocity_thresh = getOrDeclareParameter( + *node, parameter("prepare_segment_ignore_object_velocity_thresh")); p.check_objects_on_current_lanes = - get_parameter(node, parameter("check_objects_on_current_lanes")); + getOrDeclareParameter(*node, parameter("check_objects_on_current_lanes")); p.check_objects_on_other_lanes = - get_parameter(node, parameter("check_objects_on_other_lanes")); - p.use_all_predicted_path = get_parameter(node, parameter("use_all_predicted_path")); - - p.rss_params.longitudinal_distance_min_threshold = - get_parameter(node, parameter("safety_check.longitudinal_distance_min_threshold")); - p.rss_params.longitudinal_velocity_delta_time = - get_parameter(node, parameter("safety_check.longitudinal_velocity_delta_time")); - p.rss_params.front_vehicle_deceleration = - get_parameter(node, parameter("safety_check.expected_front_deceleration")); - p.rss_params.rear_vehicle_deceleration = - get_parameter(node, parameter("safety_check.expected_rear_deceleration")); - p.rss_params.rear_vehicle_reaction_time = - get_parameter(node, parameter("safety_check.rear_vehicle_reaction_time")); - p.rss_params.rear_vehicle_safety_time_margin = - get_parameter(node, parameter("safety_check.rear_vehicle_safety_time_margin")); - p.rss_params.lateral_distance_max_threshold = - get_parameter(node, parameter("safety_check.lateral_distance_max_threshold")); - - p.rss_params_for_abort.longitudinal_distance_min_threshold = - get_parameter(node, parameter("safety_check.longitudinal_distance_min_threshold")); - p.rss_params_for_abort.longitudinal_velocity_delta_time = - get_parameter(node, parameter("safety_check.longitudinal_velocity_delta_time")); - p.rss_params_for_abort.front_vehicle_deceleration = - get_parameter(node, parameter("safety_check.expected_front_deceleration_for_abort")); - p.rss_params_for_abort.rear_vehicle_deceleration = - get_parameter(node, parameter("safety_check.expected_rear_deceleration_for_abort")); - p.rss_params_for_abort.rear_vehicle_reaction_time = - get_parameter(node, parameter("safety_check.rear_vehicle_reaction_time")); - p.rss_params_for_abort.rear_vehicle_safety_time_margin = - get_parameter(node, parameter("safety_check.rear_vehicle_safety_time_margin")); - p.rss_params_for_abort.lateral_distance_max_threshold = - get_parameter(node, parameter("safety_check.lateral_distance_max_threshold")); + getOrDeclareParameter(*node, parameter("check_objects_on_other_lanes")); + p.use_all_predicted_path = + getOrDeclareParameter(*node, parameter("use_all_predicted_path")); + p.lane_expansion_left_offset = + getOrDeclareParameter(*node, parameter("safety_check.lane_expansion.left_offset")); + p.lane_expansion_right_offset = + getOrDeclareParameter(*node, parameter("safety_check.lane_expansion.right_offset")); + // lane change regulations + p.regulate_on_crosswalk = getOrDeclareParameter(*node, parameter("regulation.crosswalk")); + p.regulate_on_intersection = + getOrDeclareParameter(*node, parameter("regulation.intersection")); + + // ego vehicle stuck detection + p.stop_velocity_threshold = + getOrDeclareParameter(*node, parameter("stuck_detection.velocity")); + p.stop_time_threshold = + getOrDeclareParameter(*node, parameter("stuck_detection.stop_time")); + + // safety check + p.allow_loose_check_for_cancel = + getOrDeclareParameter(*node, parameter("safety_check.allow_loose_check_for_cancel")); + + p.rss_params.longitudinal_distance_min_threshold = getOrDeclareParameter( + *node, parameter("safety_check.execution.longitudinal_distance_min_threshold")); + p.rss_params.longitudinal_distance_min_threshold = getOrDeclareParameter( + *node, parameter("safety_check.execution.longitudinal_distance_min_threshold")); + p.rss_params.longitudinal_velocity_delta_time = getOrDeclareParameter( + *node, parameter("safety_check.execution.longitudinal_velocity_delta_time")); + p.rss_params.front_vehicle_deceleration = getOrDeclareParameter( + *node, parameter("safety_check.execution.expected_front_deceleration")); + p.rss_params.rear_vehicle_deceleration = getOrDeclareParameter( + *node, parameter("safety_check.execution.expected_rear_deceleration")); + p.rss_params.rear_vehicle_reaction_time = getOrDeclareParameter( + *node, parameter("safety_check.execution.rear_vehicle_reaction_time")); + p.rss_params.rear_vehicle_safety_time_margin = getOrDeclareParameter( + *node, parameter("safety_check.execution.rear_vehicle_safety_time_margin")); + p.rss_params.lateral_distance_max_threshold = getOrDeclareParameter( + *node, parameter("safety_check.execution.lateral_distance_max_threshold")); + + p.rss_params_for_abort.longitudinal_distance_min_threshold = getOrDeclareParameter( + *node, parameter("safety_check.cancel.longitudinal_distance_min_threshold")); + p.rss_params_for_abort.longitudinal_velocity_delta_time = getOrDeclareParameter( + *node, parameter("safety_check.cancel.longitudinal_velocity_delta_time")); + p.rss_params_for_abort.front_vehicle_deceleration = getOrDeclareParameter( + *node, parameter("safety_check.cancel.expected_front_deceleration")); + p.rss_params_for_abort.rear_vehicle_deceleration = getOrDeclareParameter( + *node, parameter("safety_check.cancel.expected_rear_deceleration")); + p.rss_params_for_abort.rear_vehicle_reaction_time = getOrDeclareParameter( + *node, parameter("safety_check.cancel.rear_vehicle_reaction_time")); + p.rss_params_for_abort.rear_vehicle_safety_time_margin = getOrDeclareParameter( + *node, parameter("safety_check.cancel.rear_vehicle_safety_time_margin")); + p.rss_params_for_abort.lateral_distance_max_threshold = getOrDeclareParameter( + *node, parameter("safety_check.cancel.lateral_distance_max_threshold")); + + p.rss_params_for_stuck.longitudinal_distance_min_threshold = getOrDeclareParameter( + *node, parameter("safety_check.stuck.longitudinal_distance_min_threshold")); + p.rss_params_for_stuck.longitudinal_velocity_delta_time = getOrDeclareParameter( + *node, parameter("safety_check.stuck.longitudinal_velocity_delta_time")); + p.rss_params_for_stuck.front_vehicle_deceleration = getOrDeclareParameter( + *node, parameter("safety_check.stuck.expected_front_deceleration")); + p.rss_params_for_stuck.rear_vehicle_deceleration = getOrDeclareParameter( + *node, parameter("safety_check.stuck.expected_rear_deceleration")); + p.rss_params_for_stuck.rear_vehicle_reaction_time = getOrDeclareParameter( + *node, parameter("safety_check.stuck.rear_vehicle_reaction_time")); + p.rss_params_for_stuck.rear_vehicle_safety_time_margin = getOrDeclareParameter( + *node, parameter("safety_check.stuck.rear_vehicle_safety_time_margin")); + p.rss_params_for_stuck.lateral_distance_max_threshold = getOrDeclareParameter( + *node, parameter("safety_check.stuck.lateral_distance_max_threshold")); // target object { std::string ns = "lane_change.target_object."; - p.check_car = get_parameter(node, ns + "car"); - p.check_truck = get_parameter(node, ns + "truck"); - p.check_bus = get_parameter(node, ns + "bus"); - p.check_trailer = get_parameter(node, ns + "trailer"); - p.check_unknown = get_parameter(node, ns + "unknown"); - p.check_bicycle = get_parameter(node, ns + "bicycle"); - p.check_motorcycle = get_parameter(node, ns + "motorcycle"); - p.check_pedestrian = get_parameter(node, ns + "pedestrian"); + p.object_types_to_check.check_car = getOrDeclareParameter(*node, ns + "car"); + p.object_types_to_check.check_truck = getOrDeclareParameter(*node, ns + "truck"); + p.object_types_to_check.check_bus = getOrDeclareParameter(*node, ns + "bus"); + p.object_types_to_check.check_trailer = getOrDeclareParameter(*node, ns + "trailer"); + p.object_types_to_check.check_unknown = getOrDeclareParameter(*node, ns + "unknown"); + p.object_types_to_check.check_bicycle = getOrDeclareParameter(*node, ns + "bicycle"); + p.object_types_to_check.check_motorcycle = + getOrDeclareParameter(*node, ns + "motorcycle"); + p.object_types_to_check.check_pedestrian = + getOrDeclareParameter(*node, ns + "pedestrian"); } // lane change cancel p.cancel.enable_on_prepare_phase = - get_parameter(node, parameter("cancel.enable_on_prepare_phase")); + getOrDeclareParameter(*node, parameter("cancel.enable_on_prepare_phase")); p.cancel.enable_on_lane_changing_phase = - get_parameter(node, parameter("cancel.enable_on_lane_changing_phase")); - p.cancel.delta_time = get_parameter(node, parameter("cancel.delta_time")); - p.cancel.duration = get_parameter(node, parameter("cancel.duration")); - p.cancel.max_lateral_jerk = get_parameter(node, parameter("cancel.max_lateral_jerk")); - p.cancel.overhang_tolerance = get_parameter(node, parameter("cancel.overhang_tolerance")); + getOrDeclareParameter(*node, parameter("cancel.enable_on_lane_changing_phase")); + p.cancel.delta_time = getOrDeclareParameter(*node, parameter("cancel.delta_time")); + p.cancel.duration = getOrDeclareParameter(*node, parameter("cancel.duration")); + p.cancel.max_lateral_jerk = + getOrDeclareParameter(*node, parameter("cancel.max_lateral_jerk")); + p.cancel.overhang_tolerance = + getOrDeclareParameter(*node, parameter("cancel.overhang_tolerance")); p.finish_judge_lateral_threshold = - get_parameter(node, parameter("finish_judge_lateral_threshold")); + getOrDeclareParameter(*node, parameter("finish_judge_lateral_threshold")); // debug marker - p.publish_debug_marker = get_parameter(node, parameter("publish_debug_marker")); + p.publish_debug_marker = getOrDeclareParameter(*node, parameter("publish_debug_marker")); // validation of parameters if (p.longitudinal_acc_sampling_num < 1 || p.lateral_acc_sampling_num < 1) { @@ -154,6 +185,26 @@ LaneChangeModuleManager::LaneChangeModuleManager( exit(EXIT_FAILURE); } + // validation of safety check parameters + // if loosely check is not allowed, lane change module will keep on chattering and canceling, and + // false positive situation might occur + if (!p.allow_loose_check_for_cancel) { + if ( + p.rss_params.front_vehicle_deceleration > p.rss_params_for_abort.front_vehicle_deceleration || + p.rss_params.rear_vehicle_deceleration > p.rss_params_for_abort.rear_vehicle_deceleration || + p.rss_params.rear_vehicle_reaction_time > p.rss_params_for_abort.rear_vehicle_reaction_time || + p.rss_params.rear_vehicle_safety_time_margin > + p.rss_params_for_abort.rear_vehicle_safety_time_margin || + p.rss_params.lateral_distance_max_threshold > + p.rss_params_for_abort.lateral_distance_max_threshold || + p.rss_params.longitudinal_distance_min_threshold > + p.rss_params_for_abort.longitudinal_distance_min_threshold || + p.rss_params.longitudinal_velocity_delta_time > + p.rss_params_for_abort.longitudinal_velocity_delta_time) { + RCLCPP_FATAL_STREAM(logger_, "abort parameter might be loose... Terminating the program..."); + exit(EXIT_FAILURE); + } + } if (p.cancel.delta_time < 1.0) { RCLCPP_WARN_STREAM( logger_, "cancel.delta_time: " << p.cancel.delta_time @@ -196,6 +247,7 @@ AvoidanceByLaneChangeModuleManager::AvoidanceByLaneChangeModuleManager( node, name, config, Direction::NONE, LaneChangeModuleType::AVOIDANCE_BY_LANE_CHANGE) { using autoware_auto_perception_msgs::msg::ObjectClassification; + using tier4_autoware_utils::getOrDeclareParameter; rtc_interface_ptr_map_.clear(); const std::vector rtc_types = {"left", "right"}; @@ -211,34 +263,39 @@ AvoidanceByLaneChangeModuleManager::AvoidanceByLaneChangeModuleManager( { std::string ns = "avoidance_by_lane_change."; p.execute_object_longitudinal_margin = - get_parameter(node, ns + "execute_object_longitudinal_margin"); + getOrDeclareParameter(*node, ns + "execute_object_longitudinal_margin"); p.execute_only_when_lane_change_finish_before_object = - get_parameter(node, ns + "execute_only_when_lane_change_finish_before_object"); + getOrDeclareParameter(*node, ns + "execute_only_when_lane_change_finish_before_object"); } // general params { std::string ns = "avoidance."; p.resample_interval_for_planning = - get_parameter(node, ns + "resample_interval_for_planning"); + getOrDeclareParameter(*node, ns + "resample_interval_for_planning"); p.resample_interval_for_output = - get_parameter(node, ns + "resample_interval_for_output"); + getOrDeclareParameter(*node, ns + "resample_interval_for_output"); p.enable_force_avoidance_for_stopped_vehicle = - get_parameter(node, ns + "enable_force_avoidance_for_stopped_vehicle"); + getOrDeclareParameter(*node, ns + "enable_force_avoidance_for_stopped_vehicle"); } // target object { const auto get_object_param = [&](std::string && ns) { ObjectParameter param{}; - param.is_target = get_parameter(node, ns + "is_target"); - param.execute_num = get_parameter(node, ns + "execute_num"); - param.moving_speed_threshold = get_parameter(node, ns + "moving_speed_threshold"); - param.moving_time_threshold = get_parameter(node, ns + "moving_time_threshold"); - param.max_expand_ratio = get_parameter(node, ns + "max_expand_ratio"); - param.envelope_buffer_margin = get_parameter(node, ns + "envelope_buffer_margin"); - param.avoid_margin_lateral = get_parameter(node, ns + "avoid_margin_lateral"); - param.safety_buffer_lateral = get_parameter(node, ns + "safety_buffer_lateral"); + param.is_target = getOrDeclareParameter(*node, ns + "is_target"); + param.execute_num = getOrDeclareParameter(*node, ns + "execute_num"); + param.moving_speed_threshold = + getOrDeclareParameter(*node, ns + "moving_speed_threshold"); + param.moving_time_threshold = + getOrDeclareParameter(*node, ns + "moving_time_threshold"); + param.max_expand_ratio = getOrDeclareParameter(*node, ns + "max_expand_ratio"); + param.envelope_buffer_margin = + getOrDeclareParameter(*node, ns + "envelope_buffer_margin"); + param.avoid_margin_lateral = + getOrDeclareParameter(*node, ns + "avoid_margin_lateral"); + param.safety_buffer_lateral = + getOrDeclareParameter(*node, ns + "safety_buffer_lateral"); return param; }; @@ -255,41 +312,50 @@ AvoidanceByLaneChangeModuleManager::AvoidanceByLaneChangeModuleManager( p.object_parameters.emplace(ObjectClassification::UNKNOWN, get_object_param(ns + "unknown.")); p.lower_distance_for_polygon_expansion = - get_parameter(node, ns + "lower_distance_for_polygon_expansion"); + getOrDeclareParameter(*node, ns + "lower_distance_for_polygon_expansion"); p.upper_distance_for_polygon_expansion = - get_parameter(node, ns + "upper_distance_for_polygon_expansion"); + getOrDeclareParameter(*node, ns + "upper_distance_for_polygon_expansion"); } // target filtering { std::string ns = "avoidance.target_filtering."; - p.threshold_time_force_avoidance_for_stopped_vehicle = - get_parameter(node, ns + "threshold_time_force_avoidance_for_stopped_vehicle"); - p.object_ignore_section_traffic_light_in_front_distance = - get_parameter(node, ns + "object_ignore_section_traffic_light_in_front_distance"); - p.object_ignore_section_crosswalk_in_front_distance = - get_parameter(node, ns + "object_ignore_section_crosswalk_in_front_distance"); + p.threshold_time_force_avoidance_for_stopped_vehicle = getOrDeclareParameter( + *node, ns + "threshold_time_force_avoidance_for_stopped_vehicle"); + p.object_ignore_section_traffic_light_in_front_distance = getOrDeclareParameter( + *node, ns + "object_ignore_section_traffic_light_in_front_distance"); + p.object_ignore_section_crosswalk_in_front_distance = getOrDeclareParameter( + *node, ns + "object_ignore_section_crosswalk_in_front_distance"); p.object_ignore_section_crosswalk_behind_distance = - get_parameter(node, ns + "object_ignore_section_crosswalk_behind_distance"); - p.object_check_forward_distance = - get_parameter(node, ns + "object_check_forward_distance"); - p.object_check_backward_distance = - get_parameter(node, ns + "object_check_backward_distance"); - p.object_check_goal_distance = get_parameter(node, ns + "object_check_goal_distance"); + getOrDeclareParameter(*node, ns + "object_ignore_section_crosswalk_behind_distance"); + p.object_check_goal_distance = + getOrDeclareParameter(*node, ns + "object_check_goal_distance"); p.threshold_distance_object_is_on_center = - get_parameter(node, ns + "threshold_distance_object_is_on_center"); + getOrDeclareParameter(*node, ns + "threshold_distance_object_is_on_center"); p.object_check_shiftable_ratio = - get_parameter(node, ns + "object_check_shiftable_ratio"); + getOrDeclareParameter(*node, ns + "object_check_shiftable_ratio"); p.object_check_min_road_shoulder_width = - get_parameter(node, ns + "object_check_min_road_shoulder_width"); - p.object_last_seen_threshold = get_parameter(node, ns + "object_last_seen_threshold"); + getOrDeclareParameter(*node, ns + "object_check_min_road_shoulder_width"); + p.object_last_seen_threshold = + getOrDeclareParameter(*node, ns + "object_last_seen_threshold"); + } + + { + std::string ns = "avoidance.target_filtering.detection_area."; + p.use_static_detection_area = getOrDeclareParameter(*node, ns + "static"); + p.object_check_min_forward_distance = + getOrDeclareParameter(*node, ns + "min_forward_distance"); + p.object_check_max_forward_distance = + getOrDeclareParameter(*node, ns + "max_forward_distance"); + p.object_check_backward_distance = + getOrDeclareParameter(*node, ns + "backward_distance"); } // safety check { std::string ns = "avoidance.safety_check."; - p.safety_check_hysteresis_factor = - get_parameter(node, ns + "safety_check_hysteresis_factor"); + p.hysteresis_factor_expand_rate = + getOrDeclareParameter(*node, ns + "hysteresis_factor_expand_rate"); } avoidance_parameters_ = std::make_shared(p); diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp index 3db3e049c3c81..de120592e7505 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp @@ -14,14 +14,19 @@ #include "behavior_path_planner/scene_module/lane_change/normal.hpp" -#include "behavior_path_planner/scene_module/scene_module_visitor.hpp" +#include "behavior_path_planner/marker_utils/utils.hpp" #include "behavior_path_planner/utils/lane_change/utils.hpp" #include "behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp" +#include "behavior_path_planner/utils/path_safety_checker/safety_check.hpp" #include "behavior_path_planner/utils/path_utils.hpp" #include "behavior_path_planner/utils/utils.hpp" #include #include +#include + +#include +#include #include #include @@ -32,16 +37,20 @@ namespace behavior_path_planner { +using motion_utils::calcSignedArcLength; + NormalLaneChange::NormalLaneChange( const std::shared_ptr & parameters, LaneChangeModuleType type, Direction direction) : LaneChangeBase(parameters, type, direction) { stop_watch_.tic(getModuleTypeStr()); + stop_watch_.tic("stop_time"); } void NormalLaneChange::updateLaneChangeStatus() { + updateStopTime(); const auto [found_valid_path, found_safe_path] = getSafePath(status_.lane_change_path); // Update status @@ -74,12 +83,22 @@ std::pair NormalLaneChange::getSafePath(LaneChangePath & safe_path) // find candidate paths LaneChangePaths valid_paths{}; - const auto found_safe_path = - getLaneChangePaths(current_lanes, target_lanes, direction_, &valid_paths); + const bool is_stuck = isVehicleStuck(current_lanes); + bool found_safe_path = getLaneChangePaths( + current_lanes, target_lanes, direction_, &valid_paths, lane_change_parameters_->rss_params, + is_stuck); + // if no safe path is found and ego is stuck, try to find a path with a small margin + if (!found_safe_path && is_stuck) { + found_safe_path = getLaneChangePaths( + current_lanes, target_lanes, direction_, &valid_paths, + lane_change_parameters_->rss_params_for_stuck, is_stuck); + } + debug_valid_path_ = valid_paths; if (valid_paths.empty()) { safe_path.info.current_lanes = current_lanes; + safe_path.info.target_lanes = target_lanes; return {false, false}; } @@ -118,7 +137,7 @@ BehaviorModuleOutput NormalLaneChange::generateOutput() const auto found_extended_path = extendPath(); if (found_extended_path) { - *output.path = utils::lane_change::combineReferencePath(*output.path, *found_extended_path); + *output.path = utils::combinePath(*output.path, *found_extended_path); } extendOutputDrivableArea(output); output.reference_path = std::make_shared(getReferencePath()); @@ -132,11 +151,12 @@ BehaviorModuleOutput NormalLaneChange::generateOutput() if (isStopState()) { const auto current_velocity = getEgoVelocity(); - const auto current_dist = motion_utils::calcSignedArcLength( + const auto current_dist = calcSignedArcLength( output.path->points, output.path->points.front().point.pose.position, getEgoPosition()); const auto stop_dist = -(current_velocity * current_velocity / (2.0 * planner_data_->parameters.min_acc)); const auto stop_point = utils::insertStopPoint(stop_dist + current_dist, *output.path); + setStopPose(stop_point.point.pose); } const auto current_seg_idx = planner_data_->findEgoSegmentIndex(output.path->points); @@ -183,20 +203,108 @@ void NormalLaneChange::insertStopPoint( const double lane_change_buffer = utils::calcMinimumLaneChangeLength(getCommonParam(), shift_intervals, 0.0); + const auto getDistanceAlongLanelet = [&](const geometry_msgs::msg::Pose & target) { + return utils::getSignedDistance(path.points.front().point.pose, target, lanelets); + }; + // If lanelets.back() is in goal route section, get distance to goal. // Otherwise, get distance to end of lane. double distance_to_terminal = 0.0; if (route_handler->isInGoalRouteSection(lanelets.back())) { const auto goal = route_handler->getGoalPose(); - distance_to_terminal = utils::getSignedDistance(path.points.front().point.pose, goal, lanelets); + distance_to_terminal = getDistanceAlongLanelet(goal); } else { distance_to_terminal = utils::getDistanceToEndOfLane(path.points.front().point.pose, lanelets); } const double stop_point_buffer = getCommonParam().backward_length_buffer_for_end_of_lane; - const double stopping_distance = distance_to_terminal - lane_change_buffer - stop_point_buffer; + const auto target_objects = getTargetObjects(status_.current_lanes, status_.target_lanes); + double stopping_distance = distance_to_terminal - lane_change_buffer - stop_point_buffer; + + // calculate minimum distance from path front to the stationary object on the ego lane. + const auto distance_to_ego_lane_obj = [&]() -> double { + double distance_to_obj = distance_to_terminal; + const double distance_to_ego = getDistanceAlongLanelet(getEgoPose()); + + for (const auto & object : target_objects.current_lane) { + // check if stationary + const auto obj_v = std::abs(object.initial_twist.twist.linear.x); + if (obj_v > lane_change_parameters_->stop_velocity_threshold) { + continue; + } + + // calculate distance from path front to the stationary object polygon on the ego lane. + const auto polygon = + tier4_autoware_utils::toPolygon2d(object.initial_pose.pose, object.shape).outer(); + for (const auto & polygon_p : polygon) { + const auto p_fp = tier4_autoware_utils::toMsg(polygon_p.to_3d()); + const auto lateral_fp = motion_utils::calcLateralOffset(path.points, p_fp); + + // ignore if the point is around the ego path + if (std::abs(lateral_fp) > planner_data_->parameters.vehicle_width) { + continue; + } + + const double current_distance_to_obj = calcSignedArcLength(path.points, 0, p_fp); + + // ignore backward object + if (current_distance_to_obj < distance_to_ego) { + continue; + } + distance_to_obj = std::min(distance_to_obj, current_distance_to_obj); + } + } + return distance_to_obj; + }(); + + // Need to stop before blocking obstacle + if (distance_to_ego_lane_obj < distance_to_terminal) { + // consider rss distance when the LC need to avoid obstacles + const auto rss_dist = calcRssDistance( + 0.0, planner_data_->parameters.minimum_lane_changing_velocity, + lane_change_parameters_->rss_params); + + const auto stopping_distance_for_obj = distance_to_ego_lane_obj - lane_change_buffer - + stop_point_buffer - rss_dist - + getCommonParam().base_link2front; + + // If the target lane in the lane change section is blocked by a stationary obstacle, there + // is no reason for stopping with a lane change margin. Instead, stop right behind the + // obstacle. + // ---------------------------------------------------------- + // [obj]> + // ---------------------------------------------------------- + // [ego]> | <--- lane change margin ---> [obj]> + // ---------------------------------------------------------- + const bool has_blocking_target_lane_obj = std::any_of( + target_objects.target_lane.begin(), target_objects.target_lane.end(), [&](const auto & o) { + const auto v = std::abs(o.initial_twist.twist.linear.x); + if (v > lane_change_parameters_->stop_velocity_threshold) { + return false; + } + + // target_objects includes objects out of target lanes, so filter them out + if (!boost::geometry::intersects( + tier4_autoware_utils::toPolygon2d(o.initial_pose.pose, o.shape).outer(), + lanelet::utils::combineLaneletsShape(status_.target_lanes) + .polygon2d() + .basicPolygon())) { + return false; + } + + const double distance_to_target_lane_obj = getDistanceAlongLanelet(o.initial_pose.pose); + return stopping_distance_for_obj < distance_to_target_lane_obj && + distance_to_target_lane_obj < distance_to_ego_lane_obj; + }); + + if (!has_blocking_target_lane_obj) { + stopping_distance = stopping_distance_for_obj; + } + } + if (stopping_distance > 0.0) { const auto stop_point = utils::insertStopPoint(stopping_distance, path); + setStopPose(stop_point.point.pose); } } @@ -211,8 +319,7 @@ std::optional NormalLaneChange::extendPath() const auto path = status_.lane_change_path.path; const auto lc_start_point = status_.lane_change_path.info.lane_changing_start.position; - const auto dist = - motion_utils::calcSignedArcLength(path.points, lc_start_point, getEgoPosition()); + const auto dist = calcSignedArcLength(path.points, lc_start_point, getEgoPosition()); if (dist < 0.0) { return std::nullopt; @@ -403,6 +510,12 @@ bool NormalLaneChange::isAbleToReturnCurrentLane() const return false; } + if (!utils::isEgoWithinOriginalLane( + status_.current_lanes, getEgoPose(), planner_data_->parameters, + lane_change_parameters_->cancel.overhang_tolerance)) { + return false; + } + const auto nearest_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( status_.lane_change_path.path.points, getEgoPose(), planner_data_->parameters.ego_nearest_dist_threshold, @@ -414,7 +527,7 @@ bool NormalLaneChange::isAbleToReturnCurrentLane() const double dist = 0.0; for (size_t idx = nearest_idx; idx < status_.lane_change_path.path.points.size() - 1; ++idx) { - dist += motion_utils::calcSignedArcLength(status_.lane_change_path.path.points, idx, idx + 1); + dist += calcSignedArcLength(status_.lane_change_path.path.points, idx, idx + 1); if (dist > estimated_travel_dist) { const auto & estimated_pose = status_.lane_change_path.path.points.at(idx + 1).point.pose; return utils::isEgoWithinOriginalLane( @@ -430,7 +543,7 @@ bool NormalLaneChange::isEgoOnPreparePhase() const { const auto & start_position = status_.lane_change_path.info.shift_line.start.position; const auto & path_points = status_.lane_change_path.path.points; - return motion_utils::calcSignedArcLength(path_points, start_position, getEgoPosition()) < 0.0; + return calcSignedArcLength(path_points, start_position, getEgoPosition()) < 0.0; } bool NormalLaneChange::isAbleToStopSafely() const @@ -450,7 +563,7 @@ bool NormalLaneChange::isAbleToStopSafely() const double dist = 0.0; for (size_t idx = nearest_idx; idx < status_.lane_change_path.path.points.size() - 1; ++idx) { - dist += motion_utils::calcSignedArcLength(status_.lane_change_path.path.points, idx, idx + 1); + dist += calcSignedArcLength(status_.lane_change_path.path.points, idx, idx + 1); if (dist > stop_dist) { const auto & estimated_pose = status_.lane_change_path.path.points.at(idx + 1).point.pose; return utils::isEgoWithinOriginalLane( @@ -466,7 +579,7 @@ bool NormalLaneChange::hasFinishedAbort() const return true; } - const auto distance_to_finish = motion_utils::calcSignedArcLength( + const auto distance_to_finish = calcSignedArcLength( abort_path_->path.points, getEgoPosition(), abort_path_->info.shift_line.end.position); if (distance_to_finish < 0.0) { @@ -499,6 +612,37 @@ int NormalLaneChange::getNumToPreferredLane(const lanelet::ConstLanelet & lane) return std::abs(getRouteHandler()->getNumLaneToPreferredLane(lane, get_opposite_direction)); } +std::pair NormalLaneChange::calcCurrentMinMaxAcceleration() const +{ + const auto & p = getCommonParam(); + + const auto vehicle_min_acc = std::max(p.min_acc, lane_change_parameters_->min_longitudinal_acc); + const auto vehicle_max_acc = std::min(p.max_acc, lane_change_parameters_->max_longitudinal_acc); + + const auto ego_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + prev_module_path_.points, getEgoPose(), p.ego_nearest_dist_threshold, + p.ego_nearest_yaw_threshold); + const auto max_path_velocity = + prev_module_path_.points.at(ego_seg_idx).point.longitudinal_velocity_mps; + + // calculate minimum and maximum acceleration + const auto min_acc = + utils::lane_change::calcMinimumAcceleration(getEgoVelocity(), vehicle_min_acc, p); + const auto max_acc = utils::lane_change::calcMaximumAcceleration( + getEgoVelocity(), max_path_velocity, vehicle_max_acc, p); + + return {min_acc, max_acc}; +} + +double NormalLaneChange::calcMaximumLaneChangeLength( + const lanelet::ConstLanelet & current_terminal_lanelet, const double max_acc) const +{ + const auto shift_intervals = + getRouteHandler()->getLateralIntervalsToPreferredLane(current_terminal_lanelet); + return utils::lane_change::calcMaximumLaneChangeLength( + getEgoVelocity(), getCommonParam(), shift_intervals, max_acc); +} + std::vector NormalLaneChange::sampleLongitudinalAccValues( const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes) const { @@ -506,42 +650,37 @@ std::vector NormalLaneChange::sampleLongitudinalAccValues( return {}; } - const auto & common_parameters = planner_data_->parameters; const auto & route_handler = *getRouteHandler(); const auto current_pose = getEgoPose(); - const auto current_velocity = getEgoVelocity(); - const auto longitudinal_acc_sampling_num = lane_change_parameters_->longitudinal_acc_sampling_num; - const auto vehicle_min_acc = - std::max(common_parameters.min_acc, lane_change_parameters_->min_longitudinal_acc); - const auto vehicle_max_acc = - std::min(common_parameters.max_acc, lane_change_parameters_->max_longitudinal_acc); - const double nearest_dist_threshold = common_parameters.ego_nearest_dist_threshold; - const double nearest_yaw_threshold = common_parameters.ego_nearest_yaw_threshold; - const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - prev_module_path_.points, current_pose, nearest_dist_threshold, nearest_yaw_threshold); - const double & max_path_velocity = - prev_module_path_.points.at(current_seg_idx).point.longitudinal_velocity_mps; - - // calculate minimum and maximum acceleration - const auto min_acc = utils::lane_change::calcMinimumAcceleration( - current_velocity, vehicle_min_acc, common_parameters); - const auto max_acc = utils::lane_change::calcMaximumAcceleration( - current_velocity, max_path_velocity, vehicle_max_acc, common_parameters); + const auto [min_acc, max_acc] = calcCurrentMinMaxAcceleration(); // if max acc is not positive, then we do the normal sampling if (max_acc <= 0.0) { + RCLCPP_DEBUG( + logger_, "Available max acc <= 0. Normal sampling for acc: [%f ~ %f]", min_acc, max_acc); return utils::lane_change::getAccelerationValues( min_acc, max_acc, longitudinal_acc_sampling_num); } // calculate maximum lane change length - const double max_lane_change_length = utils::lane_change::calcMaximumLaneChangeLength( - current_velocity, common_parameters, - route_handler.getLateralIntervalsToPreferredLane(current_lanes.back()), max_acc); + const double max_lane_change_length = calcMaximumLaneChangeLength(current_lanes.back(), max_acc); if (max_lane_change_length > utils::getDistanceToEndOfLane(current_pose, current_lanes)) { + RCLCPP_DEBUG( + logger_, "No enough distance to the end of lane. Normal sampling for acc: [%f ~ %f]", min_acc, + max_acc); + return utils::lane_change::getAccelerationValues( + min_acc, max_acc, longitudinal_acc_sampling_num); + } + + // If the ego is in stuck, sampling all possible accelerations to find avoiding path. + if (isVehicleStuck(current_lanes)) { + auto clock = rclcpp::Clock(RCL_ROS_TIME); + RCLCPP_INFO_THROTTLE( + logger_, clock, 1000, "Vehicle is in stuck. Sample all possible acc: [%f ~ %f]", min_acc, + max_acc); return utils::lane_change::getAccelerationValues( min_acc, max_acc, longitudinal_acc_sampling_num); } @@ -551,28 +690,40 @@ std::vector NormalLaneChange::sampleLongitudinalAccValues( if (route_handler.isInGoalRouteSection(target_lanes.back())) { const auto goal_pose = route_handler.getGoalPose(); if (max_lane_change_length < utils::getSignedDistance(current_pose, goal_pose, target_lanes)) { + RCLCPP_DEBUG( + logger_, "Distance to goal has enough distance. Sample only max_acc: %f", max_acc); return {max_acc}; } } else if (max_lane_change_length < utils::getDistanceToEndOfLane(current_pose, target_lanes)) { + RCLCPP_DEBUG( + logger_, "Distance to end of lane has enough distance. Sample only max_acc: %f", max_acc); return {max_acc}; } + RCLCPP_DEBUG(logger_, "Normal sampling for acc: [%f ~ %f]", min_acc, max_acc); return utils::lane_change::getAccelerationValues(min_acc, max_acc, longitudinal_acc_sampling_num); } -double NormalLaneChange::calcPrepareDuration( +std::vector NormalLaneChange::calcPrepareDuration( const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes) const { const auto & common_parameters = planner_data_->parameters; - const auto threshold = lane_change_parameters_->min_length_for_turn_signal_activation; - const auto current_vel = getEgoVelocity(); + const auto base_link2front = planner_data_->parameters.base_link2front; + const auto threshold = + lane_change_parameters_->min_length_for_turn_signal_activation + base_link2front; + + std::vector prepare_durations; + constexpr double step = 0.5; - // if the ego vehicle is close to the end of the lane at a low speed - if (isNearEndOfCurrentLanes(current_lanes, target_lanes, threshold) && current_vel < 1.0) { - return 0.0; + for (double duration = common_parameters.lane_change_prepare_duration; duration >= 0.0; + duration -= step) { + prepare_durations.push_back(duration); + if (!isNearEndOfCurrentLanes(current_lanes, target_lanes, threshold)) { + break; + } } - return common_parameters.lane_change_prepare_duration; + return prepare_durations; } PathWithLaneId NormalLaneChange::getPrepareSegment( @@ -597,7 +748,9 @@ LaneChangeTargetObjects NormalLaneChange::getTargetObjects( const auto current_pose = getEgoPose(); const auto & route_handler = *getRouteHandler(); const auto & common_parameters = planner_data_->parameters; - const auto & objects = *planner_data_->dynamic_object; + auto objects = *planner_data_->dynamic_object; + utils::path_safety_checker::filterObjectsByClass( + objects, lane_change_parameters_->object_types_to_check); // get backward lanes const auto backward_length = lane_change_parameters_->backward_lane_length; @@ -605,7 +758,8 @@ LaneChangeTargetObjects NormalLaneChange::getTargetObjects( route_handler, target_lanes, current_pose, backward_length); // filter objects to get target object index - const auto target_obj_index = filterObject(current_lanes, target_lanes, target_backward_lanes); + const auto target_obj_index = + filterObject(objects, current_lanes, target_lanes, target_backward_lanes); LaneChangeTargetObjects target_objects; target_objects.current_lane.reserve(target_obj_index.current_lane.size()); @@ -637,14 +791,13 @@ LaneChangeTargetObjects NormalLaneChange::getTargetObjects( } LaneChangeTargetObjectIndices NormalLaneChange::filterObject( - const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, + const PredictedObjects & objects, const lanelet::ConstLanelets & current_lanes, + const lanelet::ConstLanelets & target_lanes, const lanelet::ConstLanelets & target_backward_lanes) const { const auto current_pose = getEgoPose(); - const auto current_vel = getEgoVelocity(); const auto & route_handler = *getRouteHandler(); const auto & common_parameters = planner_data_->parameters; - const auto & objects = *planner_data_->dynamic_object; // Guard if (objects.objects.empty()) { @@ -659,44 +812,61 @@ LaneChangeTargetObjectIndices NormalLaneChange::filterObject( const auto current_polygon = utils::lane_change::createPolygon(current_lanes, 0.0, std::numeric_limits::max()); - const auto target_polygon = - utils::lane_change::createPolygon(target_lanes, 0.0, std::numeric_limits::max()); - const auto target_backward_polygon = utils::lane_change::createPolygon( - target_backward_lanes, 0.0, std::numeric_limits::max()); + const auto expanded_target_lanes = utils::lane_change::generateExpandedLanelets( + target_lanes, direction_, lane_change_parameters_->lane_expansion_left_offset, + lane_change_parameters_->lane_expansion_right_offset); + const auto target_polygon = utils::lane_change::createPolygon( + expanded_target_lanes, 0.0, std::numeric_limits::max()); + const auto dist_ego_to_current_lanes_center = + lanelet::utils::getLateralDistanceToClosestLanelet(current_lanes, current_pose); + std::vector> target_backward_polygons; + for (const auto & target_backward_lane : target_backward_lanes) { + // Check to see is target_backward_lane is in current_lanes + // Without this check, current lane object might be treated as target lane object + const auto is_current_lane = [&](const lanelet::ConstLanelet & current_lane) { + return current_lane.id() == target_backward_lane.id(); + }; + + if (std::any_of(current_lanes.begin(), current_lanes.end(), is_current_lane)) { + continue; + } - // minimum distance to lane changing start point - const double t_prepare = common_parameters.lane_change_prepare_duration; - const double a_min = lane_change_parameters_->min_longitudinal_acc; - const double min_dist_to_lane_changing_start = std::max( - current_vel * t_prepare, current_vel * t_prepare + 0.5 * a_min * t_prepare * t_prepare); + lanelet::ConstLanelets lanelet{target_backward_lane}; + auto lane_polygon = + utils::lane_change::createPolygon(lanelet, 0.0, std::numeric_limits::max()); + target_backward_polygons.push_back(lane_polygon); + } LaneChangeTargetObjectIndices filtered_obj_indices; for (size_t i = 0; i < objects.objects.size(); ++i) { const auto & object = objects.objects.at(i); - const auto & obj_velocity_norm = std::hypot( + const auto obj_velocity_norm = std::hypot( object.kinematics.initial_twist_with_covariance.twist.linear.x, object.kinematics.initial_twist_with_covariance.twist.linear.y); const auto extended_object = utils::lane_change::transform(object, common_parameters, *lane_change_parameters_); - // ignore specific object types - if (!utils::lane_change::isTargetObjectType(object, *lane_change_parameters_)) { - continue; - } - const auto obj_polygon = tier4_autoware_utils::toPolygon2d(object); // calc distance from the current ego position double max_dist_ego_to_obj = std::numeric_limits::lowest(); double min_dist_ego_to_obj = std::numeric_limits::max(); - for (const auto & polygon_p : obj_polygon.outer()) { + const auto obj_polygon_outer = obj_polygon.outer(); + for (const auto & polygon_p : obj_polygon_outer) { const auto obj_p = tier4_autoware_utils::createPoint(polygon_p.x(), polygon_p.y(), 0.0); - const double dist_ego_to_obj = - motion_utils::calcSignedArcLength(path.points, current_pose.position, obj_p); + const double dist_ego_to_obj = calcSignedArcLength(path.points, current_pose.position, obj_p); max_dist_ego_to_obj = std::max(dist_ego_to_obj, max_dist_ego_to_obj); min_dist_ego_to_obj = std::min(dist_ego_to_obj, min_dist_ego_to_obj); } + const auto is_lateral_far = [&]() { + const auto dist_object_to_current_lanes_center = + lanelet::utils::getLateralDistanceToClosestLanelet( + current_lanes, object.kinematics.initial_pose_with_covariance.pose); + const auto lateral = dist_object_to_current_lanes_center - dist_ego_to_current_lanes_center; + return std::abs(lateral) > (common_parameters.vehicle_width / 2); + }; + // ignore static object that are behind the ego vehicle if (obj_velocity_norm < 1.0 && max_dist_ego_to_obj < 0.0) { continue; @@ -704,21 +874,25 @@ LaneChangeTargetObjectIndices NormalLaneChange::filterObject( // check if the object intersects with target lanes if (target_polygon && boost::geometry::intersects(target_polygon.value(), obj_polygon)) { - // ignore static parked object that are in front of the ego vehicle in target lanes - bool is_parked_object = - utils::lane_change::isParkedObject(target_path, route_handler, extended_object, 0.0, 0.0); - if (is_parked_object && min_dist_ego_to_obj > min_dist_to_lane_changing_start) { + // TODO(watanabe): ignore static parked object that are in front of the ego vehicle in target + // lanes + + if (max_dist_ego_to_obj >= 0 || is_lateral_far()) { + filtered_obj_indices.target_lane.push_back(i); continue; } - - filtered_obj_indices.target_lane.push_back(i); - continue; } + const auto check_backward_polygon = [&obj_polygon](const auto & target_backward_polygon) { + return target_backward_polygon && + boost::geometry::intersects(target_backward_polygon.value(), obj_polygon); + }; + // check if the object intersects with target backward lanes if ( - target_backward_polygon && - boost::geometry::intersects(target_backward_polygon.value(), obj_polygon)) { + !target_backward_polygons.empty() && + std::any_of( + target_backward_polygons.begin(), target_backward_polygons.end(), check_backward_polygon)) { filtered_obj_indices.target_lane.push_back(i); continue; } @@ -792,6 +966,7 @@ bool NormalLaneChange::hasEnoughLength( { const auto current_pose = getEgoPose(); const auto & route_handler = *getRouteHandler(); + const auto overall_graphs_ptr = route_handler.getOverallGraphPtr(); const auto & common_parameters = planner_data_->parameters; const double lane_change_length = path.info.length.sum(); const auto shift_intervals = @@ -825,12 +1000,54 @@ bool NormalLaneChange::hasEnoughLength( return true; } +bool NormalLaneChange::hasEnoughLengthToCrosswalk( + const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes) const +{ + const auto current_pose = getEgoPose(); + const auto & route_handler = *getRouteHandler(); + const auto overall_graphs_ptr = route_handler.getOverallGraphPtr(); + + const double dist_to_crosswalk_from_lane_change_start_pose = + utils::getDistanceToCrosswalk(current_pose, current_lanes, *overall_graphs_ptr) - + path.info.length.prepare; + // Check lane changing section includes crosswalk + if ( + dist_to_crosswalk_from_lane_change_start_pose > 0.0 && + dist_to_crosswalk_from_lane_change_start_pose < path.info.length.lane_changing) { + return false; + } + + return true; +} + +bool NormalLaneChange::hasEnoughLengthToIntersection( + const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes) const +{ + const auto current_pose = getEgoPose(); + const auto & route_handler = *getRouteHandler(); + const auto overall_graphs_ptr = route_handler.getOverallGraphPtr(); + + const double dist_to_intersection_from_lane_change_start_pose = + utils::getDistanceToNextIntersection(current_pose, current_lanes) - path.info.length.prepare; + // Check lane changing section includes intersection + if ( + dist_to_intersection_from_lane_change_start_pose > 0.0 && + dist_to_intersection_from_lane_change_start_pose < path.info.length.lane_changing) { + return false; + } + + return true; +} + bool NormalLaneChange::getLaneChangePaths( const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, - Direction direction, LaneChangePaths * candidate_paths, const bool check_safety) const + Direction direction, LaneChangePaths * candidate_paths, + const utils::path_safety_checker::RSSparams rss_params, const bool is_stuck, + const bool check_safety) const { object_debug_.clear(); if (current_lanes.empty() || target_lanes.empty()) { + RCLCPP_WARN(logger_, "target_neighbor_preferred_lane_poly_2d is empty. Not expected."); return false; } const auto & route_handler = *getRouteHandler(); @@ -865,189 +1082,250 @@ bool NormalLaneChange::getLaneChangePaths( const auto target_neighbor_preferred_lane_poly_2d = utils::lane_change::getTargetNeighborLanesPolygon(route_handler, current_lanes, type_); + if (target_neighbor_preferred_lane_poly_2d.empty()) { + RCLCPP_WARN(logger_, "target_neighbor_preferred_lane_poly_2d is empty. Not expected."); + return false; + } const auto target_objects = getTargetObjects(current_lanes, target_lanes); + debug_filtered_objects_ = target_objects; - candidate_paths->reserve(longitudinal_acc_sampling_values.size() * lateral_acc_sampling_num); - - const auto prepare_duration = calcPrepareDuration(current_lanes, target_lanes); + const auto prepare_durations = calcPrepareDuration(current_lanes, target_lanes); - for (const auto & sampled_longitudinal_acc : longitudinal_acc_sampling_values) { - // get path on original lanes - const auto prepare_velocity = std::max( - current_velocity + sampled_longitudinal_acc * prepare_duration, - minimum_lane_changing_velocity); + candidate_paths->reserve( + longitudinal_acc_sampling_values.size() * lateral_acc_sampling_num * prepare_durations.size()); - // compute actual longitudinal acceleration - const double longitudinal_acc_on_prepare = - (prepare_duration < 1e-3) ? 0.0 : ((prepare_velocity - current_velocity) / prepare_duration); + RCLCPP_DEBUG( + logger_, "lane change sampling start. Sampling num for prep_time: %lu, acc: %lu", + prepare_durations.size(), longitudinal_acc_sampling_values.size()); + + for (const auto & prepare_duration : prepare_durations) { + for (const auto & sampled_longitudinal_acc : longitudinal_acc_sampling_values) { + const auto debug_print = [&](const auto & s) { + RCLCPP_DEBUG_STREAM( + logger_, " - " << s << " : prep_time = " << prepare_duration + << ", lon_acc = " << sampled_longitudinal_acc); + }; + + // get path on original lanes + const auto prepare_velocity = std::max( + current_velocity + sampled_longitudinal_acc * prepare_duration, + minimum_lane_changing_velocity); + + // compute actual longitudinal acceleration + const double longitudinal_acc_on_prepare = + (prepare_duration < 1e-3) ? 0.0 + : ((prepare_velocity - current_velocity) / prepare_duration); + + const double prepare_length = + current_velocity * prepare_duration + + 0.5 * longitudinal_acc_on_prepare * std::pow(prepare_duration, 2); + + auto prepare_segment = getPrepareSegment(current_lanes, backward_path_length, prepare_length); + + if (prepare_segment.points.empty()) { + debug_print("prepare segment is empty...? Unexpected."); + continue; + } - const double prepare_length = current_velocity * prepare_duration + - 0.5 * longitudinal_acc_on_prepare * std::pow(prepare_duration, 2); + // lane changing start getEgoPose() is at the end of prepare segment + const auto & lane_changing_start_pose = prepare_segment.points.back().point.pose; + const auto target_length_from_lane_change_start_pose = utils::getArcLengthToTargetLanelet( + current_lanes, target_lanes.front(), lane_changing_start_pose); - auto prepare_segment = getPrepareSegment(current_lanes, backward_path_length, prepare_length); + // Check if the lane changing start point is not on the lanes next to target lanes, + if (target_length_from_lane_change_start_pose > 0.0) { + debug_print("lane change start getEgoPose() is behind target lanelet!"); + break; + } - if (prepare_segment.points.empty()) { - RCLCPP_DEBUG(logger_, "prepare segment is empty!!"); - continue; - } + const auto shift_length = + lanelet::utils::getLateralDistanceToClosestLanelet(target_lanes, lane_changing_start_pose); - // lane changing start getEgoPose() is at the end of prepare segment - const auto & lane_changing_start_pose = prepare_segment.points.back().point.pose; - const auto target_length_from_lane_change_start_pose = utils::getArcLengthToTargetLanelet( - current_lanes, target_lanes.front(), lane_changing_start_pose); + const auto initial_lane_changing_velocity = prepare_velocity; + const auto max_path_velocity = prepare_segment.points.back().point.longitudinal_velocity_mps; - // Check if the lane changing start point is not on the lanes next to target lanes, - if (target_length_from_lane_change_start_pose > 0.0) { - RCLCPP_DEBUG( - logger_, "[only new arch] lane change start getEgoPose() is behind target lanelet!!"); - break; - } + // get lateral acceleration range + const auto [min_lateral_acc, max_lateral_acc] = + common_parameters.lane_change_lat_acc_map.find(initial_lane_changing_velocity); + const auto lateral_acc_resolution = + std::abs(max_lateral_acc - min_lateral_acc) / lateral_acc_sampling_num; - const auto shift_length = - lanelet::utils::getLateralDistanceToClosestLanelet(target_lanes, lane_changing_start_pose); - - const auto initial_lane_changing_velocity = prepare_velocity; - const auto & max_path_velocity = prepare_segment.points.back().point.longitudinal_velocity_mps; - - // get lateral acceleration range - const auto [min_lateral_acc, max_lateral_acc] = - common_parameters.lane_change_lat_acc_map.find(initial_lane_changing_velocity); - const auto lateral_acc_resolution = - std::abs(max_lateral_acc - min_lateral_acc) / lateral_acc_sampling_num; - constexpr double lateral_acc_epsilon = 0.01; - - for (double lateral_acc = min_lateral_acc; lateral_acc < max_lateral_acc + lateral_acc_epsilon; - lateral_acc += lateral_acc_resolution) { - const auto lane_changing_time = PathShifter::calcShiftTimeFromJerk( - shift_length, common_parameters.lane_changing_lateral_jerk, lateral_acc); - const double longitudinal_acc_on_lane_changing = - utils::lane_change::calcLaneChangingAcceleration( - initial_lane_changing_velocity, max_path_velocity, lane_changing_time, - sampled_longitudinal_acc); - const auto lane_changing_length = - initial_lane_changing_velocity * lane_changing_time + - 0.5 * longitudinal_acc_on_lane_changing * lane_changing_time * lane_changing_time; - const auto terminal_lane_changing_velocity = - initial_lane_changing_velocity + longitudinal_acc_on_lane_changing * lane_changing_time; - utils::lane_change::setPrepareVelocity( - prepare_segment, current_velocity, terminal_lane_changing_velocity); - - if (lane_changing_length + prepare_length > dist_to_end_of_current_lanes) { - RCLCPP_DEBUG(logger_, "length of lane changing path is longer than length to goal!!"); - continue; + std::vector sample_lat_acc; + constexpr double eps = 0.01; + for (double a = min_lateral_acc; a < max_lateral_acc + eps; a += lateral_acc_resolution) { + sample_lat_acc.push_back(a); } - - if (is_goal_in_route) { - const double s_start = - lanelet::utils::getArcCoordinates(target_lanes, lane_changing_start_pose).length; - const double s_goal = - lanelet::utils::getArcCoordinates(target_lanes, route_handler.getGoalPose()).length; - const auto num = - std::abs(route_handler.getNumLaneToPreferredLane(target_lanes.back(), direction)); - const double backward_buffer = - num == 0 ? 0.0 : common_parameters.backward_length_buffer_for_end_of_lane; - const double finish_judge_buffer = common_parameters.lane_change_finish_judge_buffer; - if ( - s_start + lane_changing_length + finish_judge_buffer + backward_buffer + - next_lane_change_buffer > - s_goal) { - RCLCPP_DEBUG(logger_, "length of lane changing path is longer than length to goal!!"); + RCLCPP_DEBUG(logger_, " - sampling num for lat_acc: %lu", sample_lat_acc.size()); + + for (const auto & lateral_acc : sample_lat_acc) { + const auto debug_print = [&](const auto & s) { + RCLCPP_DEBUG_STREAM( + logger_, " - " << s << " : prep_time = " << prepare_duration << ", lon_acc = " + << sampled_longitudinal_acc << ", lat_acc = " << lateral_acc); + }; + + const auto lane_changing_time = PathShifter::calcShiftTimeFromJerk( + shift_length, common_parameters.lane_changing_lateral_jerk, lateral_acc); + const double longitudinal_acc_on_lane_changing = + utils::lane_change::calcLaneChangingAcceleration( + initial_lane_changing_velocity, max_path_velocity, lane_changing_time, + sampled_longitudinal_acc); + const auto lane_changing_length = + initial_lane_changing_velocity * lane_changing_time + + 0.5 * longitudinal_acc_on_lane_changing * lane_changing_time * lane_changing_time; + const auto terminal_lane_changing_velocity = + initial_lane_changing_velocity + longitudinal_acc_on_lane_changing * lane_changing_time; + utils::lane_change::setPrepareVelocity( + prepare_segment, current_velocity, terminal_lane_changing_velocity); + + if (lane_changing_length + prepare_length > dist_to_end_of_current_lanes) { + debug_print("Reject: length of lane changing path is longer than length to goal!!"); continue; } - } - const auto target_segment = getTargetSegment( - target_lanes, lane_changing_start_pose, target_lane_length, lane_changing_length, - initial_lane_changing_velocity, next_lane_change_buffer); + if (is_goal_in_route) { + const double s_start = + lanelet::utils::getArcCoordinates(target_lanes, lane_changing_start_pose).length; + const double s_goal = + lanelet::utils::getArcCoordinates(target_lanes, route_handler.getGoalPose()).length; + const auto num = + std::abs(route_handler.getNumLaneToPreferredLane(target_lanes.back(), direction)); + const double backward_buffer = + num == 0 ? 0.0 : common_parameters.backward_length_buffer_for_end_of_lane; + const double finish_judge_buffer = common_parameters.lane_change_finish_judge_buffer; + if ( + s_start + lane_changing_length + finish_judge_buffer + backward_buffer + + next_lane_change_buffer > + s_goal) { + debug_print("Reject: length of lane changing path is longer than length to goal!!"); + continue; + } + } - if (target_segment.points.empty()) { - RCLCPP_DEBUG(logger_, "target segment is empty!! something wrong..."); - continue; - } + const auto target_segment = getTargetSegment( + target_lanes, lane_changing_start_pose, target_lane_length, lane_changing_length, + initial_lane_changing_velocity, next_lane_change_buffer); - const lanelet::BasicPoint2d lc_start_point( - prepare_segment.points.back().point.pose.position.x, - prepare_segment.points.back().point.pose.position.y); + if (target_segment.points.empty()) { + debug_print("Reject: target segment is empty!! something wrong..."); + continue; + } - const auto target_lane_polygon = lanelet::utils::getPolygonFromArcLength( - target_lanes, 0, std::numeric_limits::max()); - const auto target_lane_poly_2d = lanelet::utils::to2D(target_lane_polygon).basicPolygon(); + const lanelet::BasicPoint2d lc_start_point( + prepare_segment.points.back().point.pose.position.x, + prepare_segment.points.back().point.pose.position.y); - const auto is_valid_start_point = - boost::geometry::covered_by(lc_start_point, target_neighbor_preferred_lane_poly_2d) || - boost::geometry::covered_by(lc_start_point, target_lane_poly_2d); + const auto target_lane_polygon = lanelet::utils::getPolygonFromArcLength( + target_lanes, 0, std::numeric_limits::max()); + const auto target_lane_poly_2d = lanelet::utils::to2D(target_lane_polygon).basicPolygon(); - if (!is_valid_start_point) { - // lane changing points are not inside of the target preferred lanes or its neighbors - continue; - } + const auto is_valid_start_point = + boost::geometry::covered_by(lc_start_point, target_neighbor_preferred_lane_poly_2d) || + boost::geometry::covered_by(lc_start_point, target_lane_poly_2d); - const auto resample_interval = utils::lane_change::calcLaneChangeResampleInterval( - lane_changing_length, initial_lane_changing_velocity); - const auto target_lane_reference_path = utils::lane_change::getReferencePathFromTargetLane( - route_handler, target_lanes, lane_changing_start_pose, target_lane_length, - lane_changing_length, forward_path_length, resample_interval, is_goal_in_route, - next_lane_change_buffer); + if (!is_valid_start_point) { + debug_print( + "Reject: lane changing points are not inside of the target preferred lanes or its " + "neighbors"); + continue; + } - if (target_lane_reference_path.points.empty()) { - RCLCPP_DEBUG(logger_, "target_lane_reference_path is empty!!"); - continue; - } + const auto resample_interval = utils::lane_change::calcLaneChangeResampleInterval( + lane_changing_length, initial_lane_changing_velocity); + const auto target_lane_reference_path = utils::lane_change::getReferencePathFromTargetLane( + route_handler, target_lanes, lane_changing_start_pose, target_lane_length, + lane_changing_length, forward_path_length, resample_interval, is_goal_in_route, + next_lane_change_buffer); - LaneChangeInfo lane_change_info; - lane_change_info.longitudinal_acceleration = - LaneChangePhaseInfo{longitudinal_acc_on_prepare, longitudinal_acc_on_lane_changing}; - lane_change_info.duration = LaneChangePhaseInfo{prepare_duration, lane_changing_time}; - lane_change_info.velocity = - LaneChangePhaseInfo{prepare_velocity, initial_lane_changing_velocity}; - lane_change_info.length = LaneChangePhaseInfo{prepare_length, lane_changing_length}; - lane_change_info.current_lanes = current_lanes; - lane_change_info.target_lanes = target_lanes; - lane_change_info.lane_changing_start = prepare_segment.points.back().point.pose; - lane_change_info.lane_changing_end = target_segment.points.front().point.pose; - lane_change_info.shift_line = utils::lane_change::getLaneChangingShiftLine( - prepare_segment, target_segment, target_lane_reference_path, shift_length); - lane_change_info.lateral_acceleration = lateral_acc; - lane_change_info.terminal_lane_changing_velocity = terminal_lane_changing_velocity; - - const auto candidate_path = utils::lane_change::constructCandidatePath( - lane_change_info, prepare_segment, target_segment, target_lane_reference_path, - sorted_lane_ids); - - if (!candidate_path) { - RCLCPP_DEBUG(logger_, "no candidate path!!"); - continue; - } + if (target_lane_reference_path.points.empty()) { + debug_print("Reject: target_lane_reference_path is empty!!"); + continue; + } - const auto is_valid = - hasEnoughLength(*candidate_path, current_lanes, target_lanes, direction); + LaneChangeInfo lane_change_info; + lane_change_info.longitudinal_acceleration = + LaneChangePhaseInfo{longitudinal_acc_on_prepare, longitudinal_acc_on_lane_changing}; + lane_change_info.duration = LaneChangePhaseInfo{prepare_duration, lane_changing_time}; + lane_change_info.velocity = + LaneChangePhaseInfo{prepare_velocity, initial_lane_changing_velocity}; + lane_change_info.length = LaneChangePhaseInfo{prepare_length, lane_changing_length}; + lane_change_info.current_lanes = current_lanes; + lane_change_info.target_lanes = target_lanes; + lane_change_info.lane_changing_start = prepare_segment.points.back().point.pose; + lane_change_info.lane_changing_end = target_segment.points.front().point.pose; + lane_change_info.shift_line = utils::lane_change::getLaneChangingShiftLine( + prepare_segment, target_segment, target_lane_reference_path, shift_length); + lane_change_info.lateral_acceleration = lateral_acc; + lane_change_info.terminal_lane_changing_velocity = terminal_lane_changing_velocity; + + const auto candidate_path = utils::lane_change::constructCandidatePath( + lane_change_info, prepare_segment, target_segment, target_lane_reference_path, + sorted_lane_ids); + + if (!candidate_path) { + debug_print("Reject: failed to generate candidate path!!"); + continue; + } - if (!is_valid) { - RCLCPP_DEBUG(logger_, "invalid candidate path!!"); - continue; - } + if (!hasEnoughLength(*candidate_path, current_lanes, target_lanes, direction)) { + debug_print("Reject: invalid candidate path!!"); + continue; + } - if (utils::lane_change::passParkedObject( + if ( + lane_change_parameters_->regulate_on_crosswalk && + !hasEnoughLengthToCrosswalk(*candidate_path, current_lanes)) { + if (getStopTime() < lane_change_parameters_->stop_time_threshold) { + debug_print("Reject: including crosswalk!!"); + continue; + } + RCLCPP_INFO_THROTTLE( + logger_, clock_, 1000, "Stop time is over threshold. Allow lane change in crosswalk."); + } + + if ( + lane_change_parameters_->regulate_on_intersection && + !hasEnoughLengthToIntersection(*candidate_path, current_lanes)) { + if (getStopTime() < lane_change_parameters_->stop_time_threshold) { + debug_print("Reject: including intersection!!"); + continue; + } + RCLCPP_WARN_STREAM( + logger_, "Stop time is over threshold. Allow lane change in intersection."); + } + + candidate_paths->push_back(*candidate_path); + if ( + !is_stuck && + utils::lane_change::passParkedObject( route_handler, *candidate_path, target_objects.target_lane, lane_change_buffer, - is_goal_in_route, *lane_change_parameters_)) { - return false; - } - candidate_paths->push_back(*candidate_path); + is_goal_in_route, *lane_change_parameters_, object_debug_)) { + debug_print( + "Reject: parking vehicle exists in the target lane, and the ego is not in stuck. Skip " + "lane change."); + return false; + } - if (!check_safety) { - return false; - } + if (!check_safety) { + debug_print("ACCEPT!!!: it is valid (and safety check is skipped)."); + return false; + } - const auto [is_safe, is_object_coming_from_rear] = isLaneChangePathSafe( - *candidate_path, target_objects, lane_change_parameters_->rss_params, object_debug_); + const auto [is_safe, is_object_coming_from_rear] = isLaneChangePathSafe( + *candidate_path, target_objects, rss_params, is_stuck, object_debug_); - if (is_safe) { - return true; + if (is_safe) { + debug_print("ACCEPT!!!: it is valid and safe!"); + return true; + } + + debug_print("Reject: sampled path is not safe."); } } } + RCLCPP_DEBUG(logger_, "No safety path found."); return false; } @@ -1058,10 +1336,12 @@ PathSafetyStatus NormalLaneChange::isApprovedPathSafe() const const auto & target_lanes = status_.target_lanes; const auto target_objects = getTargetObjects(current_lanes, target_lanes); + debug_filtered_objects_ = target_objects; CollisionCheckDebugMap debug_data; + const bool is_stuck = isVehicleStuck(current_lanes); const auto safety_status = isLaneChangePathSafe( - path, target_objects, lane_change_parameters_->rss_params_for_abort, debug_data); + path, target_objects, lane_change_parameters_->rss_params_for_abort, is_stuck, debug_data); { // only for debug purpose object_debug_.clear(); @@ -1318,8 +1598,8 @@ bool NormalLaneChange::getAbortPath() PathSafetyStatus NormalLaneChange::isLaneChangePathSafe( const LaneChangePath & lane_change_path, const LaneChangeTargetObjects & target_objects, - const utils::path_safety_checker::RSSparams & rss_params, - std::unordered_map & debug_data) const + const utils::path_safety_checker::RSSparams & rss_params, const bool is_stuck, + CollisionCheckDebugMap & debug_data) const { PathSafetyStatus path_safety_status; @@ -1338,11 +1618,12 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe( const auto ego_predicted_path = utils::lane_change::convertToPredictedPath( lane_change_path, current_twist, current_pose, common_parameters, time_resolution); const auto debug_predicted_path = - utils::lane_change::convertToPredictedPath(ego_predicted_path, time_resolution); + utils::path_safety_checker::convertToPredictedPath(ego_predicted_path, time_resolution); auto collision_check_objects = target_objects.target_lane; + const auto current_lanes = getCurrentLanes(); - if (lane_change_parameters_->check_objects_on_current_lanes) { + if (lane_change_parameters_->check_objects_on_current_lanes || is_stuck) { collision_check_objects.insert( collision_check_objects.end(), target_objects.current_lane.begin(), target_objects.current_lane.end()); @@ -1354,46 +1635,158 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe( target_objects.other_lane.end()); } - const auto assignDebugData = [](const ExtendedPredictedObject & obj) { - CollisionCheckDebug debug; - debug.current_pose = obj.initial_pose.pose; - debug.current_twist = obj.initial_twist.twist; - return std::make_pair(tier4_autoware_utils::toHexString(obj.uuid), debug); - }; - - const auto updateDebugInfo = - [&debug_data](std::pair & obj, bool is_allowed) { - const auto & key = obj.first; - auto & element = obj.second; - element.allow_lane_change = is_allowed; - if (debug_data.find(key) != debug_data.end()) { - debug_data[key] = element; - } else { - debug_data.insert(obj); - } - }; + const auto expanded_target_lanes = utils::lane_change::generateExpandedLanelets( + lane_change_path.info.target_lanes, direction_, + lane_change_parameters_->lane_expansion_left_offset, + lane_change_parameters_->lane_expansion_right_offset); for (const auto & obj : collision_check_objects) { - auto current_debug_data = assignDebugData(obj); - current_debug_data.second.ego_predicted_path.push_back(debug_predicted_path); + auto current_debug_data = marker_utils::createObjectDebug(obj); const auto obj_predicted_paths = utils::path_safety_checker::getPredictedPathFromObj( obj, lane_change_parameters_->use_all_predicted_path); + auto is_safe = true; for (const auto & obj_path : obj_predicted_paths) { - if (!utils::path_safety_checker::checkCollision( - path, ego_predicted_path, obj, obj_path, common_parameters, rss_params, - current_debug_data.second)) { - path_safety_status.is_safe = false; - updateDebugInfo(current_debug_data, path_safety_status.is_safe); - const auto & obj_pose = obj.initial_pose.pose; - const auto obj_polygon = tier4_autoware_utils::toPolygon2d(obj_pose, obj.shape); - path_safety_status.is_object_coming_from_rear |= - !utils::path_safety_checker::isTargetObjectFront( - path, current_pose, common_parameters.vehicle_info, obj_polygon); + const auto collided_polygons = utils::path_safety_checker::getCollidedPolygons( + path, ego_predicted_path, obj, obj_path, common_parameters, rss_params, 1.0, + current_debug_data.second); + + if (collided_polygons.empty()) { + marker_utils::updateCollisionCheckDebugMap(debug_data, current_debug_data, is_safe); + continue; } + + const auto collision_in_current_lanes = utils::lane_change::isCollidedPolygonsInLanelet( + collided_polygons, lane_change_path.info.current_lanes); + const auto collision_in_target_lanes = + utils::lane_change::isCollidedPolygonsInLanelet(collided_polygons, expanded_target_lanes); + + if (!collision_in_current_lanes && !collision_in_target_lanes) { + marker_utils::updateCollisionCheckDebugMap(debug_data, current_debug_data, is_safe); + continue; + } + + is_safe = false; + path_safety_status.is_safe = false; + marker_utils::updateCollisionCheckDebugMap(debug_data, current_debug_data, is_safe); + const auto & obj_pose = obj.initial_pose.pose; + const auto obj_polygon = tier4_autoware_utils::toPolygon2d(obj_pose, obj.shape); + path_safety_status.is_object_coming_from_rear |= + !utils::path_safety_checker::isTargetObjectFront( + path, current_pose, common_parameters.vehicle_info, obj_polygon); } - updateDebugInfo(current_debug_data, path_safety_status.is_safe); + marker_utils::updateCollisionCheckDebugMap(debug_data, current_debug_data, is_safe); } return path_safety_status; } + +// Check if the ego vehicle is in stuck by a stationary obstacle or by the terminal of current lanes +bool NormalLaneChange::isVehicleStuck( + const lanelet::ConstLanelets & current_lanes, const double obstacle_check_distance) const +{ + // Ego is still moving, not in stuck + if (std::abs(getEgoVelocity()) > lane_change_parameters_->stop_velocity_threshold) { + RCLCPP_DEBUG(logger_, "Ego is still moving, not in stuck"); + return false; + } + + // Ego is just stopped, not sure it is in stuck yet. + if (getStopTime() < lane_change_parameters_->stop_time_threshold) { + RCLCPP_DEBUG(logger_, "Ego is just stopped, counting for stuck judge... (%f)", getStopTime()); + return false; + } + + // Check if any stationary object exist in obstacle_check_distance + using lanelet::utils::getArcCoordinates; + const auto base_distance = getArcCoordinates(current_lanes, getEgoPose()).length; + + for (const auto & object : debug_filtered_objects_.current_lane) { + const auto & p = object.initial_pose.pose; // TODO(Horibe): consider footprint point + + // Note: it needs chattering prevention. + if (std::abs(object.initial_twist.twist.linear.x) > 0.3) { // check if stationary + continue; + } + + const auto ego_to_obj_dist = getArcCoordinates(current_lanes, p).length - base_distance; + if (0 < ego_to_obj_dist && ego_to_obj_dist < obstacle_check_distance) { + RCLCPP_DEBUG(logger_, "Stationary object is in front of ego."); + return true; // Stationary object is in front of ego. + } + } + + // Check if Ego is in terminal of current lanes + const auto & route_handler = getRouteHandler(); + const double distance_to_terminal = + route_handler->isInGoalRouteSection(current_lanes.back()) + ? utils::getSignedDistance(getEgoPose(), route_handler->getGoalPose(), current_lanes) + : utils::getDistanceToEndOfLane(getEgoPose(), current_lanes); + const auto shift_intervals = + route_handler->getLateralIntervalsToPreferredLane(current_lanes.back()); + const double lane_change_buffer = + utils::calcMinimumLaneChangeLength(getCommonParam(), shift_intervals, 0.0); + const double stop_point_buffer = getCommonParam().backward_length_buffer_for_end_of_lane; + const double terminal_judge_buffer = lane_change_buffer + stop_point_buffer + 1.0; + if (distance_to_terminal < terminal_judge_buffer) { + return true; + } + + // No stationary objects found in obstacle_check_distance and Ego is not in terminal of current + RCLCPP_DEBUG( + logger_, + "No stationary objects found in obstacle_check_distance and Ego is not in " + "terminal of current lanes"); + return false; +} + +bool NormalLaneChange::isVehicleStuck(const lanelet::ConstLanelets & current_lanes) const +{ + if (current_lanes.empty()) { + return false; // can not check + } + + const auto [min_acc, max_acc] = calcCurrentMinMaxAcceleration(); + const auto max_lane_change_length = calcMaximumLaneChangeLength(current_lanes.back(), max_acc); + const auto rss_dist = calcRssDistance( + 0.0, planner_data_->parameters.minimum_lane_changing_velocity, + lane_change_parameters_->rss_params); + + // It is difficult to define the detection range. If it is too short, the stuck will not be + // determined, even though you are stuck by an obstacle. If it is too long, + // the ego will be judged to be stuck by a distant vehicle, even though the ego is only + // stopped at a traffic light. Essentially, the calculation should be based on the information of + // the stop reason, but this is outside the scope of one module. I keep it as a TODO. + constexpr double DETECTION_DISTANCE_MARGIN = 10.0; + const auto detection_distance = max_lane_change_length + rss_dist + + getCommonParam().base_link2front + DETECTION_DISTANCE_MARGIN; + RCLCPP_DEBUG(logger_, "max_lane_change_length: %f, max_acc: %f", max_lane_change_length, max_acc); + + return isVehicleStuck(current_lanes, detection_distance); +} + +void NormalLaneChange::setStopPose(const Pose & stop_pose) +{ + lane_change_stop_pose_ = stop_pose; +} + +void NormalLaneChange::updateStopTime() +{ + const auto current_vel = getEgoVelocity(); + + if (std::abs(current_vel) > lane_change_parameters_->stop_velocity_threshold) { + stop_time_ = 0.0; + } else { + const double duration = stop_watch_.toc("stop_time"); + // clip stop time + if (stop_time_ + duration * 0.001 > lane_change_parameters_->stop_time_threshold) { + constexpr double eps = 0.1; + stop_time_ = lane_change_parameters_->stop_time_threshold + eps; + } else { + stop_time_ += duration * 0.001; + } + } + + stop_watch_.tic("stop_time"); +} + } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/src/scene_module/side_shift/manager.cpp b/planning/behavior_path_planner/src/scene_module/side_shift/manager.cpp index 5a039a98f2e52..288d9d44aee54 100644 --- a/planning/behavior_path_planner/src/scene_module/side_shift/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/side_shift/manager.cpp @@ -14,6 +14,8 @@ #include "behavior_path_planner/scene_module/side_shift/manager.hpp" +#include "tier4_autoware_utils/ros/update_param.hpp" + #include #include diff --git a/planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp b/planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp index 4e9680cee4d59..2232c6d750d55 100644 --- a/planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp @@ -32,7 +32,6 @@ using motion_utils::calcSignedArcLength; using motion_utils::findNearestIndex; using motion_utils::findNearestSegmentIndex; using tier4_autoware_utils::calcDistance2d; -using tier4_autoware_utils::calcOffsetPose; using tier4_autoware_utils::getPoint; SideShiftModule::SideShiftModule( diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp index 9d2da0d13f56b..46db10417d19e 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp @@ -14,6 +14,8 @@ #include "behavior_path_planner/scene_module/start_planner/manager.hpp" +#include "tier4_autoware_utils/ros/update_param.hpp" + #include #include @@ -36,6 +38,8 @@ StartPlannerModuleManager::StartPlannerModuleManager( p.th_stopped_time = node->declare_parameter(ns + "th_stopped_time"); p.th_turn_signal_on_lateral_offset = node->declare_parameter(ns + "th_turn_signal_on_lateral_offset"); + p.th_distance_to_middle_of_the_road = + node->declare_parameter(ns + "th_distance_to_middle_of_the_road"); p.intersection_search_length = node->declare_parameter(ns + "intersection_search_length"); p.length_ratio_for_turn_signal_deactivation_near_intersection = node->declare_parameter( ns + "length_ratio_for_turn_signal_deactivation_near_intersection"); @@ -43,6 +47,7 @@ StartPlannerModuleManager::StartPlannerModuleManager( p.collision_check_distance_from_end = node->declare_parameter(ns + "collision_check_distance_from_end"); p.th_moving_object_velocity = node->declare_parameter(ns + "th_moving_object_velocity"); + p.center_line_path_interval = node->declare_parameter(ns + "center_line_path_interval"); // shift pull out p.enable_shift_pull_out = node->declare_parameter(ns + "enable_shift_pull_out"); p.check_shift_path_lane_departure = @@ -67,6 +72,8 @@ StartPlannerModuleManager::StartPlannerModuleManager( node->declare_parameter(ns + "lane_departure_margin"); p.parallel_parking_parameters.pull_out_max_steer_angle = node->declare_parameter(ns + "pull_out_max_steer_angle"); // 15deg + p.parallel_parking_parameters.center_line_path_interval = + p.center_line_path_interval; // for geometric parallel parking // search start pose backward p.search_priority = node->declare_parameter( ns + "search_priority"); // "efficient_path" or "short_back_distance" @@ -148,6 +155,126 @@ StartPlannerModuleManager::StartPlannerModuleManager( p.rrt_star_parameters.margin = node->declare_parameter(ns + "margin"); } + // stop condition + { + p.maximum_deceleration_for_stop = + node->declare_parameter(ns + "stop_condition.maximum_deceleration_for_stop"); + p.maximum_jerk_for_stop = + node->declare_parameter(ns + "stop_condition.maximum_jerk_for_stop"); + } + + std::string base_ns = "start_planner.path_safety_check."; + + // EgoPredictedPath + std::string ego_path_ns = base_ns + "ego_predicted_path."; + { + p.ego_predicted_path_params.min_velocity = + node->declare_parameter(ego_path_ns + "min_velocity"); + p.ego_predicted_path_params.acceleration = + node->declare_parameter(ego_path_ns + "acceleration"); + p.ego_predicted_path_params.max_velocity = + node->declare_parameter(ego_path_ns + "max_velocity"); + p.ego_predicted_path_params.time_horizon_for_front_object = + node->declare_parameter(ego_path_ns + "time_horizon_for_front_object"); + p.ego_predicted_path_params.time_horizon_for_rear_object = + node->declare_parameter(ego_path_ns + "time_horizon_for_rear_object"); + p.ego_predicted_path_params.time_resolution = + node->declare_parameter(ego_path_ns + "time_resolution"); + p.ego_predicted_path_params.delay_until_departure = + node->declare_parameter(ego_path_ns + "delay_until_departure"); + } + + // ObjectFilteringParams + std::string obj_filter_ns = base_ns + "target_filtering."; + { + p.objects_filtering_params.safety_check_time_horizon = + node->declare_parameter(obj_filter_ns + "safety_check_time_horizon"); + p.objects_filtering_params.safety_check_time_resolution = + node->declare_parameter(obj_filter_ns + "safety_check_time_resolution"); + p.objects_filtering_params.object_check_forward_distance = + node->declare_parameter(obj_filter_ns + "object_check_forward_distance"); + p.objects_filtering_params.object_check_backward_distance = + node->declare_parameter(obj_filter_ns + "object_check_backward_distance"); + p.objects_filtering_params.ignore_object_velocity_threshold = + node->declare_parameter(obj_filter_ns + "ignore_object_velocity_threshold"); + p.objects_filtering_params.include_opposite_lane = + node->declare_parameter(obj_filter_ns + "include_opposite_lane"); + p.objects_filtering_params.invert_opposite_lane = + node->declare_parameter(obj_filter_ns + "invert_opposite_lane"); + p.objects_filtering_params.check_all_predicted_path = + node->declare_parameter(obj_filter_ns + "check_all_predicted_path"); + p.objects_filtering_params.use_all_predicted_path = + node->declare_parameter(obj_filter_ns + "use_all_predicted_path"); + p.objects_filtering_params.use_predicted_path_outside_lanelet = + node->declare_parameter(obj_filter_ns + "use_predicted_path_outside_lanelet"); + } + + // ObjectTypesToCheck + std::string obj_types_ns = obj_filter_ns + "object_types_to_check."; + { + p.objects_filtering_params.object_types_to_check.check_car = + node->declare_parameter(obj_types_ns + "check_car"); + p.objects_filtering_params.object_types_to_check.check_truck = + node->declare_parameter(obj_types_ns + "check_truck"); + p.objects_filtering_params.object_types_to_check.check_bus = + node->declare_parameter(obj_types_ns + "check_bus"); + p.objects_filtering_params.object_types_to_check.check_trailer = + node->declare_parameter(obj_types_ns + "check_trailer"); + p.objects_filtering_params.object_types_to_check.check_unknown = + node->declare_parameter(obj_types_ns + "check_unknown"); + p.objects_filtering_params.object_types_to_check.check_bicycle = + node->declare_parameter(obj_types_ns + "check_bicycle"); + p.objects_filtering_params.object_types_to_check.check_motorcycle = + node->declare_parameter(obj_types_ns + "check_motorcycle"); + p.objects_filtering_params.object_types_to_check.check_pedestrian = + node->declare_parameter(obj_types_ns + "check_pedestrian"); + } + + // ObjectLaneConfiguration + std::string obj_lane_ns = obj_filter_ns + "object_lane_configuration."; + { + p.objects_filtering_params.object_lane_configuration.check_current_lane = + node->declare_parameter(obj_lane_ns + "check_current_lane"); + p.objects_filtering_params.object_lane_configuration.check_right_lane = + node->declare_parameter(obj_lane_ns + "check_right_side_lane"); + p.objects_filtering_params.object_lane_configuration.check_left_lane = + node->declare_parameter(obj_lane_ns + "check_left_side_lane"); + p.objects_filtering_params.object_lane_configuration.check_shoulder_lane = + node->declare_parameter(obj_lane_ns + "check_shoulder_lane"); + p.objects_filtering_params.object_lane_configuration.check_other_lane = + node->declare_parameter(obj_lane_ns + "check_other_lane"); + } + + // SafetyCheckParams + std::string safety_check_ns = base_ns + "safety_check_params."; + { + p.safety_check_params.enable_safety_check = + node->declare_parameter(safety_check_ns + "enable_safety_check"); + p.safety_check_params.hysteresis_factor_expand_rate = + node->declare_parameter(safety_check_ns + "hysteresis_factor_expand_rate"); + p.safety_check_params.backward_path_length = + node->declare_parameter(safety_check_ns + "backward_path_length"); + p.safety_check_params.forward_path_length = + node->declare_parameter(safety_check_ns + "forward_path_length"); + p.safety_check_params.publish_debug_marker = + node->declare_parameter(safety_check_ns + "publish_debug_marker"); + } + + // RSSparams + std::string rss_ns = safety_check_ns + "rss_params."; + { + p.safety_check_params.rss_params.rear_vehicle_reaction_time = + node->declare_parameter(rss_ns + "rear_vehicle_reaction_time"); + p.safety_check_params.rss_params.rear_vehicle_safety_time_margin = + node->declare_parameter(rss_ns + "rear_vehicle_safety_time_margin"); + p.safety_check_params.rss_params.lateral_distance_max_threshold = + node->declare_parameter(rss_ns + "lateral_distance_max_threshold"); + p.safety_check_params.rss_params.longitudinal_distance_min_threshold = + node->declare_parameter(rss_ns + "longitudinal_distance_min_threshold"); + p.safety_check_params.rss_params.longitudinal_velocity_delta_time = + node->declare_parameter(rss_ns + "longitudinal_velocity_delta_time"); + } + // validation of parameters if (p.lateral_acceleration_sampling_num < 1) { RCLCPP_FATAL_STREAM( @@ -193,7 +320,7 @@ bool StartPlannerModuleManager::isSimultaneousExecutableAsApprovedModule() const const auto start_planner_ptr = std::dynamic_pointer_cast(observer.lock()); // Currently simultaneous execution with other modules is not supported while backward driving - if (!start_planner_ptr->isBackFinished()) { + if (!start_planner_ptr->isDrivingForward()) { return false; } @@ -222,7 +349,7 @@ bool StartPlannerModuleManager::isSimultaneousExecutableAsCandidateModule() cons const auto start_planner_ptr = std::dynamic_pointer_cast(observer.lock()); // Currently simultaneous execution with other modules is not supported while backward driving - if (!start_planner_ptr->isBackFinished()) { + if (start_planner_ptr->isDrivingForward()) { return false; } diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp index 2dc2401571d9a..8228542141c77 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp @@ -17,12 +17,18 @@ #include "behavior_path_planner/utils/create_vehicle_footprint.hpp" #include "behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp" #include "behavior_path_planner/utils/path_utils.hpp" +#include "behavior_path_planner/utils/start_goal_planner_common/utils.hpp" #include "behavior_path_planner/utils/start_planner/util.hpp" +#include "motion_utils/trajectory/trajectory.hpp" +#include #include #include #include -#include + +#include + +#include #include #include @@ -32,7 +38,6 @@ using motion_utils::calcLongitudinalOffsetPose; using tier4_autoware_utils::calcOffsetPose; -using tier4_autoware_utils::inverseTransformPoint; namespace behavior_path_planner { @@ -90,6 +95,7 @@ void StartPlannerModule::onFreespacePlannerTimer() BehaviorModuleOutput StartPlannerModule::run() { + updateData(); if (!isActivated()) { return planWaitingApproval(); } @@ -97,6 +103,16 @@ BehaviorModuleOutput StartPlannerModule::run() return plan(); } +void StartPlannerModule::processOnEntry() +{ + // Initialize safety checker + if (parameters_->safety_check_params.enable_safety_check) { + initializeSafetyCheckParameters(); + utils::start_goal_planner_common::initializeCollisionCheckDebugMap( + start_planner_data_.collision_check); + } +} + void StartPlannerModule::processOnExit() { resetPathCandidate(); @@ -104,12 +120,41 @@ void StartPlannerModule::processOnExit() debug_marker_.markers.clear(); } +void StartPlannerModule::updateData() +{ + if (isBackwardDrivingComplete()) { + updateStatusAfterBackwardDriving(); + } else { + status_.backward_driving_complete = false; + } + + const bool has_received_new_route = + !planner_data_->prev_route_id || + *planner_data_->prev_route_id != planner_data_->route_handler->getRouteUuid(); + + if (has_received_new_route) { + status_ = PullOutStatus(); + } + // check safety status when driving forward + if (parameters_->safety_check_params.enable_safety_check && status_.driving_forward) { + status_.is_safe_dynamic_objects = isSafePath(); + } else { + status_.is_safe_dynamic_objects = true; + } +} + bool StartPlannerModule::isExecutionRequested() const { - // TODO(Sugahara): if required lateral shift distance is small, don't engage this module. - // Execute when current pose is near route start pose - const Pose & start_pose = planner_data_->route_handler->getOriginalStartPose(); const Pose & current_pose = planner_data_->self_odometry->pose.pose; + const lanelet::ConstLanelets current_lanes = utils::getCurrentLanes(planner_data_); + const double lateral_distance_to_center_lane = + lanelet::utils::getArcCoordinates(current_lanes, current_pose).distance; + + if (std::abs(lateral_distance_to_center_lane) < parameters_->th_distance_to_middle_of_the_road) { + return false; + } + + const Pose start_pose = planner_data_->route_handler->getOriginalStartPose(); if ( tier4_autoware_utils::calcDistance2d(start_pose.position, current_pose.position) > parameters_->th_arrived_distance) { @@ -117,7 +162,7 @@ bool StartPlannerModule::isExecutionRequested() const } // Check if ego arrives at goal - const Pose & goal_pose = planner_data_->route_handler->getGoalPose(); + const Pose goal_pose = planner_data_->route_handler->getGoalPose(); if ( tier4_autoware_utils::calcDistance2d(goal_pose.position, current_pose.position) < parameters_->th_arrived_distance) { @@ -134,47 +179,40 @@ bool StartPlannerModule::isExecutionRequested() const return false; } - // Create vehicle footprint - const auto local_vehicle_footprint = createVehicleFootprint(vehicle_info_); - const auto vehicle_footprint = transformVector( - local_vehicle_footprint, - tier4_autoware_utils::pose2transform(planner_data_->self_odometry->pose.pose)); - - // Check if ego is not out of lanes - const double backward_path_length = - planner_data_->parameters.backward_path_length + parameters_->max_back_distance; - const auto current_lanes = utils::getExtendedCurrentLanes( - planner_data_, backward_path_length, std::numeric_limits::max(), - /*forward_only_in_route*/ true); - const auto pull_out_lanes = - start_planner_utils::getPullOutLanes(planner_data_, backward_path_length); - auto lanes = current_lanes; - for (const auto & pull_out_lane : pull_out_lanes) { - auto it = std::find_if( - lanes.begin(), lanes.end(), [&pull_out_lane](const lanelet::ConstLanelet & lane) { - return lane.id() == pull_out_lane.id(); - }); - - if (it == lanes.end()) { - lanes.push_back(pull_out_lane); - } - } - - if (LaneDepartureChecker::isOutOfLane(lanes, vehicle_footprint)) { - return false; - } - return true; } bool StartPlannerModule::isExecutionReady() const { + // when found_pull_out_path is false,the path is not generated and approval shouldn't be + // allowed + if (!status_.found_pull_out_path) { + RCLCPP_ERROR_THROTTLE(getLogger(), *clock_, 5000, "Pull over path is not found"); + return false; + } + + if ( + parameters_->safety_check_params.enable_safety_check && status_.driving_forward && + isWaitingApproval()) { + if (!isSafePath()) { + RCLCPP_ERROR_THROTTLE(getLogger(), *clock_, 5000, "Path is not safe against dynamic objects"); + stop_pose_ = planner_data_->self_odometry->pose.pose; + return false; + } + } return true; } -ModuleStatus StartPlannerModule::updateState() +void StartPlannerModule::updateCurrentState() { - RCLCPP_DEBUG(getLogger(), "START_PLANNER updateState"); + RCLCPP_DEBUG(getLogger(), "START_PLANNER updateCurrentState"); + + const auto print = [this](const auto & from, const auto & to) { + RCLCPP_DEBUG(getLogger(), "[start_planner] Transit from %s to %s.", from.data(), to.data()); + }; + + const auto & from = current_state_; + // current_state_ = updateState(); if (isActivated() && !isWaitingApproval()) { current_state_ = ModuleStatus::RUNNING; @@ -182,13 +220,16 @@ ModuleStatus StartPlannerModule::updateState() current_state_ = ModuleStatus::IDLE; } + // TODO(someone): move to canTransitSuccessState if (hasFinishedPullOut()) { - return ModuleStatus::SUCCESS; + current_state_ = ModuleStatus::SUCCESS; + } + // TODO(someone): move to canTransitSuccessState + if (status_.backward_driving_complete) { + current_state_ = ModuleStatus::SUCCESS; // for breaking loop } - checkBackFinished(); - - return current_state_; + print(magic_enum::enum_name(from), magic_enum::enum_name(current_state_)); } BehaviorModuleOutput StartPlannerModule::plan() @@ -198,6 +239,7 @@ BehaviorModuleOutput StartPlannerModule::plan() getLogger(), *clock_, 5000, "Start plan for a backward goal is not supported now"); const auto output = generateStopOutput(); setDebugData(); // use status updated in generateStopOutput() + updateRTCStatus(0, 0); return output; } @@ -205,26 +247,55 @@ BehaviorModuleOutput StartPlannerModule::plan() clearWaitingApproval(); resetPathCandidate(); resetPathReference(); - // save current_pose when approved for start_point of turn_signal for backward driving - last_approved_pose_ = std::make_unique(planner_data_->self_odometry->pose.pose); } BehaviorModuleOutput output; - if (!status_.is_safe_static_objects) { + if (!status_.found_pull_out_path) { RCLCPP_WARN_THROTTLE( getLogger(), *clock_, 5000, "Not found safe pull out path, publish stop path"); const auto output = generateStopOutput(); setDebugData(); // use status updated in generateStopOutput() + updateRTCStatus(0, 0); return output; } PathWithLaneId path; - if (status_.back_finished) { + + // Check if backward motion is finished + if (status_.driving_forward) { + // Increment path index if the current path is finished if (hasFinishedCurrentPath()) { RCLCPP_INFO(getLogger(), "Increment path index"); incrementPathIndex(); } - path = getCurrentPath(); + + if (!status_.is_safe_dynamic_objects && !isWaitingApproval() && !status_.has_stop_point) { + auto current_path = getCurrentPath(); + const auto stop_path = + behavior_path_planner::utils::start_goal_planner_common::generateFeasibleStopPath( + current_path, planner_data_, *stop_pose_, parameters_->maximum_deceleration_for_stop, + parameters_->maximum_jerk_for_stop); + + // Insert stop point in the path if needed + if (stop_path) { + RCLCPP_ERROR_THROTTLE( + getLogger(), *clock_, 5000, "Insert stop point in the path because of dynamic objects"); + path = *stop_path; + status_.prev_stop_path_after_approval = std::make_shared(path); + status_.has_stop_point = true; + } else { + path = current_path; + } + } else if (!isWaitingApproval() && status_.has_stop_point) { + // Delete stop point if conditions are met + if (status_.is_safe_dynamic_objects && isStopped()) { + status_.has_stop_point = false; + path = getCurrentPath(); + } + path = *status_.prev_stop_path_after_approval; + } else { + path = getCurrentPath(); + } } else { path = status_.backward_path; } @@ -246,7 +317,7 @@ BehaviorModuleOutput StartPlannerModule::plan() return SteeringFactor::STRAIGHT; }); - if (status_.back_finished) { + if (status_.driving_forward) { const double start_distance = motion_utils::calcSignedArcLength( path.points, planner_data_->self_odometry->pose.pose.position, status_.pull_out_path.start_pose.position); @@ -280,6 +351,15 @@ CandidateOutput StartPlannerModule::planCandidate() const return CandidateOutput{}; } +void StartPlannerModule::initializeSafetyCheckParameters() +{ + utils::start_goal_planner_common::updateEgoPredictedPathParams( + ego_predicted_path_params_, parameters_); + utils::start_goal_planner_common::updateSafetyCheckParams(safety_check_params_, parameters_); + utils::start_goal_planner_common::updateObjectsFilteringParams( + objects_filtering_params_, parameters_); +} + PathWithLaneId StartPlannerModule::getFullPath() const { // combine partial pull out path @@ -289,7 +369,7 @@ PathWithLaneId StartPlannerModule::getFullPath() const pull_out_path.points.end(), partial_path.points.begin(), partial_path.points.end()); } - if (status_.back_finished) { + if (status_.driving_forward) { // not need backward path or finish it return pull_out_path; } @@ -311,16 +391,18 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval() clearWaitingApproval(); const auto output = generateStopOutput(); setDebugData(); // use status updated in generateStopOutput() + updateRTCStatus(0, 0); return output; } BehaviorModuleOutput output; - if (!status_.is_safe_static_objects) { + if (!status_.found_pull_out_path) { RCLCPP_WARN_THROTTLE( getLogger(), *clock_, 5000, "Not found safe pull out path, publish stop path"); clearWaitingApproval(); const auto output = generateStopOutput(); setDebugData(); // use status updated in generateStopOutput() + updateRTCStatus(0, 0); return output; } @@ -332,7 +414,7 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval() planner_data_, backward_path_length, std::numeric_limits::max(), /*forward_only_in_route*/ true); - auto stop_path = status_.back_finished ? getCurrentPath() : status_.backward_path; + auto stop_path = status_.driving_forward ? getCurrentPath() : status_.backward_path; const auto drivable_lanes = generateDrivableLanes(stop_path); const auto & dp = planner_data_->drivable_area_expansion_parameters; const auto expanded_lanes = utils::expandLanelets( @@ -359,7 +441,7 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval() return SteeringFactor::STRAIGHT; }); - if (status_.back_finished) { + if (status_.driving_forward) { const double start_distance = motion_utils::calcSignedArcLength( stop_path.points, planner_data_->self_odometry->pose.pose.position, status_.pull_out_path.start_pose.position); @@ -407,8 +489,8 @@ PathWithLaneId StartPlannerModule::getCurrentPath() const } void StartPlannerModule::planWithPriority( - const std::vector & start_pose_candidates, const Pose & goal_pose, - const std::string search_priority) + const std::vector & start_pose_candidates, const Pose & refined_start_pose, + const Pose & goal_pose, const std::string search_priority) { // check if start pose candidates are valid if (start_pose_candidates.empty()) { @@ -419,6 +501,10 @@ void StartPlannerModule::planWithPriority( // Get the pull_out_start_pose for the current index const auto & pull_out_start_pose = start_pose_candidates.at(i); + // Set back_finished to true if the current start pose is same to refined_start_pose + status_.driving_forward = + tier4_autoware_utils::calcDistance2d(pull_out_start_pose, refined_start_pose) < 0.01; + planner->setPlannerData(planner_data_); const auto pull_out_path = planner->plan(pull_out_start_pose, goal_pose); // not found safe path @@ -426,9 +512,9 @@ void StartPlannerModule::planWithPriority( return false; } // use current path if back is not needed - if (status_.back_finished) { + if (status_.driving_forward) { const std::lock_guard lock(mutex_); - status_.is_safe_static_objects = true; + status_.found_pull_out_path = true; status_.pull_out_path = *pull_out_path; status_.pull_out_start_pose = pull_out_start_pose; status_.planner_type = planner->getPlannerType(); @@ -449,7 +535,7 @@ void StartPlannerModule::planWithPriority( // Update status variables with the next path information { const std::lock_guard lock(mutex_); - status_.is_safe_static_objects = true; + status_.found_pull_out_path = true; status_.pull_out_path = *pull_out_path_next; status_.pull_out_start_pose = pull_out_start_pose_next; status_.planner_type = planner->getPlannerType(); @@ -494,9 +580,6 @@ void StartPlannerModule::planWithPriority( for (const auto & p : order_priority) { if (is_safe_with_pose_planner(p.first, p.second)) { - const std::lock_guard lock(mutex_); - // Set back_finished flag based on the current index - status_.back_finished = p.first == 0; return; } } @@ -504,7 +587,7 @@ void StartPlannerModule::planWithPriority( // not found safe path if (status_.planner_type != PlannerType::FREESPACE) { const std::lock_guard lock(mutex_); - status_.is_safe_static_objects = false; + status_.found_pull_out_path = false; status_.planner_type = PlannerType::NONE; } } @@ -541,6 +624,9 @@ lanelet::ConstLanelets StartPlannerModule::getPathRoadLanes(const PathWithLaneId std::vector lane_ids; for (const auto & p : path.points) { for (const auto & id : p.lane_ids) { + if (id == lanelet::InvalId) { + continue; + } if (route_handler->isShoulderLanelet(lanelet_layer.get(id))) { continue; } @@ -553,9 +639,7 @@ lanelet::ConstLanelets StartPlannerModule::getPathRoadLanes(const PathWithLaneId lanelet::ConstLanelets path_lanes; path_lanes.reserve(lane_ids.size()); for (const auto & id : lane_ids) { - if (id != lanelet::InvalId) { - path_lanes.push_back(lanelet_layer.get(id)); - } + path_lanes.push_back(lanelet_layer.get(id)); } return path_lanes; @@ -565,10 +649,15 @@ std::vector StartPlannerModule::generateDrivableLanes( const PathWithLaneId & path) const { const auto path_road_lanes = getPathRoadLanes(path); - if (!path_road_lanes.empty()) { - return utils::generateDrivableLanesWithShoulderLanes( - getPathRoadLanes(path), status_.pull_out_lanes); + lanelet::ConstLanelets shoulder_lanes; + const auto & rh = planner_data_->route_handler; + std::copy_if( + status_.pull_out_lanes.begin(), status_.pull_out_lanes.end(), + std::back_inserter(shoulder_lanes), + [&rh](const auto & pull_out_lane) { return rh->isShoulderLanelet(pull_out_lane); }); + + return utils::generateDrivableLanesWithShoulderLanes(path_road_lanes, shoulder_lanes); } // if path_road_lanes is empty, use only pull_out_lanes as drivable lanes @@ -588,10 +677,6 @@ void StartPlannerModule::updatePullOutStatus() !planner_data_->prev_route_id || *planner_data_->prev_route_id != planner_data_->route_handler->getRouteUuid(); - if (has_received_new_route) { - status_ = PullOutStatus(); - } - // save pull out lanes which is generated using current pose before starting pull out // (before approval) status_.pull_out_lanes = start_planner_utils::getPullOutLanes( @@ -599,7 +684,7 @@ void StartPlannerModule::updatePullOutStatus() // skip updating if enough time has not passed for preventing chattering between back and // start_planner - if (!has_received_new_route && !last_pull_out_start_update_time_ && !status_.back_finished) { + if (!has_received_new_route) { if (!last_pull_out_start_update_time_) { last_pull_out_start_update_time_ = std::make_unique(clock_->now()); } @@ -614,61 +699,97 @@ void StartPlannerModule::updatePullOutStatus() const auto & current_pose = planner_data_->self_odometry->pose.pose; const auto & goal_pose = planner_data_->route_handler->getGoalPose(); + // refine start pose with pull out lanes. + // 1) backward driving is not allowed: use refined pose just as start pose. + // 2) backward driving is allowed: use refined pose to check if backward driving is needed. + const PathWithLaneId start_pose_candidates_path = calcStartPoseCandidatesBackwardPath(); + const auto refined_start_pose = calcLongitudinalOffsetPose( + start_pose_candidates_path.points, planner_data_->self_odometry->pose.pose.position, 0.0); + if (!refined_start_pose) return; + // search pull out start candidates backward - std::vector start_pose_candidates = searchPullOutStartPoses(); - planWithPriority(start_pose_candidates, goal_pose, parameters_->search_priority); + const std::vector start_pose_candidates = std::invoke([&]() -> std::vector { + if (parameters_->enable_back) { + return searchPullOutStartPoses(start_pose_candidates_path); + } + return {*refined_start_pose}; + }); + + planWithPriority( + start_pose_candidates, *refined_start_pose, goal_pose, parameters_->search_priority); - checkBackFinished(); - if (!status_.back_finished) { + if (isBackwardDrivingComplete()) { + updateStatusAfterBackwardDriving(); + current_state_ = ModuleStatus::SUCCESS; // for breaking loop + } else { status_.backward_path = start_planner_utils::getBackwardPath( *route_handler, status_.pull_out_lanes, current_pose, status_.pull_out_start_pose, parameters_->backward_velocity); } } -std::vector StartPlannerModule::searchPullOutStartPoses() +void StartPlannerModule::updateStatusAfterBackwardDriving() { - std::vector pull_out_start_pose{}; + status_.driving_forward = true; + status_.backward_driving_complete = true; + // request start_planner approval + waitApproval(); + // To enable approval of the forward path, the RTC status is removed. + removeRTCStatus(); + for (auto itr = uuid_map_.begin(); itr != uuid_map_.end(); ++itr) { + itr->second = generateUUID(); + } +} +PathWithLaneId StartPlannerModule::calcStartPoseCandidatesBackwardPath() const +{ const Pose & current_pose = planner_data_->self_odometry->pose.pose; // get backward shoulder path const auto arc_position_pose = lanelet::utils::getArcCoordinates(status_.pull_out_lanes, current_pose); const double check_distance = parameters_->max_back_distance + 30.0; // buffer - auto backward_shoulder_path = planner_data_->route_handler->getCenterLinePath( + auto path = planner_data_->route_handler->getCenterLinePath( status_.pull_out_lanes, arc_position_pose.length - check_distance, arc_position_pose.length + check_distance); + // lateral shift to current_pose + const double distance_from_center_line = arc_position_pose.distance; + for (auto & p : path.points) { + p.point.pose = calcOffsetPose(p.point.pose, 0, distance_from_center_line, 0); + } + + return path; +} + +std::vector StartPlannerModule::searchPullOutStartPoses( + const PathWithLaneId & start_pose_candidates) const +{ + const Pose & current_pose = planner_data_->self_odometry->pose.pose; + + std::vector pull_out_start_pose{}; + // filter pull out lanes stop objects const auto [pull_out_lane_objects, others] = utils::path_safety_checker::separateObjectsByLanelets( - *planner_data_->dynamic_object, status_.pull_out_lanes); + *planner_data_->dynamic_object, status_.pull_out_lanes, + utils::path_safety_checker::isPolygonOverlapLanelet); const auto pull_out_lane_stop_objects = utils::path_safety_checker::filterObjectsByVelocity( pull_out_lane_objects, parameters_->th_moving_object_velocity); - // lateral shift to current_pose - const double distance_from_center_line = arc_position_pose.distance; - for (auto & p : backward_shoulder_path.points) { - p.point.pose = calcOffsetPose(p.point.pose, 0, distance_from_center_line, 0); - } - - // if backward driving is disable, just refine current pose to the lanes - if (!parameters_->enable_back) { - const auto refined_pose = - calcLongitudinalOffsetPose(backward_shoulder_path.points, current_pose.position, 0); - if (refined_pose) { - pull_out_start_pose.push_back(*refined_pose); - } - return pull_out_start_pose; - } + // Set the maximum backward distance less than the distance from the vehicle's base_link to the + // lane's rearmost point to prevent lane departure. + const double s_current = + lanelet::utils::getArcCoordinates(status_.pull_out_lanes, current_pose).length; + const double max_back_distance = std::clamp( + s_current - planner_data_->parameters.base_link2rear, 0.0, parameters_->max_back_distance); // check collision between footprint and object at the backed pose const auto local_vehicle_footprint = createVehicleFootprint(vehicle_info_); - for (double back_distance = 0.0; back_distance <= parameters_->max_back_distance; + for (double back_distance = 0.0; back_distance <= max_back_distance; back_distance += parameters_->backward_search_resolution) { const auto backed_pose = calcLongitudinalOffsetPose( - backward_shoulder_path.points, current_pose.position, -back_distance); + start_pose_candidates.points, current_pose.position, -back_distance); if (!backed_pose) { continue; } @@ -676,10 +797,10 @@ std::vector StartPlannerModule::searchPullOutStartPoses() // check the back pose is near the lane end const double length_to_backed_pose = lanelet::utils::getArcCoordinates(status_.pull_out_lanes, *backed_pose).length; - double length_to_lane_end = 0.0; - for (const auto & lane : status_.pull_out_lanes) { - length_to_lane_end += lanelet::utils::getLaneletLength2d(lane); - } + + const double length_to_lane_end = std::accumulate( + std::begin(status_.pull_out_lanes), std::end(status_.pull_out_lanes), 0.0, + [](double acc, const auto & lane) { return acc + lanelet::utils::getLaneletLength2d(lane); }); const double distance_from_lane_end = length_to_lane_end - length_to_backed_pose; if (distance_from_lane_end < parameters_->ignore_distance_from_lane_end) { RCLCPP_WARN_THROTTLE( @@ -714,7 +835,7 @@ bool StartPlannerModule::isOverlappedWithLane( bool StartPlannerModule::hasFinishedPullOut() const { - if (!status_.back_finished || !status_.is_safe_static_objects) { + if (!status_.driving_forward || !status_.found_pull_out_path) { return false; } @@ -742,9 +863,9 @@ bool StartPlannerModule::hasFinishedPullOut() const return has_finished; } -void StartPlannerModule::checkBackFinished() +bool StartPlannerModule::isBackwardDrivingComplete() const { - // check ego car is close enough to pull out start pose + // check ego car is close enough to pull out start pose and stopped const auto current_pose = planner_data_->self_odometry->pose.pose; const auto distance = tier4_autoware_utils::calcDistance2d(current_pose, status_.pull_out_start_pose); @@ -753,18 +874,12 @@ void StartPlannerModule::checkBackFinished() const double ego_vel = utils::l2Norm(planner_data_->self_odometry->twist.twist.linear); const bool is_stopped = ego_vel < parameters_->th_stopped_velocity; - if (!status_.back_finished && is_near && is_stopped) { + const bool back_finished = !status_.driving_forward && is_near && is_stopped; + if (back_finished) { RCLCPP_INFO(getLogger(), "back finished"); - status_.back_finished = true; - - // request start_planner approval - waitApproval(); - removeRTCStatus(); - for (auto itr = uuid_map_.begin(); itr != uuid_map_.end(); ++itr) { - itr->second = generateUUID(); - } - current_state_ = ModuleStatus::SUCCESS; // for breaking loop } + + return back_finished; } bool StartPlannerModule::isStopped() @@ -801,7 +916,7 @@ bool StartPlannerModule::isStuck() } // not found safe path - if (!status_.is_safe_static_objects) { + if (!status_.found_pull_out_path) { return true; } @@ -828,9 +943,9 @@ TurnSignalInfo StartPlannerModule::calcTurnSignalInfo() const const Pose & end_pose = status_.pull_out_path.end_pose; // turn on hazard light when backward driving - if (!status_.back_finished) { + if (!status_.driving_forward) { turn_signal.hazard_signal.command = HazardLightsCommand::ENABLE; - const auto back_start_pose = isWaitingApproval() ? current_pose : *last_approved_pose_; + const auto back_start_pose = planner_data_->route_handler->getOriginalStartPose(); turn_signal.desired_start_point = back_start_pose; turn_signal.required_start_point = back_start_pose; // pull_out start_pose is same to backward driving end_pose @@ -922,6 +1037,88 @@ TurnSignalInfo StartPlannerModule::calcTurnSignalInfo() const return turn_signal; } +bool StartPlannerModule::isSafePath() const +{ + // TODO(Sugahara): should safety check for backward path + + const auto pull_out_path = getCurrentPath(); + const auto & current_pose = planner_data_->self_odometry->pose.pose; + const double current_velocity = std::hypot( + planner_data_->self_odometry->twist.twist.linear.x, + planner_data_->self_odometry->twist.twist.linear.y); + const auto & dynamic_object = planner_data_->dynamic_object; + const auto & route_handler = planner_data_->route_handler; + const double backward_path_length = + planner_data_->parameters.backward_path_length + parameters_->max_back_distance; + + const auto current_lanes = utils::getExtendedCurrentLanes( + planner_data_, backward_path_length, std::numeric_limits::max(), + /*forward_only_in_route*/ true); + + // for ego predicted path + const size_t ego_seg_idx = planner_data_->findEgoSegmentIndex(pull_out_path.points); + const std::pair terminal_velocity_and_accel = + utils::start_goal_planner_common::getPairsTerminalVelocityAndAccel( + status_.pull_out_path.pairs_terminal_velocity_and_accel, status_.current_path_idx); + RCLCPP_DEBUG( + getLogger(), "pairs_terminal_velocity_and_accel for start_planner: %f, %f", + terminal_velocity_and_accel.first, terminal_velocity_and_accel.second); + RCLCPP_DEBUG(getLogger(), "current_path_idx %ld", status_.current_path_idx); + utils::start_goal_planner_common::updatePathProperty( + ego_predicted_path_params_, terminal_velocity_and_accel); + // TODO(Sugahara): shoule judge is_object_front properly + const bool is_object_front = true; + const bool limit_to_max_velocity = true; + const auto ego_predicted_path = + behavior_path_planner::utils::path_safety_checker::createPredictedPath( + ego_predicted_path_params_, pull_out_path.points, current_pose, current_velocity, ego_seg_idx, + is_object_front, limit_to_max_velocity); + + // filtering objects with velocity, position and class + const auto & filtered_objects = utils::path_safety_checker::filterObjects( + dynamic_object, route_handler, current_lanes, current_pose.position, objects_filtering_params_); + + // filtering objects based on the current position's lane + const auto & target_objects_on_lane = utils::path_safety_checker::createTargetObjectsOnLane( + current_lanes, route_handler, filtered_objects, objects_filtering_params_); + + const double hysteresis_factor = + status_.is_safe_dynamic_objects ? 1.0 : safety_check_params_->hysteresis_factor_expand_rate; + + utils::start_goal_planner_common::updateSafetyCheckTargetObjectsData( + start_planner_data_, filtered_objects, target_objects_on_lane, ego_predicted_path); + + bool is_safe_dynamic_objects = true; + // Check for collisions with each predicted path of the object + for (const auto & object : target_objects_on_lane.on_current_lane) { + auto current_debug_data = marker_utils::createObjectDebug(object); + + bool is_safe_dynamic_object = true; + + const auto obj_predicted_paths = utils::path_safety_checker::getPredictedPathFromObj( + object, objects_filtering_params_->check_all_predicted_path); + + // If a collision is detected, mark the object as unsafe and break the loop + for (const auto & obj_path : obj_predicted_paths) { + if (!utils::path_safety_checker::checkCollision( + pull_out_path, ego_predicted_path, object, obj_path, planner_data_->parameters, + safety_check_params_->rss_params, hysteresis_factor, current_debug_data.second)) { + marker_utils::updateCollisionCheckDebugMap( + start_planner_data_.collision_check, current_debug_data, false); + is_safe_dynamic_objects = false; + is_safe_dynamic_object = false; + break; + } + } + if (is_safe_dynamic_object) { + marker_utils::updateCollisionCheckDebugMap( + start_planner_data_.collision_check, current_debug_data, is_safe_dynamic_object); + } + } + + return is_safe_dynamic_objects; +} + bool StartPlannerModule::IsGoalBehindOfEgoInSameRouteSegment() const { const auto & rh = planner_data_->route_handler; @@ -960,7 +1157,7 @@ BehaviorModuleOutput StartPlannerModule::generateStopOutput() { const std::lock_guard lock(mutex_); - status_.back_finished = true; + status_.driving_forward = true; status_.planner_type = PlannerType::STOP; status_.pull_out_path.partial_paths.clear(); status_.pull_out_path.partial_paths.push_back(stop_path); @@ -1012,8 +1209,8 @@ bool StartPlannerModule::planFreespacePath() status_.pull_out_path = *freespace_path; status_.pull_out_start_pose = current_pose; status_.planner_type = freespace_planner_->getPlannerType(); - status_.is_safe_static_objects = true; - status_.back_finished = true; + status_.found_pull_out_path = true; + status_.driving_forward = true; return true; } @@ -1033,15 +1230,23 @@ void StartPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) cons DrivableAreaInfo current_drivable_area_info; current_drivable_area_info.drivable_lanes = target_drivable_lanes; - output.drivable_area_info = utils::combineDrivableAreaInfo( - current_drivable_area_info, getPreviousModuleOutput().drivable_area_info); + output.drivable_area_info = + status_.driving_forward + ? utils::combineDrivableAreaInfo( + current_drivable_area_info, getPreviousModuleOutput().drivable_area_info) + : current_drivable_area_info; } } void StartPlannerModule::setDebugData() const { + using marker_utils::createObjectsMarkerArray; using marker_utils::createPathMarkerArray; using marker_utils::createPoseMarkerArray; + using marker_utils::createPredictedPathMarkerArray; + using marker_utils::showPolygon; + using marker_utils::showPredictedPath; + using marker_utils::showSafetyCheckInfo; using tier4_autoware_utils::createDefaultMarker; using tier4_autoware_utils::createMarkerColor; using tier4_autoware_utils::createMarkerScale; @@ -1059,18 +1264,40 @@ void StartPlannerModule::setDebugData() const add(createPoseMarkerArray(status_.pull_out_path.start_pose, "start_pose", 0, 0.3, 0.9, 0.3)); add(createPoseMarkerArray(status_.pull_out_path.end_pose, "end_pose", 0, 0.9, 0.9, 0.3)); add(createPathMarkerArray(getFullPath(), "full_path", 0, 0.0, 0.5, 0.9)); + add(createPathMarkerArray(status_.backward_path, "backward_driving_path", 0, 0.0, 0.9, 0.0)); + + // safety check + if (parameters_->safety_check_params.enable_safety_check) { + if (start_planner_data_.ego_predicted_path.size() > 0) { + const auto & ego_predicted_path = utils::path_safety_checker::convertToPredictedPath( + start_planner_data_.ego_predicted_path, ego_predicted_path_params_->time_resolution); + add(createPredictedPathMarkerArray( + ego_predicted_path, vehicle_info_, "ego_predicted_path_start_planner", 0, 0.0, 0.5, 0.9)); + } + + if (start_planner_data_.filtered_objects.objects.size() > 0) { + add(createObjectsMarkerArray( + start_planner_data_.filtered_objects, "filtered_objects", 0, 0.0, 0.5, 0.9)); + } + + add(showSafetyCheckInfo(start_planner_data_.collision_check, "object_debug_info")); + add(showPredictedPath(start_planner_data_.collision_check, "ego_predicted_path")); + add(showPolygon(start_planner_data_.collision_check, "ego_and_target_polygon_relation")); + utils::start_goal_planner_common::initializeCollisionCheckDebugMap( + start_planner_data_.collision_check); + } // Visualize planner type text const auto header = planner_data_->route_handler->getRouteHeader(); { visualization_msgs::msg::MarkerArray planner_type_marker_array{}; - const auto color = status_.is_safe_static_objects ? createMarkerColor(1.0, 1.0, 1.0, 0.99) - : createMarkerColor(1.0, 0.0, 0.0, 0.99); + const auto color = status_.found_pull_out_path ? createMarkerColor(1.0, 1.0, 1.0, 0.99) + : createMarkerColor(1.0, 0.0, 0.0, 0.99); auto marker = createDefaultMarker( header.frame_id, header.stamp, "planner_type", 0, visualization_msgs::msg::Marker::TEXT_VIEW_FACING, createMarkerScale(0.0, 0.0, 1.0), color); marker.pose = status_.pull_out_start_pose; - if (!status_.back_finished) { + if (!status_.driving_forward) { marker.text = "BACK -> "; } marker.text += magic_enum::enum_name(status_.planner_type); diff --git a/planning/behavior_path_planner/src/turn_signal_decider.cpp b/planning/behavior_path_planner/src/turn_signal_decider.cpp index fa661aef06681..3fe30b11e3a57 100644 --- a/planning/behavior_path_planner/src/turn_signal_decider.cpp +++ b/planning/behavior_path_planner/src/turn_signal_decider.cpp @@ -18,11 +18,16 @@ #include #include +#include #include +#include #include -#include +#include +#include +#include #include +#include #include #include diff --git a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp index 4cbfebef07a39..a9f40dad86ae4 100644 --- a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp +++ b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp @@ -16,14 +16,28 @@ #include "behavior_path_planner/utils/avoidance/avoidance_module_data.hpp" #include "behavior_path_planner/utils/avoidance/utils.hpp" +#include "behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp" #include "behavior_path_planner/utils/path_utils.hpp" +#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" #include #include -#include +#include +#include +#include #include +#include +#include +#include +#include +#include +#include +#include + +#include + #include #include #include @@ -195,6 +209,22 @@ Polygon2d createOneStepPolygon( return hull_polygon; } + +bool isEndPointsConnected( + const lanelet::ConstLanelet & left_lane, const lanelet::ConstLanelet & right_lane) +{ + const auto & left_back_point_2d = right_lane.leftBound2d().back().basicPoint(); + const auto & right_back_point_2d = left_lane.rightBound2d().back().basicPoint(); + + constexpr double epsilon = 1e-5; + return (right_back_point_2d - left_back_point_2d).norm() < epsilon; +} + +template +void pushUniqueVector(T & base_vector, const T & additional_vector) +{ + base_vector.insert(base_vector.end(), additional_vector.begin(), additional_vector.end()); +} } // namespace bool isOnRight(const ObjectData & obj) @@ -359,20 +389,7 @@ std::vector calcParentIds(const AvoidLineArray & lines1, const AvoidLine continue; } - // Id the shift is overlapped, insert the shift point. Additionally, the shift which refers - // to the same object id (created by the same object) will be set. - // - // Why? : think that there are two shifts, avoiding and . - // If you register only the avoiding shift, the return-to-center shift will not be generated - // when you get too close to or over the obstacle. The return-shift can be handled with - // addReturnShift(), but it maybe reasonable to register the return-to-center shift for the - // object at the same time as registering the avoidance shift to remove the complexity of the - // addReturnShift(). - for (const auto & al_local : lines1) { - if (al_local.object.object.object_id == al.object.object.object_id) { - ids.insert(al_local.id); - } - } + ids.insert(al.id); } return std::vector(ids.begin(), ids.end()); } @@ -396,7 +413,8 @@ void fillLongitudinalAndLengthByClosestEnvelopeFootprint( double max_distance = std::numeric_limits::lowest(); for (const auto & p : obj.envelope_poly.outer()) { const auto point = tier4_autoware_utils::createPoint(p.x(), p.y(), 0.0); - const double arc_length = calcSignedArcLengthToFirstNearestPoint(path.points, ego_pos, point); + // TODO(someone): search around first position where the ego should avoid the object. + const double arc_length = calcSignedArcLength(path.points, ego_pos, point); min_distance = std::min(min_distance, arc_length); max_distance = std::max(max_distance, arc_length); } @@ -406,13 +424,15 @@ void fillLongitudinalAndLengthByClosestEnvelopeFootprint( } double calcEnvelopeOverhangDistance( - const ObjectData & object_data, const Pose & base_pose, Point & overhang_pose) + const ObjectData & object_data, const PathWithLaneId & path, Point & overhang_pose) { double largest_overhang = isOnRight(object_data) ? -100.0 : 100.0; // large number for (const auto & p : object_data.envelope_poly.outer()) { const auto point = tier4_autoware_utils::createPoint(p.x(), p.y(), 0.0); - const auto lateral = tier4_autoware_utils::calcLateralDeviation(base_pose, point); + // TODO(someone): search around first position where the ego should avoid the object. + const auto idx = findNearestIndex(path.points, point); + const auto lateral = calcLateralDeviation(getPose(path.points.at(idx)), point); const auto & overhang_pose_on_right = [&overhang_pose, &largest_overhang, &point, &lateral]() { if (lateral > largest_overhang) { @@ -454,15 +474,14 @@ void setStartData( } Polygon2d createEnvelopePolygon( - const ObjectData & object_data, const Pose & closest_pose, const double envelope_buffer) + const Polygon2d & object_polygon, const Pose & closest_pose, const double envelope_buffer) { namespace bg = boost::geometry; + using tier4_autoware_utils::expandPolygon; using tier4_autoware_utils::Point2d; using tier4_autoware_utils::Polygon2d; using Box = bg::model::box; - const auto object_polygon = tier4_autoware_utils::toPolygon2d(object_data.object); - const auto toPolygon2d = [](const geometry_msgs::msg::Polygon & polygon) { Polygon2d ret{}; @@ -497,11 +516,17 @@ Polygon2d createEnvelopePolygon( tf2::doTransform( toMsg(envelope_poly, closest_pose.position.z), envelope_ros_polygon, geometry_tf); - const auto expanded_polygon = - tier4_autoware_utils::expandPolygon(toPolygon2d(envelope_ros_polygon), envelope_buffer); + const auto expanded_polygon = expandPolygon(toPolygon2d(envelope_ros_polygon), envelope_buffer); return expanded_polygon; } +Polygon2d createEnvelopePolygon( + const ObjectData & object_data, const Pose & closest_pose, const double envelope_buffer) +{ + const auto object_polygon = tier4_autoware_utils::toPolygon2d(object_data.object); + return createEnvelopePolygon(object_polygon, closest_pose, envelope_buffer); +} + std::vector generateObstaclePolygonsForDrivableArea( const ObjectDataArray & objects, const std::shared_ptr & parameters, const double vehicle_width) @@ -546,25 +571,6 @@ double getLongitudinalVelocity(const Pose & p_ref, const Pose & p_target, const return v * std::cos(calcYawDeviation(p_ref, p_target)); } -bool isCentroidWithinLanelets( - const PredictedObject & object, const lanelet::ConstLanelets & target_lanelets) -{ - if (target_lanelets.empty()) { - return false; - } - - const auto & object_pos = object.kinematics.initial_pose_with_covariance.pose.position; - lanelet::BasicPoint2d object_centroid(object_pos.x, object_pos.y); - - for (const auto & llt : target_lanelets) { - if (boost::geometry::within(object_centroid, llt.polygon2d().basicPolygon())) { - return true; - } - } - - return false; -} - lanelet::ConstLanelets getTargetLanelets( const std::shared_ptr & planner_data, lanelet::ConstLanelets & route_lanelets, const double left_offset, const double right_offset) @@ -656,8 +662,6 @@ void fillObjectEnvelopePolygon( ObjectData & object_data, const ObjectDataArray & registered_objects, const Pose & closest_pose, const std::shared_ptr & parameters) { - using boost::geometry::within; - const auto object_type = utils::getHighestProbLabel(object_data.object.classification); const auto object_parameter = parameters->object_parameters.at(object_type); @@ -675,15 +679,26 @@ void fillObjectEnvelopePolygon( return; } - const auto object_polygon = tier4_autoware_utils::toPolygon2d(object_data.object); + const auto envelope_poly = + createEnvelopePolygon(object_data, closest_pose, envelope_buffer_margin); + + if (boost::geometry::within(envelope_poly, same_id_obj->envelope_poly)) { + object_data.envelope_poly = same_id_obj->envelope_poly; + return; + } - if (!within(object_polygon, same_id_obj->envelope_poly)) { + std::vector unions; + boost::geometry::union_(envelope_poly, same_id_obj->envelope_poly, unions); + + if (unions.empty()) { object_data.envelope_poly = createEnvelopePolygon(object_data, closest_pose, envelope_buffer_margin); return; } - object_data.envelope_poly = same_id_obj->envelope_poly; + boost::geometry::correct(unions.front()); + + object_data.envelope_poly = createEnvelopePolygon(unions.front(), closest_pose, 0.0); } void fillObjectMovingTime( @@ -771,7 +786,7 @@ void fillAvoidanceNecessity( } // TRUE -> ? (check with hysteresis factor) - object_data.avoid_required = check_necessity(parameters->safety_check_hysteresis_factor); + object_data.avoid_required = check_necessity(parameters->hysteresis_factor_expand_rate); } void fillObjectStoppableJudge( @@ -975,7 +990,7 @@ void filterTargetObjects( data.other_objects.push_back(o); continue; } - if (o.longitudinal > parameters->object_check_forward_distance) { + if (o.longitudinal > parameters->object_check_max_forward_distance) { o.reason = AvoidanceDebugFactor::OBJECT_IS_IN_FRONT_THRESHOLD; data.other_objects.push_back(o); continue; @@ -1014,7 +1029,7 @@ void filterTargetObjects( const auto lines = rh->getFurthestLinestring(target_lanelet, get_right, get_left, get_opposite, true); const auto & line = isOnRight(o) ? lines.back() : lines.front(); - const auto d = distance2d(to2D(overhang_basic_pose), to2D(line.basicLineString())); + const auto d = boost::geometry::distance(o.envelope_poly, to2D(line.basicLineString())); if (d < o.to_road_shoulder_distance) { o.to_road_shoulder_distance = d; target_line = line; @@ -1382,6 +1397,30 @@ void fillAdditionalInfoFromPoint(const AvoidancePlanningData & data, AvoidLineAr } } +void fillAdditionalInfoFromLongitudinal(const AvoidancePlanningData & data, AvoidLine & line) +{ + const auto & path = data.reference_path; + const auto & arc = data.arclength_from_ego; + + line.start_idx = findPathIndexFromArclength(arc, line.start_longitudinal); + line.start = path.points.at(line.start_idx).point.pose; + line.end_idx = findPathIndexFromArclength(arc, line.end_longitudinal); + line.end = path.points.at(line.end_idx).point.pose; +} + +void fillAdditionalInfoFromLongitudinal( + const AvoidancePlanningData & data, AvoidOutlines & outlines) +{ + for (auto & outline : outlines) { + fillAdditionalInfoFromLongitudinal(data, outline.avoid_line); + fillAdditionalInfoFromLongitudinal(data, outline.return_line); + + std::for_each(outline.middle_lines.begin(), outline.middle_lines.end(), [&](auto & line) { + fillAdditionalInfoFromLongitudinal(data, line); + }); + } +} + void fillAdditionalInfoFromLongitudinal(const AvoidancePlanningData & data, AvoidLineArray & lines) { const auto & path = data.reference_path; @@ -1434,77 +1473,12 @@ AvoidLineArray combineRawShiftLinesWithUniqueCheck( return combined; } -std::vector convertToPredictedPath( - const PathWithLaneId & path, const std::shared_ptr & planner_data, - const std::shared_ptr & parameters) -{ - if (path.points.empty()) { - return {}; - } - - const auto & acceleration = parameters->max_acceleration; - const auto & vehicle_pose = planner_data->self_odometry->pose.pose; - const auto & initial_velocity = std::abs(planner_data->self_odometry->twist.twist.linear.x); - const auto & time_horizon = parameters->safety_check_time_horizon; - const auto & time_resolution = parameters->safety_check_time_resolution; - - const size_t ego_seg_idx = planner_data->findEgoSegmentIndex(path.points); - std::vector predicted_path; - const auto vehicle_pose_frenet = - convertToFrenetPoint(path.points, vehicle_pose.position, ego_seg_idx); - - for (double t = 0.0; t < time_horizon + 1e-3; t += time_resolution) { - const double velocity = - std::max(initial_velocity + acceleration * t, parameters->min_slow_down_speed); - const double length = initial_velocity * t + 0.5 * acceleration * t * t; - const auto pose = - motion_utils::calcInterpolatedPose(path.points, vehicle_pose_frenet.length + length); - predicted_path.emplace_back(t, pose, velocity); - } - - return predicted_path; -} - -ExtendedPredictedObject transform( - const PredictedObject & object, const std::shared_ptr & parameters) -{ - ExtendedPredictedObject extended_object; - extended_object.uuid = object.object_id; - extended_object.initial_pose = object.kinematics.initial_pose_with_covariance; - extended_object.initial_twist = object.kinematics.initial_twist_with_covariance; - extended_object.initial_acceleration = object.kinematics.initial_acceleration_with_covariance; - extended_object.shape = object.shape; - - const auto & obj_velocity_norm = std::hypot( - extended_object.initial_twist.twist.linear.x, extended_object.initial_twist.twist.linear.y); - const auto & time_horizon = parameters->safety_check_time_horizon; - const auto & time_resolution = parameters->safety_check_time_resolution; - - extended_object.predicted_paths.resize(object.kinematics.predicted_paths.size()); - for (size_t i = 0; i < object.kinematics.predicted_paths.size(); ++i) { - const auto & path = object.kinematics.predicted_paths.at(i); - extended_object.predicted_paths.at(i).confidence = path.confidence; - - // create path - for (double t = 0.0; t < time_horizon + 1e-3; t += time_resolution) { - const auto obj_pose = object_recognition_utils::calcInterpolatedPose(path, t); - if (obj_pose) { - const auto obj_polygon = tier4_autoware_utils::toPolygon2d(*obj_pose, object.shape); - extended_object.predicted_paths.at(i).path.emplace_back( - t, *obj_pose, obj_velocity_norm, obj_polygon); - } - } - } - - return extended_object; -} - lanelet::ConstLanelets getAdjacentLane( const std::shared_ptr & planner_data, const std::shared_ptr & parameters, const bool is_right_shift) { const auto & rh = planner_data->route_handler; - const auto & forward_distance = parameters->object_check_forward_distance; + const auto & forward_distance = parameters->object_check_max_forward_distance; const auto & backward_distance = parameters->safety_check_backward_distance; const auto & vehicle_pose = planner_data->self_odometry->pose.pose; @@ -1552,14 +1526,25 @@ std::vector getSafetyCheckTargetObjects( std::vector target_objects; - const auto append_target_objects = [&](const auto & check_lanes, const auto & objects) { - std::for_each(objects.begin(), objects.end(), [&](const auto & object) { - if (isCentroidWithinLanelets(object.object, check_lanes)) { - target_objects.push_back(utils::avoidance::transform(object.object, p)); - } + const auto time_horizon = std::max( + parameters->ego_predicted_path_params.time_horizon_for_front_object, + parameters->ego_predicted_path_params.time_horizon_for_rear_object); + + const auto append = [&](const auto & objects) { + std::for_each(objects.objects.begin(), objects.objects.end(), [&](const auto & object) { + target_objects.push_back(utils::path_safety_checker::transform( + object, time_horizon, parameters->ego_predicted_path_params.time_resolution)); }); }; + const auto to_predicted_objects = [&p](const auto & objects) { + PredictedObjects ret{}; + std::for_each(objects.begin(), objects.end(), [&p, &ret](const auto & object) { + ret.objects.push_back(object.object); + }); + return ret; + }; + const auto unavoidable_objects = [&data]() { ObjectDataArray ret; std::for_each(data.target_objects.begin(), data.target_objects.end(), [&](const auto & object) { @@ -1575,11 +1560,17 @@ std::vector getSafetyCheckTargetObjects( const auto check_lanes = getAdjacentLane(planner_data, p, true); if (p->check_other_object) { - append_target_objects(check_lanes, data.other_objects); + const auto [targets, others] = utils::path_safety_checker::separateObjectsByLanelets( + to_predicted_objects(data.other_objects), check_lanes, + utils::path_safety_checker::isCentroidWithinLanelet); + append(targets); } if (p->check_unavoidable_object) { - append_target_objects(check_lanes, unavoidable_objects); + const auto [targets, others] = utils::path_safety_checker::separateObjectsByLanelets( + to_predicted_objects(unavoidable_objects), check_lanes, + utils::path_safety_checker::isCentroidWithinLanelet); + append(targets); } } @@ -1588,11 +1579,17 @@ std::vector getSafetyCheckTargetObjects( const auto check_lanes = getAdjacentLane(planner_data, p, false); if (p->check_other_object) { - append_target_objects(check_lanes, data.other_objects); + const auto [targets, others] = utils::path_safety_checker::separateObjectsByLanelets( + to_predicted_objects(data.other_objects), check_lanes, + utils::path_safety_checker::isCentroidWithinLanelet); + append(targets); } if (p->check_unavoidable_object) { - append_target_objects(check_lanes, unavoidable_objects); + const auto [targets, others] = utils::path_safety_checker::separateObjectsByLanelets( + to_predicted_objects(unavoidable_objects), check_lanes, + utils::path_safety_checker::isCentroidWithinLanelet); + append(targets); } } @@ -1601,11 +1598,17 @@ std::vector getSafetyCheckTargetObjects( const auto check_lanes = data.current_lanelets; if (p->check_other_object) { - append_target_objects(check_lanes, data.other_objects); + const auto [targets, others] = utils::path_safety_checker::separateObjectsByLanelets( + to_predicted_objects(data.other_objects), check_lanes, + utils::path_safety_checker::isCentroidWithinLanelet); + append(targets); } if (p->check_unavoidable_object) { - append_target_objects(check_lanes, unavoidable_objects); + const auto [targets, others] = utils::path_safety_checker::separateObjectsByLanelets( + to_predicted_objects(unavoidable_objects), check_lanes, + utils::path_safety_checker::isCentroidWithinLanelet); + append(targets); } } @@ -1615,7 +1618,7 @@ std::vector getSafetyCheckTargetObjects( std::pair separateObjectsByPath( const PathWithLaneId & path, const std::shared_ptr & planner_data, const AvoidancePlanningData & data, const std::shared_ptr & parameters, - DebugData & debug) + const double object_check_forward_distance, const bool is_running, DebugData & debug) { PredictedObjects target_objects; PredictedObjects other_objects; @@ -1638,7 +1641,7 @@ std::pair separateObjectsByPath( const auto & p_ego_back = path.points.at(i + 1).point.pose; const auto distance_from_ego = calcSignedArcLength(path.points, ego_idx, i); - if (distance_from_ego > parameters->object_check_forward_distance) { + if (distance_from_ego > object_check_forward_distance) { break; } @@ -1652,6 +1655,25 @@ std::pair separateObjectsByPath( } } + // expand detection area width only when the module is running. + if (is_running) { + constexpr int PER_CIRCLE = 36; + constexpr double MARGIN = 1.0; // [m] + boost::geometry::strategy::buffer::distance_symmetric distance_strategy(MARGIN); + boost::geometry::strategy::buffer::join_round join_strategy(PER_CIRCLE); + boost::geometry::strategy::buffer::end_round end_strategy(PER_CIRCLE); + boost::geometry::strategy::buffer::point_circle circle_strategy(PER_CIRCLE); + boost::geometry::strategy::buffer::side_straight side_strategy; + boost::geometry::model::multi_polygon result; + // Create the buffer of a multi polygon + boost::geometry::buffer( + attention_area, result, distance_strategy, side_strategy, join_strategy, end_strategy, + circle_strategy); + if (!result.empty()) { + attention_area = result.front(); + } + } + debug.detection_area = toMsg(attention_area, data.reference_pose.position.z); const auto objects = planner_data->dynamic_object->objects; @@ -1666,4 +1688,143 @@ std::pair separateObjectsByPath( return std::make_pair(target_objects, other_objects); } + +DrivableLanes generateExpandDrivableLanes( + const lanelet::ConstLanelet & lanelet, const std::shared_ptr & planner_data, + const std::shared_ptr & parameters) +{ + const auto & route_handler = planner_data->route_handler; + + DrivableLanes current_drivable_lanes; + current_drivable_lanes.left_lane = lanelet; + current_drivable_lanes.right_lane = lanelet; + + if (!parameters->use_adjacent_lane) { + return current_drivable_lanes; + } + + // 1. get left/right side lanes + const auto update_left_lanelets = [&](const lanelet::ConstLanelet & target_lane) { + const auto all_left_lanelets = route_handler->getAllLeftSharedLinestringLanelets( + target_lane, parameters->use_opposite_lane, true); + if (!all_left_lanelets.empty()) { + current_drivable_lanes.left_lane = all_left_lanelets.back(); // leftmost lanelet + pushUniqueVector( + current_drivable_lanes.middle_lanes, + lanelet::ConstLanelets(all_left_lanelets.begin(), all_left_lanelets.end() - 1)); + } + }; + const auto update_right_lanelets = [&](const lanelet::ConstLanelet & target_lane) { + const auto all_right_lanelets = route_handler->getAllRightSharedLinestringLanelets( + target_lane, parameters->use_opposite_lane, true); + if (!all_right_lanelets.empty()) { + current_drivable_lanes.right_lane = all_right_lanelets.back(); // rightmost lanelet + pushUniqueVector( + current_drivable_lanes.middle_lanes, + lanelet::ConstLanelets(all_right_lanelets.begin(), all_right_lanelets.end() - 1)); + } + }; + + update_left_lanelets(lanelet); + update_right_lanelets(lanelet); + + // 2.1 when there are multiple lanes whose previous lanelet is the same + const auto get_next_lanes_from_same_previous_lane = + [&route_handler](const lanelet::ConstLanelet & lane) { + // get previous lane, and return false if previous lane does not exist + lanelet::ConstLanelets prev_lanes; + if (!route_handler->getPreviousLaneletsWithinRoute(lane, &prev_lanes)) { + return lanelet::ConstLanelets{}; + } + + lanelet::ConstLanelets next_lanes; + for (const auto & prev_lane : prev_lanes) { + const auto next_lanes_from_prev = route_handler->getNextLanelets(prev_lane); + pushUniqueVector(next_lanes, next_lanes_from_prev); + } + return next_lanes; + }; + + const auto next_lanes_for_right = + get_next_lanes_from_same_previous_lane(current_drivable_lanes.right_lane); + const auto next_lanes_for_left = + get_next_lanes_from_same_previous_lane(current_drivable_lanes.left_lane); + + // 2.2 look for neighbor lane recursively, where end line of the lane is connected to end line + // of the original lane + const auto update_drivable_lanes = + [&](const lanelet::ConstLanelets & next_lanes, const bool is_left) { + for (const auto & next_lane : next_lanes) { + const auto & edge_lane = + is_left ? current_drivable_lanes.left_lane : current_drivable_lanes.right_lane; + if (next_lane.id() == edge_lane.id()) { + continue; + } + + const auto & left_lane = is_left ? next_lane : edge_lane; + const auto & right_lane = is_left ? edge_lane : next_lane; + if (!isEndPointsConnected(left_lane, right_lane)) { + continue; + } + + if (is_left) { + current_drivable_lanes.left_lane = next_lane; + } else { + current_drivable_lanes.right_lane = next_lane; + } + + const auto & middle_lanes = current_drivable_lanes.middle_lanes; + const auto has_same_lane = std::any_of( + middle_lanes.begin(), middle_lanes.end(), + [&edge_lane](const auto & lane) { return lane.id() == edge_lane.id(); }); + + if (!has_same_lane) { + if (is_left) { + if (current_drivable_lanes.right_lane.id() != edge_lane.id()) { + current_drivable_lanes.middle_lanes.push_back(edge_lane); + } + } else { + if (current_drivable_lanes.left_lane.id() != edge_lane.id()) { + current_drivable_lanes.middle_lanes.push_back(edge_lane); + } + } + } + + return true; + } + return false; + }; + + const auto expand_drivable_area_recursively = + [&](const lanelet::ConstLanelets & next_lanes, const bool is_left) { + // NOTE: set max search num to avoid infinity loop for drivable area expansion + constexpr size_t max_recursive_search_num = 3; + for (size_t i = 0; i < max_recursive_search_num; ++i) { + const bool is_update_kept = update_drivable_lanes(next_lanes, is_left); + if (!is_update_kept) { + break; + } + if (i == max_recursive_search_num - 1) { + RCLCPP_ERROR( + rclcpp::get_logger("behavior_path_planner").get_child("avoidance"), + "Drivable area expansion reaches max iteration."); + } + } + }; + expand_drivable_area_recursively(next_lanes_for_right, false); + expand_drivable_area_recursively(next_lanes_for_left, true); + + // 3. update again for new left/right lanes + update_left_lanelets(current_drivable_lanes.left_lane); + update_right_lanelets(current_drivable_lanes.right_lane); + + // 4. compensate that current_lane is in either of left_lane, right_lane or middle_lanes. + if ( + current_drivable_lanes.left_lane.id() != lanelet.id() && + current_drivable_lanes.right_lane.id() != lanelet.id()) { + current_drivable_lanes.middle_lanes.push_back(lanelet); + } + + return current_drivable_lanes; +} } // namespace behavior_path_planner::utils::avoidance diff --git a/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp b/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp index 2b62d5e7c3b53..3008f98331c92 100644 --- a/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp +++ b/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp @@ -14,264 +14,322 @@ #include "behavior_path_planner/utils/drivable_area_expansion/drivable_area_expansion.hpp" -#include "behavior_path_planner/utils/drivable_area_expansion/expansion.hpp" #include "behavior_path_planner/utils/drivable_area_expansion/footprints.hpp" #include "behavior_path_planner/utils/drivable_area_expansion/map_utils.hpp" #include "behavior_path_planner/utils/drivable_area_expansion/parameters.hpp" +#include "behavior_path_planner/utils/drivable_area_expansion/path_projection.hpp" #include "behavior_path_planner/utils/drivable_area_expansion/types.hpp" -#include "interpolation/linear_interpolation.hpp" #include +#include +#include +#include +#include +#include #include +#include + namespace drivable_area_expansion { -void expandDrivableArea( - PathWithLaneId & path, const DrivableAreaExpansionParameters & params, - const PredictedObjects & dynamic_objects, const route_handler::RouteHandler & route_handler, - const lanelet::ConstLanelets & path_lanes) +namespace { - const auto uncrossable_lines = - extractUncrossableLines(*route_handler.getLaneletMapPtr(), params.avoid_linestring_types); - multi_linestring_t uncrossable_lines_in_range; - const auto & p = path.points.front().point.pose.position; - for (const auto & line : uncrossable_lines) - if (boost::geometry::distance(line, point_t{p.x, p.y}) < params.max_path_arc_length) - uncrossable_lines_in_range.push_back(line); - const auto path_footprints = createPathFootprints(path, params); - const auto predicted_paths = createObjectFootprints(dynamic_objects, params); - const auto expansion_polygons = - params.expansion_method == "lanelet" - ? createExpansionLaneletPolygons( - path_lanes, route_handler, path_footprints, predicted_paths, params) - : createExpansionPolygons( - path, path_footprints, predicted_paths, uncrossable_lines_in_range, params); - const auto expanded_drivable_area = createExpandedDrivableAreaPolygon(path, expansion_polygons); - updateDrivableAreaBounds(path, expanded_drivable_area); -} - -point_t convert_point(const Point & p) +Point2d convert_point(const Point & p) { - return point_t{p.x, p.y}; + return Point2d{p.x, p.y}; } +} // namespace -Point convert_point(const point_t & p) +void reuse_previous_poses( + const PathWithLaneId & path, std::vector & prev_poses, + std::vector & prev_curvatures, const Point & ego_point, + const DrivableAreaExpansionParameters & params) { - return Point().set__x(p.x()).set__y(p.y()); + std::vector cropped_poses; + std::vector cropped_curvatures; + const auto ego_is_behind = prev_poses.size() > 1 && motion_utils::calcLongitudinalOffsetToSegment( + prev_poses, 0, ego_point) < 0.0; + const auto ego_is_far = !prev_poses.empty() && + tier4_autoware_utils::calcDistance2d(ego_point, prev_poses.front()) < 0.0; + if (!ego_is_behind && !ego_is_far && prev_poses.size() > 1) { + const auto first_idx = + motion_utils::findNearestSegmentIndex(prev_poses, path.points.front().point.pose); + const auto deviation = + motion_utils::calcLateralOffset(prev_poses, path.points.front().point.pose.position); + if (first_idx && deviation < params.max_reuse_deviation) { + LineString2d path_ls; + for (const auto & p : path.points) path_ls.push_back(convert_point(p.point.pose.position)); + for (auto idx = *first_idx; idx < prev_poses.size(); ++idx) { + double lateral_offset = std::numeric_limits::max(); + for (auto segment_idx = 0LU; segment_idx + 1 < path_ls.size(); ++segment_idx) { + const auto projection = point_to_line_projection( + convert_point(prev_poses[idx].position), path_ls[segment_idx], + path_ls[segment_idx + 1]); + lateral_offset = std::min(projection.distance, lateral_offset); + } + if (lateral_offset > params.max_reuse_deviation) break; + cropped_poses.push_back(prev_poses[idx]); + cropped_curvatures.push_back(prev_curvatures[idx]); + } + } + } + if (cropped_poses.empty()) { + const auto resampled_path_points = + motion_utils::resamplePath(path, params.resample_interval, true, true, false).points; + const auto cropped_path = + params.max_path_arc_length <= 0.0 + ? resampled_path_points + : motion_utils::cropForwardPoints( + resampled_path_points, resampled_path_points.front().point.pose.position, 0, + params.max_path_arc_length); + for (const auto & p : cropped_path) cropped_poses.push_back(p.point.pose); + } else { + const auto initial_arc_length = motion_utils::calcArcLength(cropped_poses); + const auto max_path_arc_length = motion_utils::calcArcLength(path.points); + const auto first_arc_length = motion_utils::calcSignedArcLength( + path.points, path.points.front().point.pose.position, cropped_poses.back().position); + for (auto arc_length = first_arc_length + params.resample_interval; + (params.max_path_arc_length <= 0.0 || + initial_arc_length + (arc_length - first_arc_length) <= params.max_path_arc_length) && + arc_length <= max_path_arc_length; + arc_length += params.resample_interval) + cropped_poses.push_back(motion_utils::calcInterpolatedPose(path.points, arc_length)); + } + prev_poses = motion_utils::removeOverlapPoints(cropped_poses); + prev_curvatures = cropped_curvatures; } -polygon_t createExpandedDrivableAreaPolygon( - const PathWithLaneId & path, const multi_polygon_t & expansion_polygons) +double calculate_minimum_lane_width( + const double curvature_radius, const DrivableAreaExpansionParameters & params) { - polygon_t original_da_poly; - original_da_poly.outer().reserve(path.left_bound.size() + path.right_bound.size() + 1); - for (const auto & p : path.left_bound) original_da_poly.outer().push_back(convert_point(p)); - for (auto it = path.right_bound.rbegin(); it != path.right_bound.rend(); ++it) - original_da_poly.outer().push_back(convert_point(*it)); - original_da_poly.outer().push_back(original_da_poly.outer().front()); - - multi_polygon_t unions; - auto expanded_da_poly = original_da_poly; - for (const auto & p : expansion_polygons) { - unions.clear(); - boost::geometry::union_(expanded_da_poly, p, unions); - if (unions.size() == 1) // union of overlapping polygons should produce a single polygon - expanded_da_poly = unions[0]; - } - return expanded_da_poly; + const auto k = curvature_radius; + const auto a = params.vehicle_info.front_overhang_m + params.extra_front_overhang; + const auto w = params.vehicle_info.vehicle_width_m + params.extra_width; + const auto l = params.vehicle_info.wheel_base_m + params.extra_wheelbase; + return (a * a + 2.0 * a * l + 2.0 * k * w + l * l + w * w) / (2.0 * k + w); } -void copy_z_over_arc_length( - const std::vector & from, std::vector & to) +std::vector calculate_minimum_expansions( + const std::vector & path_poses, const std::vector bound, + const std::vector curvatures, const Side side, + const DrivableAreaExpansionParameters & params) { - if (from.empty() || to.empty()) return; - to.front().z = from.front().z; - if (from.size() < 2 || to.size() < 2) return; - to.back().z = from.back().z; - auto i_from = 1lu; - auto s_from = tier4_autoware_utils::calcDistance2d(from[0], from[1]); - auto s_to = 0.0; - auto s_from_prev = 0.0; - for (auto i_to = 1lu; i_to + 1 < to.size(); ++i_to) { - s_to += tier4_autoware_utils::calcDistance2d(to[i_to - 1], to[i_to]); - for (; s_from < s_to && i_from + 1 < from.size(); ++i_from) { - s_from_prev = s_from; - s_from += tier4_autoware_utils::calcDistance2d(from[i_from], from[i_from + 1]); - } - if (s_from - s_from_prev != 0.0) { - const auto ratio = (s_to - s_from_prev) / (s_from - s_from_prev); - to[i_to].z = interpolation::lerp(from[i_from - 1].z, from[i_from].z, ratio); - } else { - to[i_to].z = to[i_to - 1].z; + std::vector minimum_expansions(bound.size()); + size_t lb_idx = 0; + for (auto path_idx = 0UL; path_idx < path_poses.size(); ++path_idx) { + const auto & path_pose = path_poses[path_idx]; + if (curvatures[path_idx] == 0.0) continue; + const auto curvature_radius = 1 / curvatures[path_idx]; + const auto min_lane_width = calculate_minimum_lane_width(curvature_radius, params); + const auto side_distance = min_lane_width / 2.0 * (side == LEFT ? 1.0 : -1.0); + const auto offset_point = + tier4_autoware_utils::calcOffsetPose(path_pose, 0.0, side_distance, 0.0).position; + for (auto bound_idx = lb_idx; bound_idx + 1 < bound.size(); ++bound_idx) { + const auto & prev_p = bound[bound_idx]; + const auto & next_p = bound[bound_idx + 1]; + const auto intersection_point = + tier4_autoware_utils::intersect(offset_point, path_pose.position, prev_p, next_p); + if (intersection_point) { + lb_idx = bound_idx; + const auto dist = tier4_autoware_utils::calcDistance2d(*intersection_point, offset_point); + minimum_expansions[bound_idx] = std::max(minimum_expansions[bound_idx], dist); + minimum_expansions[bound_idx + 1] = std::max(minimum_expansions[bound_idx + 1], dist); + // apply the expansion to all bound points within the extra arc length + auto arc_length = + tier4_autoware_utils::calcDistance2d(*intersection_point, bound[bound_idx + 1]); + for (auto up_bound_idx = bound_idx + 2; up_bound_idx < bound.size(); ++up_bound_idx) { + arc_length += + tier4_autoware_utils::calcDistance2d(bound[up_bound_idx - 1], bound[up_bound_idx]); + if (arc_length > params.extra_arc_length) break; + minimum_expansions[up_bound_idx] = std::max(minimum_expansions[up_bound_idx], dist); + } + arc_length = tier4_autoware_utils::calcDistance2d(*intersection_point, bound[bound_idx]); + for (auto down_offset_idx = 1LU; down_offset_idx < bound_idx; ++down_offset_idx) { + const auto idx = bound_idx - down_offset_idx; + arc_length += tier4_autoware_utils::calcDistance2d(bound[idx - 1], bound[idx]); + if (arc_length > params.extra_arc_length) break; + minimum_expansions[idx] = std::max(minimum_expansions[idx], dist); + } + break; + } } } + return minimum_expansions; } -void updateDrivableAreaBounds(PathWithLaneId & path, const polygon_t & expanded_drivable_area) +void apply_bound_change_rate_limit( + std::vector & distances, const std::vector & bound, const double max_rate) { - const auto original_left_bound = path.left_bound; - const auto original_right_bound = path.right_bound; - const auto is_left_of_segment = [](const point_t & a, const point_t & b, const point_t & p) { - return (b.x() - a.x()) * (p.y() - a.y()) - (b.y() - a.y()) * (p.x() - a.x()) > 0; - }; - // prepare delimiting lines: start and end of the original expanded drivable area - const auto start_segment = - segment_t{convert_point(path.left_bound.front()), convert_point(path.right_bound.front())}; - const auto end_segment = - segment_t{convert_point(path.left_bound.back()), convert_point(path.right_bound.back())}; - point_t start_segment_center; - boost::geometry::centroid(start_segment, start_segment_center); - const auto path_start_segment = - segment_t{start_segment_center, convert_point(path.points[1].point.pose.position)}; - point_t end_segment_center; - boost::geometry::centroid(end_segment, end_segment_center); - const auto path_end_segment = - segment_t{convert_point(path.points.back().point.pose.position), end_segment_center}; - const auto segment_to_line_intersection = - [](const auto p1, const auto p2, const auto q1, const auto q2) -> std::optional { - const auto line = Eigen::Hyperplane::Through(q1, q2); - const auto segment = Eigen::Hyperplane::Through(p1, p2); - const auto intersection = line.intersection(segment); - std::optional result; - const auto is_on_segment = - (p1.x() <= p2.x() ? intersection.x() >= p1.x() && intersection.x() <= p2.x() - : intersection.x() <= p1.x() && intersection.x() >= p2.x()) && - (p1.y() <= p2.y() ? intersection.y() >= p1.y() && intersection.y() <= p2.y() - : intersection.y() <= p1.y() && intersection.y() >= p2.y()); - if (is_on_segment) result = point_t{intersection.x(), intersection.y()}; - return result; - }; - // find intersection between the expanded drivable area and the delimiting lines - const auto & da = expanded_drivable_area.outer(); - struct Intersection - { - point_t intersection_point; - ring_t::const_iterator segment_it; - double distance = std::numeric_limits::max(); - explicit Intersection(ring_t::const_iterator it) : segment_it(it) {} - void update(const point_t & p, const ring_t::const_iterator & it, const double dist) - { - intersection_point = p; - segment_it = it; - distance = dist; + if (distances.empty()) return; + const auto apply_max_vel = [&](auto & exp, const auto from, const auto to) { + if (exp[from] > exp[to]) { + const auto arc_length = tier4_autoware_utils::calcDistance2d(bound[from], bound[to]); + const auto smoothed_dist = exp[from] - arc_length * max_rate; + exp[to] = std::max(exp[to], smoothed_dist); } }; - Intersection start_left(da.end()); - Intersection end_left(da.end()); - Intersection start_right(da.end()); - Intersection end_right(da.end()); - for (auto it = da.begin(); it != da.end(); ++it) { - if (boost::geometry::distance(*it, start_segment.first) < 1e-3) - start_left.update(*it, it, 0.0); - else if (boost::geometry::distance(*it, start_segment.second) < 1e-3) - start_right.update(*it, it, 0.0); - else if (boost::geometry::distance(*it, end_segment.first) < 1e-3) - end_left.update(*it, it, 0.0); - else if (boost::geometry::distance(*it, end_segment.second) < 1e-3) - end_right.update(*it, it, 0.0); - const auto inter_start = - std::next(it) == da.end() - ? segment_to_line_intersection(*it, da.front(), start_segment.first, start_segment.second) - : segment_to_line_intersection( - *it, *std::next(it), start_segment.first, start_segment.second); - if (inter_start) { - const auto dist = boost::geometry::distance(*inter_start, path_start_segment); - const auto is_left_of_path_start = is_left_of_segment( - convert_point(path.points[0].point.pose.position), - convert_point(path.points[1].point.pose.position), *inter_start); - if (is_left_of_path_start && dist < start_left.distance) - start_left.update(*inter_start, it, dist); - else if (!is_left_of_path_start && dist < start_right.distance) - start_right.update(*inter_start, it, dist); + for (auto idx = 0LU; idx + 1 < distances.size(); ++idx) apply_max_vel(distances, idx, idx + 1); + for (auto idx = distances.size() - 1; idx > 0; --idx) apply_max_vel(distances, idx, idx - 1); +} + +std::vector calculate_maximum_distance( + const std::vector & path_poses, const std::vector bound, + const SegmentRtree & uncrossable_segments, const std::vector & uncrossable_polygons, + const DrivableAreaExpansionParameters & params) +{ + // TODO(Maxime): improve performances (dont use bg::distance ? use rtree ?) + std::vector maximum_distances(bound.size(), std::numeric_limits::max()); + LineString2d path_ls; + LineString2d bound_ls; + for (const auto & p : bound) bound_ls.push_back(convert_point(p)); + for (const auto & p : path_poses) path_ls.push_back(convert_point(p.position)); + for (auto i = 0UL; i + 1 < bound_ls.size(); ++i) { + const Segment2d segment_ls = {bound_ls[i], bound_ls[i + 1]}; + std::vector query_result; + boost::geometry::index::query( + uncrossable_segments, boost::geometry::index::nearest(segment_ls, 1), + std::back_inserter(query_result)); + if (!query_result.empty()) { + const auto bound_to_line_dist = boost::geometry::distance(segment_ls, query_result.front()); + const auto dist_limit = std::max(0.0, bound_to_line_dist - params.avoid_linestring_dist); + maximum_distances[i] = std::min(maximum_distances[i], dist_limit); + maximum_distances[i + 1] = std::min(maximum_distances[i + 1], dist_limit); } - const auto inter_end = - std::next(it) == da.end() - ? segment_to_line_intersection(*it, da.front(), end_segment.first, end_segment.second) - : segment_to_line_intersection(*it, *std::next(it), end_segment.first, end_segment.second); - if (inter_end) { - const auto dist = boost::geometry::distance(*inter_end, path_end_segment); - const auto is_left_of_path_end = is_left_of_segment( - convert_point(path.points.back().point.pose.position), end_segment_center, *inter_end); - if (is_left_of_path_end && dist < end_left.distance) - end_left.update(*inter_end, it, dist); - else if (!is_left_of_path_end && dist < end_right.distance) - end_right.update(*inter_end, it, dist); + for (const auto & uncrossable_poly : uncrossable_polygons) { + const auto bound_to_poly_dist = boost::geometry::distance(segment_ls, uncrossable_poly); + maximum_distances[i] = std::min(maximum_distances[i], bound_to_poly_dist); + maximum_distances[i + 1] = std::min(maximum_distances[i + 1], bound_to_poly_dist); } } - if (start_left.segment_it == da.end()) { - const auto closest_it = - std::min_element(da.begin(), da.end(), [&](const auto & a, const auto & b) { - return boost::geometry::distance(a, start_segment.first) < - boost::geometry::distance(b, start_segment.first); - }); - start_left.update(*closest_it, closest_it, 0.0); - } - if (start_right.segment_it == da.end()) { - const auto closest_it = - std::min_element(da.begin(), da.end(), [&](const auto & a, const auto & b) { - return boost::geometry::distance(a, start_segment.second) < - boost::geometry::distance(b, start_segment.second); - }); - start_right.update(*closest_it, closest_it, 0.0); - } - if (end_left.segment_it == da.end()) { - const auto closest_it = - std::min_element(da.begin(), da.end(), [&](const auto & a, const auto & b) { - return boost::geometry::distance(a, end_segment.first) < - boost::geometry::distance(b, end_segment.first); - }); - end_left.update(*closest_it, closest_it, 0.0); - } - if (end_right.segment_it == da.end()) { - const auto closest_it = - std::min_element(da.begin(), da.end(), [&](const auto & a, const auto & b) { - return boost::geometry::distance(a, end_segment.second) < - boost::geometry::distance(b, end_segment.second); - }); - end_right.update(*closest_it, closest_it, 0.0); + if (params.max_expansion_distance > 0.0) + for (auto & d : maximum_distances) d = std::min(params.max_expansion_distance, d); + return maximum_distances; +} + +void expand_bound( + std::vector & bound, const std::vector & path_poses, + const std::vector & expansions) +{ + LineString2d path_ls; + for (const auto & p : path_poses) path_ls.push_back(convert_point(p.position)); + for (auto idx = 0LU; idx < bound.size(); ++idx) { + if (expansions[idx] > 0.0) { + const auto bound_p = convert_point(bound[idx]); + const auto projection = point_to_linestring_projection(bound_p, path_ls); + const auto expansion_ratio = + (expansions[idx] + std::abs(projection.distance)) / std::abs(projection.distance); + const auto & path_p = projection.projected_point; + const auto expanded_p = lerp_point(path_p, bound_p, expansion_ratio); + bound[idx].x = expanded_p.x(); + bound[idx].y = expanded_p.y(); + } } - // extract the expanded left and right bound from the expanded drivable area - path.left_bound.clear(); - path.right_bound.clear(); - path.left_bound.push_back(convert_point(start_left.intersection_point)); - path.right_bound.push_back(convert_point(start_right.intersection_point)); - if (!boost::geometry::equals(start_right.intersection_point, *start_right.segment_it)) - path.right_bound.push_back(convert_point(*start_right.segment_it)); - if (start_left.segment_it < end_left.segment_it) { - for (auto it = std::next(start_left.segment_it); it <= end_left.segment_it; ++it) - path.left_bound.push_back(convert_point(*it)); - } else { - for (auto it = std::next(start_left.segment_it); it < da.end(); ++it) - path.left_bound.push_back(convert_point(*it)); - for (auto it = da.begin(); it <= end_left.segment_it; ++it) - path.left_bound.push_back(convert_point(*it)); + // remove any self intersection by skipping the points inside of the loop + std::vector no_loop_bound = {bound.front()}; + for (auto idx = 1LU; idx < bound.size(); ++idx) { + bool is_intersecting = false; + for (auto succ_idx = idx + 1; succ_idx < bound.size(); ++succ_idx) { + const auto intersection = tier4_autoware_utils::intersect( + bound[idx - 1], bound[idx], bound[succ_idx - 1], bound[succ_idx]); + if ( + intersection && + tier4_autoware_utils::calcDistance2d(*intersection, bound[idx - 1]) < 1e-3 && + tier4_autoware_utils::calcDistance2d(*intersection, bound[idx]) < 1e-3) { + idx = succ_idx; + is_intersecting = true; + } + } + if (!is_intersecting) no_loop_bound.push_back(bound[idx]); } - if (!boost::geometry::equals(end_left.intersection_point, *end_left.segment_it)) - path.left_bound.push_back(convert_point(end_left.intersection_point)); - if (start_right.segment_it < end_right.segment_it) { - for (auto it = std::prev(start_right.segment_it); it >= da.begin(); --it) - path.right_bound.push_back(convert_point(*it)); - for (auto it = std::prev(da.end()); it > end_right.segment_it; --it) - path.right_bound.push_back(convert_point(*it)); - } else { - for (auto it = std::prev(start_right.segment_it); it > end_right.segment_it; --it) - path.right_bound.push_back(convert_point(*it)); + bound = no_loop_bound; +} + +std::vector calculate_smoothed_curvatures( + const std::vector & poses, const size_t smoothing_window_size) +{ + const auto curvatures = motion_utils::calcCurvature(poses); + std::vector smoothed_curvatures(curvatures.size()); + for (auto i = 0UL; i < curvatures.size(); ++i) { + auto sum = 0.0; + const auto from_idx = (i >= smoothing_window_size ? i - smoothing_window_size : 0); + const auto to_idx = std::min(i + smoothing_window_size, curvatures.size() - 1); + for (auto j = from_idx; j <= to_idx; ++j) sum += std::abs(curvatures[j]); + smoothed_curvatures[i] = sum / static_cast(to_idx - from_idx + 1); } - if (!boost::geometry::equals(end_right.intersection_point, *std::next(end_right.segment_it))) - path.right_bound.push_back(convert_point(end_right.intersection_point)); - // remove possible duplicated points - const auto point_cmp = [](const auto & p1, const auto & p2) { - return p1.x == p2.x && p1.y == p2.y; - }; - path.left_bound.erase( - std::unique(path.left_bound.begin(), path.left_bound.end(), point_cmp), path.left_bound.end()); - path.right_bound.erase( - std::unique(path.right_bound.begin(), path.right_bound.end(), point_cmp), - path.right_bound.end()); - copy_z_over_arc_length(original_left_bound, path.left_bound); - copy_z_over_arc_length(original_right_bound, path.right_bound); + return smoothed_curvatures; +} + +void expand_drivable_area( + PathWithLaneId & path, + const std::shared_ptr planner_data) +{ + // skip if no bounds or not enough points to calculate path curvature + if (path.points.size() < 3 || path.left_bound.empty() || path.right_bound.empty()) return; + tier4_autoware_utils::StopWatch stop_watch; + stop_watch.tic("overall"); + stop_watch.tic("preprocessing"); + // crop first/last non deviating path_poses + const auto & params = planner_data->drivable_area_expansion_parameters; + const auto & route_handler = *planner_data->route_handler; + const auto uncrossable_segments = extract_uncrossable_segments( + *route_handler.getLaneletMapPtr(), planner_data->self_odometry->pose.pose.position, params); + const auto uncrossable_polygons = create_object_footprints(*planner_data->dynamic_object, params); + const auto preprocessing_ms = stop_watch.toc("preprocessing"); + + stop_watch.tic("crop"); + std::vector path_poses = planner_data->drivable_area_expansion_prev_path_poses; + std::vector curvatures = planner_data->drivable_area_expansion_prev_curvatures; + + reuse_previous_poses( + path, path_poses, curvatures, planner_data->self_odometry->pose.pose.position, params); + const auto crop_ms = stop_watch.toc("crop"); + + stop_watch.tic("curvatures_expansion"); + // Only add curvatures for the new points. Curvatures of reused path points are not updated. + const auto new_curvatures = + calculate_smoothed_curvatures(path_poses, params.curvature_average_window); + const auto first_new_point_idx = curvatures.size(); + curvatures.insert( + curvatures.end(), new_curvatures.begin() + first_new_point_idx, new_curvatures.end()); + auto left_expansions = + calculate_minimum_expansions(path_poses, path.left_bound, curvatures, LEFT, params); + auto right_expansions = + calculate_minimum_expansions(path_poses, path.right_bound, curvatures, RIGHT, params); + const auto curvature_expansion_ms = stop_watch.toc("curvatures_expansion"); + + stop_watch.tic("max_dist"); + const auto max_left_expansions = calculate_maximum_distance( + path_poses, path.left_bound, uncrossable_segments, uncrossable_polygons, params); + const auto max_right_expansions = calculate_maximum_distance( + path_poses, path.right_bound, uncrossable_segments, uncrossable_polygons, params); + for (auto i = 0LU; i < left_expansions.size(); ++i) + left_expansions[i] = std::min(left_expansions[i], max_left_expansions[i]); + for (auto i = 0LU; i < right_expansions.size(); ++i) + right_expansions[i] = std::min(right_expansions[i], max_right_expansions[i]); + const auto max_dist_ms = stop_watch.toc("max_dist"); + + stop_watch.tic("smooth"); + apply_bound_change_rate_limit(left_expansions, path.left_bound, params.max_bound_rate); + apply_bound_change_rate_limit(right_expansions, path.right_bound, params.max_bound_rate); + const auto smooth_ms = stop_watch.toc("smooth"); + + // TODO(Maxime): limit the distances based on the total width (left + right < min_lane_width) + stop_watch.tic("expand"); + expand_bound(path.left_bound, path_poses, left_expansions); + expand_bound(path.right_bound, path_poses, right_expansions); + const auto expand_ms = stop_watch.toc("expand"); + + const auto total_ms = stop_watch.toc("overall"); + if (params.print_runtime) + std::printf( + "Total runtime(ms): %2.2f\n\tPreprocessing: %2.2f\n\tCrop: %2.2f\n\tCurvature expansion: " + "%2.2f\n\tMaximum expansion: %2.2f\n\tSmoothing: %2.2f\n\tExpansion: %2.2f\n\n", + total_ms, preprocessing_ms, crop_ms, curvature_expansion_ms, max_dist_ms, smooth_ms, + expand_ms); + planner_data->drivable_area_expansion_prev_path_poses = path_poses; + planner_data->drivable_area_expansion_prev_curvatures = curvatures; } } // namespace drivable_area_expansion diff --git a/planning/behavior_path_planner/src/utils/drivable_area_expansion/expansion.cpp b/planning/behavior_path_planner/src/utils/drivable_area_expansion/expansion.cpp deleted file mode 100644 index 035cc579d2a82..0000000000000 --- a/planning/behavior_path_planner/src/utils/drivable_area_expansion/expansion.cpp +++ /dev/null @@ -1,232 +0,0 @@ -// Copyright 2023 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/utils/drivable_area_expansion/expansion.hpp" - -#include "behavior_path_planner/utils/drivable_area_expansion/map_utils.hpp" -#include "behavior_path_planner/utils/drivable_area_expansion/path_projection.hpp" - -namespace drivable_area_expansion -{ - -double calculateDistanceLimit( - const linestring_t & base_ls, const polygon_t & expansion_polygon, - const multi_linestring_t & limit_lines) -{ - auto dist_limit = std::numeric_limits::max(); - multi_point_t intersections; - boost::geometry::intersection(expansion_polygon, limit_lines, intersections); - for (const auto & p : intersections) - dist_limit = std::min(dist_limit, boost::geometry::distance(p, base_ls)); - return dist_limit; -} - -double calculateDistanceLimit( - const linestring_t & base_ls, const polygon_t & expansion_polygon, - const multi_polygon_t & limit_polygons) -{ - auto dist_limit = std::numeric_limits::max(); - for (const auto & polygon : limit_polygons) { - multi_point_t intersections; - boost::geometry::intersection(expansion_polygon, polygon, intersections); - for (const auto & p : intersections) - dist_limit = std::min(dist_limit, boost::geometry::distance(p, base_ls)); - } - return dist_limit; -} - -polygon_t createExpansionPolygon( - const linestring_t & base_ls, const double dist, const bool is_left_side) -{ - namespace strategy = boost::geometry::strategy::buffer; - multi_polygon_t polygons; - // set a non 0 value for the buffer as it sometimes causes no polygon to be returned by bg:buffer - constexpr auto zero = 0.1; - const auto left_dist = is_left_side ? dist : zero; - const auto right_dist = !is_left_side ? dist : zero; - const auto distance_strategy = strategy::distance_asymmetric(left_dist, right_dist); - boost::geometry::buffer( - base_ls, polygons, distance_strategy, strategy::side_straight(), strategy::join_miter(), - strategy::end_flat(), strategy::point_square()); - return polygons.front(); -} - -std::array calculate_arc_length_range_and_distance( - const linestring_t & path_ls, const polygon_t & footprint, const linestring_t & bound, - const bool is_left, const double path_length) -{ - multi_point_t intersections; - double expansion_dist = 0.0; - double from_arc_length = std::numeric_limits::max(); - double to_arc_length = std::numeric_limits::min(); - boost::geometry::intersection(footprint, bound, intersections); - if (!intersections.empty()) { - for (const auto & intersection : intersections) { - const auto projection = point_to_linestring_projection(intersection, path_ls); - if (projection.arc_length <= 0.0 || projection.arc_length >= path_length) continue; - from_arc_length = std::min(from_arc_length, projection.arc_length); - to_arc_length = std::max(to_arc_length, projection.arc_length); - } - for (const auto & p : footprint.outer()) { - const auto projection = point_to_linestring_projection(p, path_ls); - if (projection.arc_length <= 0.0 || projection.arc_length >= path_length) continue; - if (is_left == (projection.distance > 0) && std::abs(projection.distance) > expansion_dist) { - expansion_dist = std::abs(projection.distance); - from_arc_length = std::min(from_arc_length, projection.arc_length); - to_arc_length = std::max(to_arc_length, projection.arc_length); - } - } - } - return std::array({from_arc_length, to_arc_length, expansion_dist}); -} - -polygon_t create_compensation_polygon( - const linestring_t & base_ls, const double compensation_dist, const bool is_left, - const multi_linestring_t uncrossable_lines, const multi_polygon_t & predicted_paths) -{ - polygon_t compensation_polygon = createExpansionPolygon(base_ls, compensation_dist, !is_left); - double dist_limit = std::min( - compensation_dist, calculateDistanceLimit(base_ls, compensation_polygon, uncrossable_lines)); - if (!predicted_paths.empty()) - dist_limit = - std::min(dist_limit, calculateDistanceLimit(base_ls, compensation_polygon, predicted_paths)); - if (dist_limit < compensation_dist) - compensation_polygon = createExpansionPolygon(base_ls, dist_limit, !is_left); - return compensation_polygon; -} - -multi_polygon_t createExpansionPolygons( - const PathWithLaneId & path, const multi_polygon_t & path_footprints, - const multi_polygon_t & predicted_paths, const multi_linestring_t & uncrossable_lines, - const DrivableAreaExpansionParameters & params) -{ - linestring_t path_ls; - linestring_t left_ls; - linestring_t right_ls; - for (const auto & p : path.points) - path_ls.emplace_back(p.point.pose.position.x, p.point.pose.position.y); - for (const auto & p : path.left_bound) left_ls.emplace_back(p.x, p.y); - for (const auto & p : path.right_bound) right_ls.emplace_back(p.x, p.y); - // extend the path linestring to the beginning and end of the drivable area - if (!right_ls.empty() && !left_ls.empty() && path_ls.size() > 2) { - const auto left_proj_begin = point_to_line_projection(left_ls.front(), path_ls[0], path_ls[1]); - const auto right_proj_begin = - point_to_line_projection(right_ls.front(), path_ls[0], path_ls[1]); - const auto left_ls_proj_begin = point_to_linestring_projection(left_proj_begin.point, path_ls); - const auto right_ls_proj_begin = - point_to_linestring_projection(right_proj_begin.point, path_ls); - if (left_ls_proj_begin.arc_length < right_ls_proj_begin.arc_length) - path_ls.insert(path_ls.begin(), left_proj_begin.point); - else - path_ls.insert(path_ls.begin(), right_proj_begin.point); - const auto left_proj_end = - point_to_line_projection(left_ls.back(), path_ls[path_ls.size() - 2], path_ls.back()); - const auto right_proj_end = - point_to_line_projection(right_ls.back(), path_ls[path_ls.size() - 2], path_ls.back()); - const auto left_ls_proj_end = point_to_linestring_projection(left_proj_end.point, path_ls); - const auto right_ls_proj_end = point_to_linestring_projection(right_proj_end.point, path_ls); - if (left_ls_proj_end.arc_length > right_ls_proj_end.arc_length) - path_ls.push_back(left_proj_end.point); - else - path_ls.push_back(right_proj_end.point); - } - const auto path_length = static_cast(boost::geometry::length(path_ls)); - - multi_polygon_t expansion_polygons; - for (const auto & footprint : path_footprints) { - bool is_left = true; - for (const auto & bound : {left_ls, right_ls}) { - auto [from_arc_length, to_arc_length, footprint_dist] = - calculate_arc_length_range_and_distance(path_ls, footprint, bound, is_left, path_length); - if (footprint_dist > 0.0) { - from_arc_length -= params.extra_arc_length; - to_arc_length += params.extra_arc_length; - from_arc_length = std::max(0.0, from_arc_length); - to_arc_length = std::min(path_length, to_arc_length); - const auto base_ls = sub_linestring(path_ls, from_arc_length, to_arc_length); - const auto expansion_dist = params.max_expansion_distance != 0.0 - ? std::min(params.max_expansion_distance, footprint_dist) - : footprint_dist; - auto expansion_polygon = createExpansionPolygon(base_ls, expansion_dist, is_left); - auto limited_dist = expansion_dist; - const auto uncrossable_dist_limit = std::max( - 0.0, calculateDistanceLimit(base_ls, expansion_polygon, uncrossable_lines) - - params.avoid_linestring_dist); - if (uncrossable_dist_limit < limited_dist) { - limited_dist = uncrossable_dist_limit; - if (params.compensate_uncrossable_lines) { - const auto compensation_dist = - footprint_dist - limited_dist + params.compensate_extra_dist; - expansion_polygons.push_back(create_compensation_polygon( - base_ls, compensation_dist, is_left, uncrossable_lines, predicted_paths)); - } - } - limited_dist = std::min( - limited_dist, calculateDistanceLimit(base_ls, expansion_polygon, predicted_paths)); - if (limited_dist < expansion_dist) - expansion_polygon = createExpansionPolygon(base_ls, limited_dist, is_left); - expansion_polygons.push_back(expansion_polygon); - } - is_left = false; - } - } - return expansion_polygons; -} - -multi_polygon_t createExpansionLaneletPolygons( - const lanelet::ConstLanelets & path_lanes, const route_handler::RouteHandler & route_handler, - const multi_polygon_t & path_footprints, const multi_polygon_t & predicted_paths, - const DrivableAreaExpansionParameters & params) -{ - multi_polygon_t expansion_polygons; - lanelet::ConstLanelets candidates; - const auto already_added = [&](const auto & ll) { - return std::find_if(candidates.begin(), candidates.end(), [&](const auto & l) { - return ll.id() == l.id(); - }) != candidates.end(); - }; - const auto add_if_valid = [&](const auto & ll, const auto is_left) { - const auto bound_to_check = is_left ? ll.rightBound() : ll.leftBound(); - if (std::find_if(path_lanes.begin(), path_lanes.end(), [&](const auto & l) { - return ll.id() == l.id(); - }) == path_lanes.end()) - if (!already_added(ll) && !hasTypes(bound_to_check, params.avoid_linestring_types)) - candidates.push_back(ll); - }; - for (const auto & current_ll : path_lanes) { - for (const auto & left_ll : - route_handler.getLaneletsFromPoint(current_ll.leftBound3d().front())) - add_if_valid(left_ll, true); - for (const auto & left_ll : route_handler.getLaneletsFromPoint(current_ll.leftBound3d().back())) - add_if_valid(left_ll, true); - for (const auto & right_ll : - route_handler.getLaneletsFromPoint(current_ll.rightBound3d().front())) - add_if_valid(right_ll, false); - for (const auto & right_ll : - route_handler.getLaneletsFromPoint(current_ll.rightBound3d().back())) - add_if_valid(right_ll, false); - } - for (const auto & candidate : candidates) { - polygon_t candidate_poly; - for (const auto & p : candidate.polygon2d()) candidate_poly.outer().emplace_back(p.x(), p.y()); - boost::geometry::correct(candidate_poly); - if ( - !boost::geometry::overlaps(candidate_poly, predicted_paths) && - boost::geometry::overlaps(path_footprints, candidate_poly)) - expansion_polygons.push_back(candidate_poly); - } - return expansion_polygons; -} - -} // namespace drivable_area_expansion diff --git a/planning/behavior_path_planner/src/utils/drivable_area_expansion/footprints.cpp b/planning/behavior_path_planner/src/utils/drivable_area_expansion/footprints.cpp index 0c31093a83c0e..2e6d4929fdf02 100644 --- a/planning/behavior_path_planner/src/utils/drivable_area_expansion/footprints.cpp +++ b/planning/behavior_path_planner/src/utils/drivable_area_expansion/footprints.cpp @@ -16,6 +16,9 @@ #include "behavior_path_planner/utils/drivable_area_expansion/parameters.hpp" +#include +#include + #include #include @@ -25,63 +28,39 @@ namespace drivable_area_expansion { -polygon_t translatePolygon(const polygon_t & polygon, const double x, const double y) +Polygon2d translate_polygon(const Polygon2d & polygon, const double x, const double y) { - polygon_t translated_polygon; + Polygon2d translated_polygon; const boost::geometry::strategy::transform::translate_transformer translation(x, y); boost::geometry::transform(polygon, translated_polygon, translation); return translated_polygon; } -polygon_t createFootprint(const geometry_msgs::msg::Pose & pose, const polygon_t base_footprint) +Polygon2d create_footprint(const geometry_msgs::msg::Pose & pose, const Polygon2d base_footprint) { const auto angle = tf2::getYaw(pose.orientation); - return translatePolygon(rotatePolygon(base_footprint, angle), pose.position.x, pose.position.y); + return translate_polygon( + tier4_autoware_utils::rotatePolygon(base_footprint, angle), pose.position.x, pose.position.y); } -multi_polygon_t createObjectFootprints( +MultiPolygon2d create_object_footprints( const autoware_auto_perception_msgs::msg::PredictedObjects & objects, const DrivableAreaExpansionParameters & params) { - multi_polygon_t footprints; + MultiPolygon2d footprints; if (params.avoid_dynamic_objects) { for (const auto & object : objects.objects) { const auto front = object.shape.dimensions.x / 2 + params.dynamic_objects_extra_front_offset; const auto rear = -object.shape.dimensions.x / 2 - params.dynamic_objects_extra_rear_offset; const auto left = object.shape.dimensions.y / 2 + params.dynamic_objects_extra_left_offset; const auto right = -object.shape.dimensions.y / 2 - params.dynamic_objects_extra_right_offset; - polygon_t base_footprint; + Polygon2d base_footprint; base_footprint.outer() = { - point_t{front, left}, point_t{front, right}, point_t{rear, right}, point_t{rear, left}, - point_t{front, left}}; + Point2d{front, left}, Point2d{front, right}, Point2d{rear, right}, Point2d{rear, left}, + Point2d{front, left}}; for (const auto & path : object.kinematics.predicted_paths) for (const auto & pose : path.path) - footprints.push_back(createFootprint(pose, base_footprint)); - } - } - return footprints; -} - -multi_polygon_t createPathFootprints( - const PathWithLaneId & path, const DrivableAreaExpansionParameters & params) -{ - const auto left = params.ego_left_offset + params.ego_extra_left_offset; - const auto right = params.ego_right_offset - params.ego_extra_right_offset; - const auto rear = params.ego_rear_offset - params.ego_extra_rear_offset; - const auto front = params.ego_front_offset + params.ego_extra_front_offset; - polygon_t base_footprint; - base_footprint.outer() = { - point_t{front, left}, point_t{front, right}, point_t{rear, right}, point_t{rear, left}, - point_t{front, left}}; - multi_polygon_t footprints; - // skip the last footprint as its orientation is usually wrong - footprints.reserve(path.points.size() - 1); - double arc_length = 0.0; - for (auto it = path.points.begin(); std::next(it) != path.points.end(); ++it) { - footprints.push_back(createFootprint(it->point.pose, base_footprint)); - if (params.max_path_arc_length > 0.0) { - arc_length += tier4_autoware_utils::calcDistance2d(it->point.pose, std::next(it)->point.pose); - if (arc_length > params.max_path_arc_length) break; + footprints.push_back(create_footprint(pose, base_footprint)); } } return footprints; diff --git a/planning/behavior_path_planner/src/utils/drivable_area_expansion/map_utils.cpp b/planning/behavior_path_planner/src/utils/drivable_area_expansion/map_utils.cpp index ded67c251f7ae..6ed14138c62e4 100644 --- a/planning/behavior_path_planner/src/utils/drivable_area_expansion/map_utils.cpp +++ b/planning/behavior_path_planner/src/utils/drivable_area_expansion/map_utils.cpp @@ -15,33 +15,38 @@ #include "behavior_path_planner/utils/drivable_area_expansion/map_utils.hpp" -#include "behavior_path_planner/utils/drivable_area_expansion/types.hpp" -#include "lanelet2_core/primitives/LineString.h" - #include #include +#include #include namespace drivable_area_expansion { -multi_linestring_t extractUncrossableLines( - const lanelet::LaneletMap & lanelet_map, const std::vector & uncrossable_types) +SegmentRtree extract_uncrossable_segments( + const lanelet::LaneletMap & lanelet_map, const Point & ego_point, + const DrivableAreaExpansionParameters & params) { - multi_linestring_t lines; - linestring_t line; + SegmentRtree uncrossable_segments_in_range; + LineString2d line; + const auto ego_p = Point2d{ego_point.x, ego_point.y}; for (const auto & ls : lanelet_map.lineStringLayer) { - if (hasTypes(ls, uncrossable_types)) { + if (has_types(ls, params.avoid_linestring_types)) { line.clear(); - for (const auto & p : ls) line.push_back(point_t{p.x(), p.y()}); - lines.push_back(line); + for (const auto & p : ls) line.push_back(Point2d{p.x(), p.y()}); + for (auto segment_idx = 0LU; segment_idx + 1 < line.size(); ++segment_idx) { + Segment2d segment = {line[segment_idx], line[segment_idx + 1]}; + if (boost::geometry::distance(segment, ego_p) < params.max_path_arc_length) { + uncrossable_segments_in_range.insert(segment); + } + } } } - return lines; + return uncrossable_segments_in_range; } -bool hasTypes(const lanelet::ConstLineString3d & ls, const std::vector & types) +bool has_types(const lanelet::ConstLineString3d & ls, const std::vector & types) { constexpr auto no_type = ""; const auto type = ls.attributeOr(lanelet::AttributeName::Type, no_type); diff --git a/planning/behavior_path_planner/src/utils/geometric_parallel_parking/geometric_parallel_parking.cpp b/planning/behavior_path_planner/src/utils/geometric_parallel_parking/geometric_parallel_parking.cpp index a5ccfc9517771..d5bf67f754ab7 100644 --- a/planning/behavior_path_planner/src/utils/geometric_parallel_parking/geometric_parallel_parking.cpp +++ b/planning/behavior_path_planner/src/utils/geometric_parallel_parking/geometric_parallel_parking.cpp @@ -18,11 +18,10 @@ #include "behavior_path_planner/utils/start_planner/util.hpp" #include "behavior_path_planner/utils/utils.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" +#include "tier4_autoware_utils/math/unit_conversion.hpp" #include -#include #include -#include #ifdef ROS_DISTRO_GALACTIC #include @@ -31,10 +30,12 @@ #include #include + +#include #endif -#include -#include +#include + #include #include @@ -117,9 +118,10 @@ void GeometricParallelParking::setVelocityToArcPaths( } std::vector GeometricParallelParking::generatePullOverPaths( - const Pose & start_pose, const Pose & goal_pose, const double R_E_r, + const Pose & start_pose, const Pose & goal_pose, const double R_E_far, const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes, - const bool is_forward, const double end_pose_offset, const double velocity) + const bool is_forward, const bool left_side_parking, const double end_pose_offset, + const double velocity) { const double lane_departure_margin = is_forward ? parameters_.forward_parking_lane_departure_margin @@ -127,8 +129,8 @@ std::vector GeometricParallelParking::generatePullOverPaths( const double arc_path_interval = is_forward ? parameters_.forward_parking_path_interval : parameters_.backward_parking_path_interval; auto arc_paths = planOneTrial( - start_pose, goal_pose, R_E_r, road_lanes, shoulder_lanes, is_forward, end_pose_offset, - lane_departure_margin, arc_path_interval); + start_pose, goal_pose, R_E_far, road_lanes, shoulder_lanes, is_forward, left_side_parking, + end_pose_offset, lane_departure_margin, arc_path_interval); if (arc_paths.empty()) { return std::vector{}; } @@ -163,11 +165,13 @@ void GeometricParallelParking::clearPaths() current_path_idx_ = 0; arc_paths_.clear(); paths_.clear(); + pairs_terminal_velocity_and_accel_.clear(); } bool GeometricParallelParking::planPullOver( const Pose & goal_pose, const lanelet::ConstLanelets & road_lanes, - const lanelet::ConstLanelets & shoulder_lanes, const bool is_forward) + const lanelet::ConstLanelets & shoulder_lanes, const bool is_forward, + const bool left_side_parking) { const auto & common_params = planner_data_->parameters; const double end_pose_offset = is_forward ? -parameters_.after_forward_parking_straight_distance @@ -182,21 +186,22 @@ bool GeometricParallelParking::planPullOver( if (is_forward) { // When turning forward to the right, the front left goes out, // so reduce the steer angle at that time for seach no lane departure path. + // TODO(Sugahara): define in the config constexpr double start_pose_offset = 0.0; constexpr double min_steer_rad = 0.05; constexpr double steer_interval = 0.1; for (double steer = parameters_.forward_parking_max_steer_angle; steer > min_steer_rad; steer -= steer_interval) { - const double R_E_r = common_params.wheel_base / std::tan(steer); - const auto start_pose = - calcStartPose(arc_end_pose, road_lanes, start_pose_offset, R_E_r, is_forward); + const double R_E_far = common_params.wheel_base / std::tan(steer); + const auto start_pose = calcStartPose( + arc_end_pose, road_lanes, start_pose_offset, R_E_far, is_forward, left_side_parking); if (!start_pose) { continue; } const auto paths = generatePullOverPaths( - *start_pose, goal_pose, R_E_r, road_lanes, shoulder_lanes, is_forward, end_pose_offset, - parameters_.forward_parking_velocity); + *start_pose, goal_pose, R_E_far, road_lanes, shoulder_lanes, is_forward, left_side_parking, + end_pose_offset, parameters_.forward_parking_velocity); if (!paths.empty()) { paths_ = paths; return true; @@ -210,15 +215,15 @@ bool GeometricParallelParking::planPullOver( constexpr double offset_interval = 1.0; for (double start_pose_offset = 0; start_pose_offset < max_offset; start_pose_offset += offset_interval) { - const auto start_pose = - calcStartPose(arc_end_pose, road_lanes, start_pose_offset, R_E_min_, is_forward); + const auto start_pose = calcStartPose( + arc_end_pose, road_lanes, start_pose_offset, R_E_min_, is_forward, left_side_parking); if (!start_pose) { continue; } const auto paths = generatePullOverPaths( - *start_pose, goal_pose, R_E_min_, road_lanes, shoulder_lanes, is_forward, end_pose_offset, - parameters_.backward_parking_velocity); + *start_pose, goal_pose, R_E_min_, road_lanes, shoulder_lanes, is_forward, left_side_parking, + end_pose_offset, parameters_.backward_parking_velocity); if (!paths.empty()) { paths_ = paths; return true; @@ -231,7 +236,7 @@ bool GeometricParallelParking::planPullOver( bool GeometricParallelParking::planPullOut( const Pose & start_pose, const Pose & goal_pose, const lanelet::ConstLanelets & road_lanes, - const lanelet::ConstLanelets & shoulder_lanes) + const lanelet::ConstLanelets & shoulder_lanes, const bool left_side_start) { constexpr bool is_forward = false; // parking backward means pull_out forward constexpr double start_pose_offset = 0.0; // start_pose is current_pose @@ -242,15 +247,16 @@ bool GeometricParallelParking::planPullOut( end_pose_offset += offset_interval) { // pull_out end pose which is the second arc path end const auto end_pose = - calcStartPose(start_pose, road_lanes, end_pose_offset, R_E_min_, is_forward); + calcStartPose(start_pose, road_lanes, end_pose_offset, R_E_min_, is_forward, left_side_start); if (!end_pose) { continue; } // plan reverse path of parking. end_pose <-> start_pose auto arc_paths = planOneTrial( - *end_pose, start_pose, R_E_min_, road_lanes, shoulder_lanes, is_forward, start_pose_offset, - parameters_.pull_out_lane_departure_margin, parameters_.pull_out_path_interval); + *end_pose, start_pose, R_E_min_, road_lanes, shoulder_lanes, is_forward, left_side_start, + start_pose_offset, parameters_.pull_out_lane_departure_margin, + parameters_.pull_out_path_interval); if (arc_paths.empty()) { // not found path continue; @@ -277,8 +283,13 @@ bool GeometricParallelParking::planPullOut( s_start, planner_data_->parameters.forward_path_length, road_lanes, goal_pose); const double s_end = path_end_info.first; const bool path_terminal_is_goal = path_end_info.second; - PathWithLaneId road_center_line_path = - planner_data_->route_handler->getCenterLinePath(road_lanes, s_start, s_end, true); + const PathWithLaneId road_center_line_path = utils::resamplePathWithSpline( + planner_data_->route_handler->getCenterLinePath(road_lanes, s_start, s_end, true), + parameters_.center_line_path_interval); + + if (road_center_line_path.points.empty()) { + continue; + } // check the continuity of straight path and arc path const Pose & road_path_first_pose = road_center_line_path.points.front().point.pose; @@ -317,7 +328,7 @@ bool GeometricParallelParking::planPullOut( boost::optional GeometricParallelParking::calcStartPose( const Pose & goal_pose, const lanelet::ConstLanelets & road_lanes, const double start_pose_offset, - const double R_E_r, const bool is_forward) + const double R_E_far, const bool is_forward, const bool left_side_parking) { const auto arc_coordinates = lanelet::utils::getArcCoordinates(road_lanes, goal_pose); @@ -326,7 +337,8 @@ boost::optional GeometricParallelParking::calcStartPose( // But the left turn should also have a minimum turning radius. // see https://www.sciencedirect.com/science/article/pii/S1474667016436852 for the dx detail const double squared_distance_to_arc_connect = - std::pow(R_E_r, 2) - std::pow(-arc_coordinates.distance / 2 + R_E_r, 2); + left_side_parking ? std::pow(R_E_far, 2) - std::pow(-arc_coordinates.distance / 2 + R_E_far, 2) + : std::pow(R_E_far, 2) - std::pow(arc_coordinates.distance / 2 + R_E_far, 2); if (squared_distance_to_arc_connect < 0) { // may be current_pose is behind the lane return boost::none; @@ -348,8 +360,10 @@ PathWithLaneId GeometricParallelParking::generateStraightPath( const Pose current_pose = planner_data_->self_odometry->pose.pose; const auto current_arc_position = lanelet::utils::getArcCoordinates(road_lanes, current_pose); - auto path = planner_data_->route_handler->getCenterLinePath( - road_lanes, current_arc_position.length, start_arc_position.length, true); + auto path = utils::resamplePathWithSpline( + planner_data_->route_handler->getCenterLinePath( + road_lanes, current_arc_position.length, start_arc_position.length, true), + parameters_.center_line_path_interval); path.header = planner_data_->route_handler->getRouteHeader(); if (!path.points.empty()) { path.points.back().point.longitudinal_velocity_mps = 0; @@ -359,10 +373,10 @@ PathWithLaneId GeometricParallelParking::generateStraightPath( } std::vector GeometricParallelParking::planOneTrial( - const Pose & start_pose, const Pose & goal_pose, const double R_E_r, + const Pose & start_pose, const Pose & goal_pose, const double R_E_far, const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes, - const bool is_forward, const double end_pose_offset, const double lane_departure_margin, - const double arc_path_interval) + const bool is_forward, const bool left_side_parking, const double end_pose_offset, + const double lane_departure_margin, const double arc_path_interval) { clearPaths(); @@ -374,18 +388,21 @@ std::vector GeometricParallelParking::planOneTrial( const double goal_yaw = tf2::getYaw(arc_end_pose.orientation); const double psi = normalizeRadian(self_yaw - goal_yaw); - const Pose Cr = calcOffsetPose(arc_end_pose, 0, -R_E_r, 0); - const double d_Cr_Einit = calcDistance2d(Cr, start_pose); + const Pose C_far = left_side_parking ? calcOffsetPose(arc_end_pose, 0, -R_E_far, 0) + : calcOffsetPose(arc_end_pose, 0, R_E_far, 0); + const double d_C_far_Einit = calcDistance2d(C_far, start_pose); - const Point Cr_goal_coords = inverseTransformPoint(Cr.position, arc_end_pose); + const Point C_far_goal_coords = inverseTransformPoint(C_far.position, arc_end_pose); const Point self_point_goal_coords = inverseTransformPoint(start_pose.position, arc_end_pose); const double alpha = - M_PI_2 - psi + std::asin((self_point_goal_coords.y - Cr_goal_coords.y) / d_Cr_Einit); + left_side_parking + ? M_PI_2 - psi + std::asin((self_point_goal_coords.y - C_far_goal_coords.y) / d_C_far_Einit) + : M_PI_2 + psi - std::asin((self_point_goal_coords.y - C_far_goal_coords.y) / d_C_far_Einit); - const double R_E_l = - (std::pow(d_Cr_Einit, 2) - std::pow(R_E_r, 2)) / (2 * (R_E_r + d_Cr_Einit * std::cos(alpha))); - if (R_E_l <= 0) { + const double R_E_near = (std::pow(d_C_far_Einit, 2) - std::pow(R_E_far, 2)) / + (2 * (R_E_far + d_C_far_Einit * std::cos(alpha))); + if (R_E_near <= 0) { return std::vector{}; } @@ -404,49 +421,65 @@ std::vector GeometricParallelParking::planOneTrial( // If start_pose is parallel to goal_pose, we can know lateral deviation of edges of vehicle, // and detect lane departure. - if (is_forward) { // Check left bound - const double R_front_left = - std::hypot(R_E_r + common_params.vehicle_width / 2, common_params.base_link2front); - const double distance_to_left_bound = - utils::getSignedDistanceFromBoundary(shoulder_lanes, arc_end_pose, true); - const double left_deviation = R_front_left - R_E_r; - if (std::abs(distance_to_left_bound) - left_deviation < lane_departure_margin) { + if (is_forward) { // Check near bound + const double R_front_near = + std::hypot(R_E_far + common_params.vehicle_width / 2, common_params.base_link2front); + const double distance_to_near_bound = + utils::getSignedDistanceFromBoundary(shoulder_lanes, arc_end_pose, left_side_parking); + const double near_deviation = R_front_near - R_E_far; + if (std::abs(distance_to_near_bound) - near_deviation < lane_departure_margin) { return std::vector{}; } - } else { // Check right bound - const double R_front_right = - std::hypot(R_E_l + common_params.vehicle_width / 2, common_params.base_link2front); - const double right_deviation = R_front_right - R_E_l; - const double distance_to_right_bound = - utils::getSignedDistanceFromBoundary(lanes, start_pose, false); - if (distance_to_right_bound - right_deviation < lane_departure_margin) { + } else { // Check far bound + const double R_front_far = + std::hypot(R_E_near + common_params.vehicle_width / 2, common_params.base_link2front); + const double far_deviation = R_front_far - R_E_near; + const double distance_to_far_bound = + utils::getSignedDistanceFromBoundary(lanes, start_pose, !left_side_parking); + if (std::abs(distance_to_far_bound) - far_deviation < lane_departure_margin) { return std::vector{}; } } - // Generate arc path(left turn -> right turn) - const Pose Cl = calcOffsetPose(start_pose, 0, R_E_l, 0); - double theta_l = std::acos( - (std::pow(R_E_l, 2) + std::pow(R_E_l + R_E_r, 2) - std::pow(d_Cr_Einit, 2)) / - (2 * R_E_l * (R_E_l + R_E_r))); - theta_l = is_forward ? theta_l : -theta_l; - - PathWithLaneId path_turn_left = generateArcPath( - Cl, R_E_l, -M_PI_2, normalizeRadian(-M_PI_2 + theta_l), arc_path_interval, is_forward, - is_forward); - path_turn_left.header = route_handler->getRouteHeader(); + // Generate arc path(first turn -> second turn) + const Pose C_near = left_side_parking ? calcOffsetPose(start_pose, 0, R_E_near, 0) + : calcOffsetPose(start_pose, 0, -R_E_near, 0); + const double theta_near = + std::acos( + (std::pow(R_E_near, 2) + std::pow(R_E_near + R_E_far, 2) - std::pow(d_C_far_Einit, 2)) / + (2 * R_E_near * (R_E_near + R_E_far))) * + (is_forward == left_side_parking ? 1 : -1); + + const auto generateArcPathWithHeader = + [&]( + const auto & C, const auto & R_E, const auto & start_angle, const auto & end_angle, + bool is_forward_first, bool is_forward_second) -> PathWithLaneId { + auto path = generateArcPath( + C, R_E, start_angle, end_angle, arc_path_interval, is_forward_first, is_forward_second); + path.header = route_handler->getRouteHeader(); + return path; + }; - PathWithLaneId path_turn_right = generateArcPath( - Cr, R_E_r, normalizeRadian(psi + M_PI_2 + theta_l), M_PI_2, arc_path_interval, !is_forward, - is_forward); - path_turn_right.header = route_handler->getRouteHeader(); + PathWithLaneId path_turn_first = + left_side_parking + ? generateArcPathWithHeader( + C_near, R_E_near, -M_PI_2, normalizeRadian(-M_PI_2 + theta_near), is_forward, is_forward) + : generateArcPathWithHeader( + C_near, R_E_near, M_PI_2, normalizeRadian(M_PI_2 + theta_near), !is_forward, is_forward); + + PathWithLaneId path_turn_second = + left_side_parking ? generateArcPathWithHeader( + C_far, R_E_far, normalizeRadian(psi + M_PI_2 + theta_near), M_PI_2, + !is_forward, is_forward) + : generateArcPathWithHeader( + C_far, R_E_far, normalizeRadian(psi - M_PI_2 + theta_near), -M_PI_2, + is_forward, is_forward); // Need to add straight path to last right_turning for parking in parallel if (std::abs(end_pose_offset) > 0) { PathPointWithLaneId straight_point{}; straight_point.point.pose = goal_pose; - // setLaneIds(straight_point); - path_turn_right.points.push_back(straight_point); + path_turn_second.points.push_back(straight_point); } // Populate lane ids for a given path. @@ -464,8 +497,8 @@ std::vector GeometricParallelParking::planOneTrial( } } }; - populateLaneIds(path_turn_left); - populateLaneIds(path_turn_right); + populateLaneIds(path_turn_first); + populateLaneIds(path_turn_second); // Set lane ids to each point in a given path. // It assigns the accumulated lane ids from path_lane_ids to each point's lane_ids member. @@ -474,12 +507,25 @@ std::vector GeometricParallelParking::planOneTrial( p.lane_ids = path_lane_ids; } }; - setLaneIdsToPath(path_turn_left); - setLaneIdsToPath(path_turn_right); + setLaneIdsToPath(path_turn_first); + setLaneIdsToPath(path_turn_second); // generate arc path vector - paths_.push_back(path_turn_left); - paths_.push_back(path_turn_right); + paths_.push_back(path_turn_first); + paths_.push_back(path_turn_second); + + // set terminal velocity and acceleration(temporary implementation) + if (is_forward) { + pairs_terminal_velocity_and_accel_.push_back( + std::make_pair(parameters_.forward_parking_velocity, 0.0)); + pairs_terminal_velocity_and_accel_.push_back( + std::make_pair(parameters_.forward_parking_velocity, 0.0)); + } else { + pairs_terminal_velocity_and_accel_.push_back( + std::make_pair(parameters_.backward_parking_velocity, 0.0)); + pairs_terminal_velocity_and_accel_.push_back( + std::make_pair(parameters_.backward_parking_velocity, 0.0)); + } // set pull_over start and end pose // todo: make start and end pose for pull_out @@ -487,8 +533,8 @@ std::vector GeometricParallelParking::planOneTrial( arc_end_pose_ = arc_end_pose; // debug - Cr_ = Cr; - Cl_ = Cl; + Cr_ = left_side_parking ? C_far : C_near; + Cl_ = left_side_parking ? C_near : C_far; return paths_; } diff --git a/planning/behavior_path_planner/src/utils/goal_planner/default_fixed_goal_planner.cpp b/planning/behavior_path_planner/src/utils/goal_planner/default_fixed_goal_planner.cpp index 5d77027efa5d0..4c9832c374c63 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/default_fixed_goal_planner.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/default_fixed_goal_planner.cpp @@ -21,7 +21,6 @@ #include #include -#include namespace behavior_path_planner { @@ -35,7 +34,7 @@ BehaviorModuleOutput DefaultFixedGoalPlanner::plan( const PathWithLaneId smoothed_path = modifyPathForSmoothGoalConnection(*(output.path), planner_data); output.path = std::make_shared(smoothed_path); - output.reference_path = std::make_shared(smoothed_path); + output.reference_path = getPreviousModuleOutput().reference_path; return output; } diff --git a/planning/behavior_path_planner/src/utils/goal_planner/freespace_pull_over.cpp b/planning/behavior_path_planner/src/utils/goal_planner/freespace_pull_over.cpp index f4aabb13b8916..9322350877ad1 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/freespace_pull_over.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/freespace_pull_over.cpp @@ -16,6 +16,7 @@ #include "behavior_path_planner/utils/goal_planner/util.hpp" #include "behavior_path_planner/utils/path_utils.hpp" +#include "behavior_path_planner/utils/start_goal_planner_common/utils.hpp" #include #include @@ -55,16 +56,16 @@ boost::optional FreespacePullOver::plan(const Pose & goal_pose) const Pose end_pose = use_back_ ? goal_pose : tier4_autoware_utils::calcOffsetPose(goal_pose, -straight_distance, 0.0, 0.0); - const bool found_path = planner_->makePlan(current_pose, end_pose); - if (!found_path) { + if (!planner_->makePlan(current_pose, end_pose)) { return {}; } const auto road_lanes = utils::getExtendedCurrentLanes( planner_data_, parameters_.backward_goal_search_length, parameters_.forward_goal_search_length, /*forward_only_in_route*/ false); - const auto pull_over_lanes = - goal_planner_utils::getPullOverLanes(*(planner_data_->route_handler), left_side_parking_); + const auto pull_over_lanes = goal_planner_utils::getPullOverLanes( + *(planner_data_->route_handler), left_side_parking_, parameters_.backward_goal_search_length, + parameters_.forward_goal_search_length); if (road_lanes.empty() || pull_over_lanes.empty()) { return {}; } @@ -113,18 +114,21 @@ boost::optional FreespacePullOver::plan(const Pose & goal_pose) addPose(goal_pose); } - utils::correctDividedPathVelocity(partial_paths); + std::vector> pairs_terminal_velocity_and_accel{}; + pairs_terminal_velocity_and_accel.resize(partial_paths.size()); + utils::start_goal_planner_common::modifyVelocityByDirection( + partial_paths, pairs_terminal_velocity_and_accel, velocity_, 0); + // Check if driving forward for each path, return empty if not for (auto & path : partial_paths) { - const auto is_driving_forward = motion_utils::isDrivingForward(path.points); - if (!is_driving_forward) { - // path points is less than 2 + if (!motion_utils::isDrivingForward(path.points)) { return {}; } } PullOverPath pull_over_path{}; pull_over_path.partial_paths = partial_paths; + pull_over_path.pairs_terminal_velocity_and_accel = pairs_terminal_velocity_and_accel; pull_over_path.start_pose = current_pose; pull_over_path.end_pose = goal_pose; pull_over_path.type = getPlannerType(); diff --git a/planning/behavior_path_planner/src/utils/goal_planner/geometric_pull_over.cpp b/planning/behavior_path_planner/src/utils/goal_planner/geometric_pull_over.cpp index 6a5ccc827db29..017ecb218bea3 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/geometric_pull_over.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/geometric_pull_over.cpp @@ -15,7 +15,6 @@ #include "behavior_path_planner/utils/goal_planner/geometric_pull_over.hpp" #include "behavior_path_planner/utils/goal_planner/util.hpp" -#include "behavior_path_planner/utils/path_utils.hpp" #include #include @@ -34,7 +33,8 @@ GeometricPullOver::GeometricPullOver( parallel_parking_parameters_{parameters.parallel_parking_parameters}, lane_departure_checker_{lane_departure_checker}, occupancy_grid_map_{occupancy_grid_map}, - is_forward_{is_forward} + is_forward_{is_forward}, + left_side_parking_{parameters.parking_policy == ParkingPolicy::LEFT_SIDE} { planner_.setParameters(parallel_parking_parameters_); } @@ -47,13 +47,13 @@ boost::optional GeometricPullOver::plan(const Pose & goal_pose) const auto road_lanes = utils::getExtendedCurrentLanes( planner_data_, parameters_.backward_goal_search_length, parameters_.forward_goal_search_length, /*forward_only_in_route*/ false); - const auto shoulder_lanes = - goal_planner_utils::getPullOverLanes(*route_handler, left_side_parking_); - if (road_lanes.empty() || shoulder_lanes.empty()) { + const auto pull_over_lanes = goal_planner_utils::getPullOverLanes( + *route_handler, left_side_parking_, parameters_.backward_goal_search_length, + parameters_.forward_goal_search_length); + if (road_lanes.empty() || pull_over_lanes.empty()) { return {}; } - auto lanes = road_lanes; - lanes.insert(lanes.end(), shoulder_lanes.begin(), shoulder_lanes.end()); + const auto lanes = utils::combineLanelets(road_lanes, pull_over_lanes); const auto & p = parallel_parking_parameters_; const double max_steer_angle = @@ -62,7 +62,7 @@ boost::optional GeometricPullOver::plan(const Pose & goal_pose) planner_.setPlannerData(planner_data_); const bool found_valid_path = - planner_.planPullOver(goal_pose, road_lanes, shoulder_lanes, is_forward_); + planner_.planPullOver(goal_pose, road_lanes, pull_over_lanes, is_forward_, left_side_parking_); if (!found_valid_path) { return {}; } @@ -75,6 +75,7 @@ boost::optional GeometricPullOver::plan(const Pose & goal_pose) PullOverPath pull_over_path{}; pull_over_path.type = getPlannerType(); pull_over_path.partial_paths = planner_.getPaths(); + pull_over_path.pairs_terminal_velocity_and_accel = planner_.getPairsTerminalVelocityAndAccel(); pull_over_path.start_pose = planner_.getStartPose(); pull_over_path.end_pose = planner_.getArcEndPose(); diff --git a/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp b/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp index 1e57dcc319dc5..1e6dc1776359b 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp @@ -17,10 +17,16 @@ #include "behavior_path_planner/utils/goal_planner/util.hpp" #include "behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp" #include "behavior_path_planner/utils/path_utils.hpp" +#include "lanelet2_extension/regulatory_elements/no_parking_area.hpp" +#include "lanelet2_extension/regulatory_elements/no_stopping_area.hpp" #include "lanelet2_extension/utility/utilities.hpp" +#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" +#include #include +#include + #include #include @@ -30,7 +36,58 @@ using lane_departure_checker::LaneDepartureChecker; using lanelet::autoware::NoParkingArea; using lanelet::autoware::NoStoppingArea; using tier4_autoware_utils::calcOffsetPose; -using tier4_autoware_utils::inverseTransformPose; + +// Sort with smaller longitudinal distances taking precedence over smaller lateral distances. +struct SortByLongitudinalDistance +{ + bool prioritize_goals_before_objects{false}; + explicit SortByLongitudinalDistance(bool prioritize_goals_before_objects) + : prioritize_goals_before_objects(prioritize_goals_before_objects) + { + } + + bool operator()(const GoalCandidate & a, const GoalCandidate & b) const noexcept + { + if (prioritize_goals_before_objects) { + if (a.num_objects_to_avoid != b.num_objects_to_avoid) { + return a.num_objects_to_avoid < b.num_objects_to_avoid; + } + } + + const double diff = a.distance_from_original_goal - b.distance_from_original_goal; + constexpr double eps = 0.01; + // If the longitudinal distances are approximately equal, sort based on lateral offset. + if (std::abs(diff) < eps) { + return a.lateral_offset < b.lateral_offset; + } + return a.distance_from_original_goal < b.distance_from_original_goal; + } +}; + +// Sort with the weighted sum of the longitudinal distance and the lateral distance weighted by +// lateral_cost. +struct SortByWeightedDistance +{ + double lateral_cost{0.0}; + bool prioritize_goals_before_objects{false}; + + SortByWeightedDistance(double cost, bool prioritize_goals_before_objects) + : lateral_cost(cost), prioritize_goals_before_objects(prioritize_goals_before_objects) + { + } + + bool operator()(const GoalCandidate & a, const GoalCandidate & b) const noexcept + { + if (prioritize_goals_before_objects) { + if (a.num_objects_to_avoid != b.num_objects_to_avoid) { + return a.num_objects_to_avoid < b.num_objects_to_avoid; + } + } + + return a.distance_from_original_goal + lateral_cost * a.lateral_offset < + b.distance_from_original_goal + lateral_cost * b.lateral_offset; + } +}; GoalSearcher::GoalSearcher( const GoalPlannerParameters & parameters, const LinearRing2d & vehicle_footprint, @@ -42,7 +99,7 @@ GoalSearcher::GoalSearcher( { } -GoalCandidates GoalSearcher::search(const Pose & original_goal_pose) +GoalCandidates GoalSearcher::search() { GoalCandidates goal_candidates{}; @@ -53,30 +110,33 @@ GoalCandidates GoalSearcher::search(const Pose & original_goal_pose) const double lateral_offset_interval = parameters_.lateral_offset_interval; const double max_lateral_offset = parameters_.max_lateral_offset; const double ignore_distance_from_lane_start = parameters_.ignore_distance_from_lane_start; + const double vehicle_width = planner_data_->parameters.vehicle_width; + const double base_link2front = planner_data_->parameters.base_link2front; + const double base_link2rear = planner_data_->parameters.base_link2rear; - const auto pull_over_lanes = - goal_planner_utils::getPullOverLanes(*route_handler, left_side_parking_); + const auto pull_over_lanes = goal_planner_utils::getPullOverLanes( + *route_handler, left_side_parking_, parameters_.backward_goal_search_length, + parameters_.forward_goal_search_length); auto lanes = utils::getExtendedCurrentLanes( planner_data_, backward_length, forward_length, /*forward_only_in_route*/ false); lanes.insert(lanes.end(), pull_over_lanes.begin(), pull_over_lanes.end()); const auto goal_arc_coords = - lanelet::utils::getArcCoordinates(pull_over_lanes, original_goal_pose); + lanelet::utils::getArcCoordinates(pull_over_lanes, reference_goal_pose_); const double s_start = std::max(0.0, goal_arc_coords.length - backward_length); const double s_end = goal_arc_coords.length + forward_length; auto center_line_path = utils::resamplePathWithSpline( route_handler->getCenterLinePath(pull_over_lanes, s_start, s_end), parameters_.goal_search_interval); - const auto [shoulder_lane_objects, others] = - utils::path_safety_checker::separateObjectsByLanelets( - *(planner_data_->dynamic_object), pull_over_lanes); - std::vector original_search_poses{}; // for search area visualizing size_t goal_id = 0; for (const auto & p : center_line_path.points) { - const Pose & center_pose = p.point.pose; + // todo(kosuke55): fix orientation for inverseTransformPoint temporarily + Pose center_pose = p.point.pose; + center_pose.orientation = + tier4_autoware_utils::createQuaternionFromYaw(tf2::getYaw(center_pose.orientation)); // ignore goal_pose near lane start const double distance_from_lane_start = @@ -85,24 +145,26 @@ GoalCandidates GoalSearcher::search(const Pose & original_goal_pose) continue; } - const auto distance_from_left_bound = utils::getSignedDistanceFromBoundary( - pull_over_lanes, vehicle_footprint_, center_pose, left_side_parking_); - if (!distance_from_left_bound) continue; + const auto distance_from_bound = utils::getSignedDistanceFromBoundary( + pull_over_lanes, vehicle_width, base_link2front, base_link2rear, center_pose, + left_side_parking_); + if (!distance_from_bound) continue; - const double sign = left_side_parking_ ? 1.0 : -1.0; + const double sign = left_side_parking_ ? -1.0 : 1.0; const double offset_from_center_line = - sign * (std::abs(distance_from_left_bound.value()) - margin_from_boundary); + -distance_from_bound.value() + sign * margin_from_boundary; + // original means non lateral offset poses const Pose original_search_pose = calcOffsetPose(center_pose, 0, offset_from_center_line, 0); const double longitudinal_distance_from_original_goal = std::abs(motion_utils::calcSignedArcLength( - center_line_path.points, original_goal_pose.position, original_search_pose.position)); + center_line_path.points, reference_goal_pose_.position, original_search_pose.position)); original_search_poses.push_back(original_search_pose); // for createAreaPolygon Pose search_pose{}; // search goal_pose in lateral direction double lateral_offset = 0.0; for (double dy = 0; dy <= max_lateral_offset; dy += lateral_offset_interval) { lateral_offset = dy; - search_pose = calcOffsetPose(original_search_pose, 0, -sign * dy, 0); + search_pose = calcOffsetPose(original_search_pose, 0, sign * dy, 0); const auto transformed_vehicle_footprint = transformVector(vehicle_footprint_, tier4_autoware_utils::pose2transform(search_pose)); @@ -131,33 +193,116 @@ GoalCandidates GoalSearcher::search(const Pose & original_goal_pose) } createAreaPolygons(original_search_poses); - // Sort with distance from original goal - std::sort(goal_candidates.begin(), goal_candidates.end()); + update(goal_candidates); return goal_candidates; } +void GoalSearcher::countObjectsToAvoid( + GoalCandidates & goal_candidates, const PredictedObjects & objects) const +{ + const auto & route_handler = planner_data_->route_handler; + const double forward_length = parameters_.forward_goal_search_length; + const double backward_length = parameters_.backward_goal_search_length; + + // calculate search start/end pose in pull over lanes + const auto [search_start_pose, search_end_pose] = std::invoke([&]() -> std::pair { + const auto pull_over_lanes = goal_planner_utils::getPullOverLanes( + *route_handler, left_side_parking_, parameters_.backward_goal_search_length, + parameters_.forward_goal_search_length); + const auto goal_arc_coords = + lanelet::utils::getArcCoordinates(pull_over_lanes, reference_goal_pose_); + const double s_start = std::max(0.0, goal_arc_coords.length - backward_length); + const double s_end = goal_arc_coords.length + forward_length; + const auto center_line_path = utils::resamplePathWithSpline( + route_handler->getCenterLinePath(pull_over_lanes, s_start, s_end), + parameters_.goal_search_interval); + return std::make_pair( + center_line_path.points.front().point.pose, center_line_path.points.back().point.pose); + }); + + // generate current lane center line path to check collision with objects + const auto current_lanes = utils::getExtendedCurrentLanes( + planner_data_, parameters_.backward_goal_search_length, parameters_.forward_goal_search_length, + /*forward_only_in_route*/ false); + const auto current_center_line_path = std::invoke([&]() -> PathWithLaneId { + const double s_start = + lanelet::utils::getArcCoordinates(current_lanes, search_start_pose).length; + const double s_end = lanelet::utils::getArcCoordinates(current_lanes, search_end_pose).length; + return utils::resamplePathWithSpline( + route_handler->getCenterLinePath(current_lanes, s_start, s_end), 1.0); + }); + + // reset num_objects_to_avoid + for (auto & goal_candidate : goal_candidates) { + goal_candidate.num_objects_to_avoid = 0; + } + + // count number of objects to avoid + for (const auto & object : objects.objects) { + for (const auto & p : current_center_line_path.points) { + const auto transformed_vehicle_footprint = + transformVector(vehicle_footprint_, tier4_autoware_utils::pose2transform(p.point.pose)); + const auto obj_polygon = tier4_autoware_utils::toPolygon2d(object); + const double distance = boost::geometry::distance(obj_polygon, transformed_vehicle_footprint); + if (distance > parameters_.object_recognition_collision_check_margin) { + continue; + } + const Pose & object_pose = object.kinematics.initial_pose_with_covariance.pose; + const double s_object = lanelet::utils::getArcCoordinates(current_lanes, object_pose).length; + for (auto & goal_candidate : goal_candidates) { + const Pose & goal_pose = goal_candidate.goal_pose; + const double s_goal = lanelet::utils::getArcCoordinates(current_lanes, goal_pose).length; + if (s_object < s_goal) { + goal_candidate.num_objects_to_avoid++; + } + } + break; + } + } +} + void GoalSearcher::update(GoalCandidates & goal_candidates) const { + const auto stop_objects = utils::path_safety_checker::filterObjectsByVelocity( + *(planner_data_->dynamic_object), parameters_.th_moving_object_velocity); + const auto pull_over_lanes = goal_planner_utils::getPullOverLanes( + *(planner_data_->route_handler), left_side_parking_, parameters_.backward_goal_search_length, + parameters_.forward_goal_search_length); + const auto [pull_over_lane_stop_objects, others] = + utils::path_safety_checker::separateObjectsByLanelets( + stop_objects, pull_over_lanes, utils::path_safety_checker::isPolygonOverlapLanelet); + + if (parameters_.prioritize_goals_before_objects) { + countObjectsToAvoid(goal_candidates, pull_over_lane_stop_objects); + } + + if (parameters_.goal_priority == "minimum_weighted_distance") { + std::sort( + goal_candidates.begin(), goal_candidates.end(), + SortByWeightedDistance( + parameters_.minimum_weighted_distance_lateral_weight, + parameters_.prioritize_goals_before_objects)); + } else if (parameters_.goal_priority == "minimum_longitudinal_distance") { + std::sort( + goal_candidates.begin(), goal_candidates.end(), + SortByLongitudinalDistance(parameters_.prioritize_goals_before_objects)); + } + // update is_safe for (auto & goal_candidate : goal_candidates) { const Pose goal_pose = goal_candidate.goal_pose; // check collision with footprint - if (checkCollision(goal_pose)) { + if (checkCollision(goal_pose, pull_over_lane_stop_objects)) { goal_candidate.is_safe = false; continue; } - // check margin with pull over lane objects - const auto pull_over_lanes = - goal_planner_utils::getPullOverLanes(*(planner_data_->route_handler), left_side_parking_); - const auto [shoulder_lane_objects, others] = - utils::path_safety_checker::separateObjectsByLanelets( - *(planner_data_->dynamic_object), pull_over_lanes); + // check longitudinal margin with pull over lane objects constexpr bool filter_inside = true; const auto target_objects = goal_planner_utils::filterObjectsByLateralDistance( - goal_pose, planner_data_->parameters.vehicle_width, shoulder_lane_objects, + goal_pose, planner_data_->parameters.vehicle_width, pull_over_lane_stop_objects, parameters_.object_recognition_collision_check_margin, filter_inside); if (checkCollisionWithLongitudinalDistance(goal_pose, target_objects)) { goal_candidate.is_safe = false; @@ -168,9 +313,9 @@ void GoalSearcher::update(GoalCandidates & goal_candidates) const } } -bool GoalSearcher::checkCollision(const Pose & pose) const +bool GoalSearcher::checkCollision(const Pose & pose, const PredictedObjects & objects) const { - if (parameters_.use_occupancy_grid) { + if (parameters_.use_occupancy_grid_for_goal_search) { const Pose pose_grid_coords = global2local(occupancy_grid_map_->getMap(), pose); const auto idx = pose2index( occupancy_grid_map_->getMap(), pose_grid_coords, occupancy_grid_map_->getParam().theta_size); @@ -182,7 +327,7 @@ bool GoalSearcher::checkCollision(const Pose & pose) const if (parameters_.use_object_recognition) { if (utils::checkCollisionBetweenFootprintAndObjects( - vehicle_footprint_, pose, *(planner_data_->dynamic_object), + vehicle_footprint_, pose, objects, parameters_.object_recognition_collision_check_margin)) { return true; } @@ -191,9 +336,11 @@ bool GoalSearcher::checkCollision(const Pose & pose) const } bool GoalSearcher::checkCollisionWithLongitudinalDistance( - const Pose & ego_pose, const PredictedObjects & dynamic_objects) const + const Pose & ego_pose, const PredictedObjects & objects) const { - if (parameters_.use_occupancy_grid && parameters_.use_occupancy_grid_for_longitudinal_margin) { + if ( + parameters_.use_occupancy_grid_for_goal_search && + parameters_.use_occupancy_grid_for_goal_longitudinal_margin) { constexpr bool check_out_of_range = false; const double offset = std::max( parameters_.longitudinal_margin - parameters_.occupancy_grid_collision_check_margin, 0.0); @@ -225,8 +372,7 @@ bool GoalSearcher::checkCollisionWithLongitudinalDistance( if ( utils::calcLongitudinalDistanceFromEgoToObjects( ego_pose, planner_data_->parameters.base_link2front, - planner_data_->parameters.base_link2rear, - dynamic_objects) < parameters_.longitudinal_margin) { + planner_data_->parameters.base_link2rear, objects) < parameters_.longitudinal_margin) { return true; } } diff --git a/planning/behavior_path_planner/src/utils/goal_planner/shift_pull_over.cpp b/planning/behavior_path_planner/src/utils/goal_planner/shift_pull_over.cpp index 2f5c1d9b05b7f..60cc2d19c5f35 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/shift_pull_over.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/shift_pull_over.cpp @@ -49,16 +49,16 @@ boost::optional ShiftPullOver::plan(const Pose & goal_pose) const auto road_lanes = utils::getExtendedCurrentLanes( planner_data_, backward_search_length, forward_search_length, /*forward_only_in_route*/ false); - const auto shoulder_lanes = - goal_planner_utils::getPullOverLanes(*route_handler, left_side_parking_); - if (road_lanes.empty() || shoulder_lanes.empty()) { + const auto pull_over_lanes = goal_planner_utils::getPullOverLanes( + *route_handler, left_side_parking_, backward_search_length, forward_search_length); + if (road_lanes.empty() || pull_over_lanes.empty()) { return {}; } // find safe one from paths with different jerk for (double lateral_jerk = min_jerk; lateral_jerk <= max_jerk; lateral_jerk += jerk_resolution) { const auto pull_over_path = - generatePullOverPath(road_lanes, shoulder_lanes, goal_pose, lateral_jerk); + generatePullOverPath(road_lanes, pull_over_lanes, goal_pose, lateral_jerk); if (!pull_over_path) continue; return *pull_over_path; } @@ -110,7 +110,7 @@ boost::optional ShiftPullOver::generatePullOverPath( // generate road lane reference path to shift end const auto road_lane_reference_path_to_shift_end = utils::resamplePathWithSpline( - generateReferencePath(road_lanes, shift_end_pose), resample_interval_); + generateReferencePath(road_lanes, shift_end_pose), parameters_.center_line_path_interval); // calculate shift length const Pose & shift_end_pose_road_lane = @@ -204,6 +204,7 @@ boost::optional ShiftPullOver::generatePullOverPath( PullOverPath pull_over_path{}; pull_over_path.type = getPlannerType(); pull_over_path.partial_paths.push_back(shifted_path.path); + pull_over_path.pairs_terminal_velocity_and_accel.push_back(std::make_pair(pull_over_velocity, 0)); pull_over_path.start_pose = path_shifter.getShiftLines().front().start; pull_over_path.end_pose = path_shifter.getShiftLines().front().end; pull_over_path.debug_poses.push_back(shift_end_pose_road_lane); diff --git a/planning/behavior_path_planner/src/utils/goal_planner/util.cpp b/planning/behavior_path_planner/src/utils/goal_planner/util.cpp index 19b6bd27ea07c..1cd985d566f73 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/util.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/util.cpp @@ -14,22 +14,18 @@ #include "behavior_path_planner/utils/goal_planner/util.hpp" -#include "behavior_path_planner/utils/path_shifter/path_shifter.hpp" -#include "behavior_path_planner/utils/path_utils.hpp" - #include #include #include -#include #include #include +#include #include #include #include -#include #include #include #include @@ -44,47 +40,38 @@ namespace behavior_path_planner { namespace goal_planner_utils { -PathWithLaneId combineReferencePath(const PathWithLaneId & path1, const PathWithLaneId & path2) -{ - PathWithLaneId path; - path.points.insert(path.points.end(), path1.points.begin(), path1.points.end()); - - // skip overlapping point - path.points.insert(path.points.end(), next(path2.points.begin()), path2.points.end()); - - return path; -} -lanelet::ConstLanelets getPullOverLanes(const RouteHandler & route_handler, const bool left_side) +lanelet::ConstLanelets getPullOverLanes( + const RouteHandler & route_handler, const bool left_side, const double backward_distance, + const double forward_distance) { - const Pose goal_pose = route_handler.getGoalPose(); + const Pose goal_pose = route_handler.getOriginalGoalPose(); + + // Buffer to get enough lanes in front of the goal, need much longer than the pull over distance. + // In the case of loop lanes, it may not be possible to extend the lane forward. + // todo(kosuek55): automatically calculates this distance. + const double backward_distance_with_buffer = backward_distance + 100; lanelet::ConstLanelet target_shoulder_lane{}; if (route_handler::RouteHandler::getPullOverTarget( route_handler.getShoulderLanelets(), goal_pose, &target_shoulder_lane)) { // pull over on shoulder lane - return route_handler.getShoulderLaneletSequence(target_shoulder_lane, goal_pose); + return route_handler.getShoulderLaneletSequence( + target_shoulder_lane, goal_pose, backward_distance_with_buffer, forward_distance); } - // pull over on road lane lanelet::ConstLanelet closest_lane{}; route_handler.getClosestLaneletWithinRoute(goal_pose, &closest_lane); - lanelet::ConstLanelet outermost_lane{}; if (left_side) { - outermost_lane = route_handler.getMostLeftLanelet(closest_lane); + outermost_lane = route_handler.getMostLeftLanelet(closest_lane, false, true); } else { - outermost_lane = route_handler.getMostRightLanelet(closest_lane); - } - - lanelet::ConstLanelet outermost_shoulder_lane; - if (route_handler.getLeftShoulderLanelet(outermost_lane, &outermost_shoulder_lane)) { - return route_handler.getShoulderLaneletSequence(outermost_shoulder_lane, goal_pose); + outermost_lane = route_handler.getMostRightLanelet(closest_lane, false, true); } - const bool dist = std::numeric_limits::max(); constexpr bool only_route_lanes = false; - return route_handler.getLaneletSequence(outermost_lane, dist, dist, only_route_lanes); + return route_handler.getLaneletSequence( + outermost_lane, backward_distance_with_buffer, forward_distance, only_route_lanes); } PredictedObjects filterObjectsByLateralDistance( @@ -140,7 +127,7 @@ MarkerArray createPosesMarkerArray( return msg; } -MarkerArray createTextsMarkerArray( +MarkerArray createGoalPriorityTextsMarkerArray( const std::vector & poses, std::string && ns, const std_msgs::msg::ColorRGBA & color) { MarkerArray msg{}; @@ -159,47 +146,72 @@ MarkerArray createTextsMarkerArray( return msg; } -MarkerArray createGoalCandidatesMarkerArray( - GoalCandidates & goal_candidates, const std_msgs::msg::ColorRGBA & color) +MarkerArray createNumObjectsToAvoidTextsMarkerArray( + const GoalCandidates & goal_candidates, std::string && ns, const std_msgs::msg::ColorRGBA & color) { - // convert to pose vector - std::vector pose_vector{}; + MarkerArray msg{}; + int32_t i = 0; for (const auto & goal_candidate : goal_candidates) { - if (goal_candidate.is_safe) { - pose_vector.push_back(goal_candidate.goal_pose); - } + const Pose & pose = goal_candidate.goal_pose; + Marker marker = createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, i, Marker::TEXT_VIEW_FACING, + createMarkerScale(0.3, 0.3, 0.3), color); + marker.pose = calcOffsetPose(pose, -0.5, 0, 1.0); + marker.id = i; + marker.text = std::to_string(goal_candidate.num_objects_to_avoid); + msg.markers.push_back(marker); + i++; } + return msg; +} + +MarkerArray createGoalCandidatesMarkerArray( + const GoalCandidates & goal_candidates, const std_msgs::msg::ColorRGBA & color) +{ + GoalCandidates safe_goal_candidates{}; + std::copy_if( + goal_candidates.begin(), goal_candidates.end(), std::back_inserter(safe_goal_candidates), + [](const auto & goal_candidate) { return goal_candidate.is_safe; }); + + std::vector pose_vector{}; + std::transform( + safe_goal_candidates.begin(), safe_goal_candidates.end(), std::back_inserter(pose_vector), + [](const auto & goal_candidate) { return goal_candidate.goal_pose; }); + auto marker_array = createPosesMarkerArray(pose_vector, "goal_candidates", color); for (const auto & text_marker : - createTextsMarkerArray( + createGoalPriorityTextsMarkerArray( pose_vector, "goal_candidates_priority", createMarkerColor(1.0, 1.0, 1.0, 0.999)) .markers) { marker_array.markers.push_back(text_marker); } + for (const auto & text_marker : createNumObjectsToAvoidTextsMarkerArray( + safe_goal_candidates, "goal_candidates_num_objects_to_avoid", + createMarkerColor(0.5, 0.5, 0.5, 0.999)) + .markers) { + marker_array.markers.push_back(text_marker); + } return marker_array; } -bool isAllowedGoalModification( - const std::shared_ptr & route_handler, const bool left_side_parking) +bool isAllowedGoalModification(const std::shared_ptr & route_handler) { - return route_handler->isAllowedGoalModification() || - checkOriginalGoalIsInShoulder(route_handler, left_side_parking); + return route_handler->isAllowedGoalModification() || checkOriginalGoalIsInShoulder(route_handler); } -bool checkOriginalGoalIsInShoulder( - const std::shared_ptr & route_handler, const bool left_side_parking) +bool checkOriginalGoalIsInShoulder(const std::shared_ptr & route_handler) { const Pose & goal_pose = route_handler->getGoalPose(); + const auto shoulder_lanes = route_handler->getShoulderLanelets(); - const lanelet::ConstLanelets pull_over_lanes = - goal_planner_utils::getPullOverLanes(*(route_handler), left_side_parking); - lanelet::ConstLanelet target_lane{}; - lanelet::utils::query::getClosestLanelet(pull_over_lanes, goal_pose, &target_lane); + lanelet::ConstLanelet closest_shoulder_lane{}; + if (lanelet::utils::query::getClosestLanelet(shoulder_lanes, goal_pose, &closest_shoulder_lane)) { + return lanelet::utils::isInLanelet(goal_pose, closest_shoulder_lane, 0.1); + } - return route_handler->isShoulderLanelet(target_lane) && - lanelet::utils::isInLanelet(goal_pose, target_lane, 0.1); + return false; } } // namespace goal_planner_utils diff --git a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp index f3a1260eecadc..d5867c40824d2 100644 --- a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp +++ b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp @@ -14,27 +14,32 @@ #include "behavior_path_planner/utils/lane_change/utils.hpp" +#include "behavior_path_planner/marker_utils/utils.hpp" #include "behavior_path_planner/parameters.hpp" #include "behavior_path_planner/utils/lane_change/lane_change_module_data.hpp" #include "behavior_path_planner/utils/lane_change/lane_change_path.hpp" -#include "behavior_path_planner/utils/path_safety_checker/safety_check.hpp" #include "behavior_path_planner/utils/path_shifter/path_shifter.hpp" #include "behavior_path_planner/utils/path_utils.hpp" #include "behavior_path_planner/utils/utils.hpp" +#include "object_recognition_utils/predicted_path_utils.hpp" -#include #include #include +#include +#include #include #include +#include #include +#include +#include +#include #include #include #include #include -#include #include #include @@ -175,17 +180,6 @@ std::vector getAccelerationValues( return sampled_values; } -PathWithLaneId combineReferencePath(const PathWithLaneId & path1, const PathWithLaneId & path2) -{ - PathWithLaneId path; - path.points.insert(path.points.end(), path1.points.begin(), path1.points.end()); - - // skip overlapping point - path.points.insert(path.points.end(), next(path2.points.begin()), path2.points.end()); - - return path; -} - lanelet::ConstLanelets getTargetPreferredLanes( const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, const Direction & direction, @@ -242,6 +236,9 @@ lanelet::BasicPolygon2d getTargetNeighborLanesPolygon( { const auto target_neighbor_lanelets = utils::lane_change::getTargetNeighborLanes(route_handler, current_lanes, type); + if (target_neighbor_lanelets.empty()) { + return {}; + } const auto target_neighbor_preferred_lane_poly = lanelet::utils::getPolygonFromArcLength( target_neighbor_lanelets, 0, std::numeric_limits::max()); @@ -304,6 +301,15 @@ std::optional constructCandidatePath( "failed to generate shifted path."); } + // TODO(Zulfaqar Azmi): have to think of a more feasible solution for points being remove by path + // shifter. + if (shifted_path.path.points.size() < shift_line.end_idx + 1) { + RCLCPP_DEBUG( + rclcpp::get_logger("behavior_path_planner").get_child("utils").get_child(__func__), + "path points are removed by PathShifter."); + return std::nullopt; + } + const auto & prepare_length = lane_change_length.prepare; const auto & lane_changing_length = lane_change_length.lane_changing; @@ -350,7 +356,7 @@ std::optional constructCandidatePath( return std::nullopt; } - candidate_path.path = combineReferencePath(prepare_segment, shifted_path.path); + candidate_path.path = utils::combinePath(prepare_segment, shifted_path.path); candidate_path.shifted_path = shifted_path; return std::optional{candidate_path}; @@ -652,22 +658,6 @@ lanelet::ConstLanelets getBackwardLanelets( return backward_lanes; } -bool isTargetObjectType(const PredictedObject & object, const LaneChangeParameters & parameters) -{ - using autoware_auto_perception_msgs::msg::ObjectClassification; - const auto t = utils::getHighestProbLabel(object.classification); - const auto is_object_type = - ((t == ObjectClassification::CAR && parameters.check_car) || - (t == ObjectClassification::TRUCK && parameters.check_truck) || - (t == ObjectClassification::BUS && parameters.check_bus) || - (t == ObjectClassification::TRAILER && parameters.check_trailer) || - (t == ObjectClassification::UNKNOWN && parameters.check_unknown) || - (t == ObjectClassification::BICYCLE && parameters.check_bicycle) || - (t == ObjectClassification::MOTORCYCLE && parameters.check_motorcycle) || - (t == ObjectClassification::PEDESTRIAN && parameters.check_pedestrian)); - return is_object_type; -} - double calcLateralBufferForFiltering(const double vehicle_width, const double lateral_buffer) { return lateral_buffer + 0.5 * vehicle_width; @@ -824,19 +814,6 @@ std::vector convertToPredictedPath( return predicted_path; } -PredictedPath convertToPredictedPath( - const std::vector & path, const double time_resolution) -{ - PredictedPath predicted_path; - predicted_path.time_step = rclcpp::Duration::from_seconds(time_resolution); - predicted_path.path.resize(path.size()); - - for (size_t i = 0; i < path.size(); ++i) { - predicted_path.path.at(i) = path.at(i).pose; - } - return predicted_path; -} - bool isParkedObject( const PathWithLaneId & path, const RouteHandler & route_handler, const ExtendedPredictedObject & object, const double object_check_min_road_shoulder_width, @@ -962,7 +939,8 @@ bool isParkedObject( bool passParkedObject( const RouteHandler & route_handler, const LaneChangePath & lane_change_path, const std::vector & objects, const double minimum_lane_change_length, - const bool is_goal_in_route, const LaneChangeParameters & lane_change_parameters) + const bool is_goal_in_route, const LaneChangeParameters & lane_change_parameters, + CollisionCheckDebugMap & object_debug) { const auto & object_check_min_road_shoulder_width = lane_change_parameters.object_check_min_road_shoulder_width; @@ -985,6 +963,7 @@ bool passParkedObject( } const auto & leading_obj = objects.at(*leading_obj_idx); + auto debug = marker_utils::createObjectDebug(leading_obj); const auto leading_obj_poly = tier4_autoware_utils::toPolygon2d(leading_obj.initial_pose.pose, leading_obj.shape); if (leading_obj_poly.outer().empty()) { @@ -1008,6 +987,8 @@ bool passParkedObject( // If there are still enough length after the target object, we delay the lane change if (min_dist_to_end_of_current_lane > minimum_lane_change_length) { + debug.second.unsafe_reason = "delay lane change"; + marker_utils::updateCollisionCheckDebugMap(object_debug, debug, false); return true; } @@ -1127,4 +1108,25 @@ ExtendedPredictedObject transform( return extended_object; } + +bool isCollidedPolygonsInLanelet( + const std::vector & collided_polygons, const lanelet::ConstLanelets & lanes) +{ + const auto lanes_polygon = createPolygon(lanes, 0.0, std::numeric_limits::max()); + + const auto is_in_lanes = [&](const auto & collided_polygon) { + return lanes_polygon && boost::geometry::intersects(lanes_polygon.value(), collided_polygon); + }; + + return std::any_of(collided_polygons.begin(), collided_polygons.end(), is_in_lanes); +} + +lanelet::ConstLanelets generateExpandedLanelets( + const lanelet::ConstLanelets & lanes, const Direction direction, const double left_offset, + const double right_offset) +{ + const auto left_extend_offset = (direction == Direction::LEFT) ? left_offset : 0.0; + const auto right_extend_offset = (direction == Direction::RIGHT) ? -right_offset : 0.0; + return lanelet::utils::getExpandedLanelets(lanes, left_extend_offset, right_extend_offset); +} } // namespace behavior_path_planner::utils::lane_change diff --git a/planning/behavior_path_planner/src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp b/planning/behavior_path_planner/src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp index d244d08abf99b..0f87c68e38289 100644 --- a/planning/behavior_path_planner/src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp +++ b/planning/behavior_path_planner/src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp @@ -15,7 +15,7 @@ #include "behavior_path_planner/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp" #include -#include +#include #include diff --git a/planning/behavior_path_planner/src/utils/path_safety_checker/objects_filtering.cpp b/planning/behavior_path_planner/src/utils/path_safety_checker/objects_filtering.cpp index 33afe0fe6642f..a7c604985de3a 100644 --- a/planning/behavior_path_planner/src/utils/path_safety_checker/objects_filtering.cpp +++ b/planning/behavior_path_planner/src/utils/path_safety_checker/objects_filtering.cpp @@ -15,10 +15,30 @@ #include "behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp" #include "behavior_path_planner/utils/utils.hpp" +#include "object_recognition_utils/predicted_path_utils.hpp" + +#include +#include + +#include namespace behavior_path_planner::utils::path_safety_checker { +bool isCentroidWithinLanelet(const PredictedObject & object, const lanelet::ConstLanelet & lanelet) +{ + const auto & object_pos = object.kinematics.initial_pose_with_covariance.pose.position; + lanelet::BasicPoint2d object_centroid(object_pos.x, object_pos.y); + return boost::geometry::within(object_centroid, lanelet.polygon2d().basicPolygon()); +} + +bool isPolygonOverlapLanelet(const PredictedObject & object, const lanelet::ConstLanelet & lanelet) +{ + const auto object_polygon = tier4_autoware_utils::toPolygon2d(object); + const auto lanelet_polygon = utils::toPolygon2d(lanelet); + return !boost::geometry::disjoint(lanelet_polygon, object_polygon); +} + PredictedObjects filterObjects( const std::shared_ptr & objects, const std::shared_ptr & route_handler, const lanelet::ConstLanelets & current_lanes, @@ -37,7 +57,7 @@ PredictedObjects filterObjects( PredictedObjects filtered_objects; - filtered_objects = filterObjectsByVelocity(*objects, ignore_object_velocity_threshold); + filtered_objects = filterObjectsByVelocity(*objects, ignore_object_velocity_threshold, false); filterObjectsByClass(filtered_objects, target_object_types); @@ -51,13 +71,19 @@ PredictedObjects filterObjects( return filtered_objects; } -PredictedObjects filterObjectsByVelocity(const PredictedObjects & objects, double lim_v) +PredictedObjects filterObjectsByVelocity( + const PredictedObjects & objects, const double velocity_threshold, + const bool remove_above_threshold) { - return filterObjectsByVelocity(objects, -lim_v, lim_v); + if (remove_above_threshold) { + return filterObjectsByVelocity(objects, -velocity_threshold, velocity_threshold); + } else { + return filterObjectsByVelocity(objects, velocity_threshold, std::numeric_limits::max()); + } } PredictedObjects filterObjectsByVelocity( - const PredictedObjects & objects, double min_v, double max_v) + const PredictedObjects & objects, double velocity_threshold, double max_velocity) { PredictedObjects filtered; filtered.header = objects.header; @@ -65,7 +91,7 @@ PredictedObjects filterObjectsByVelocity( const auto v_norm = std::hypot( obj.kinematics.initial_twist_with_covariance.twist.linear.x, obj.kinematics.initial_twist_with_covariance.twist.linear.y); - if (min_v < v_norm && v_norm < max_v) { + if (velocity_threshold < v_norm && v_norm < max_velocity) { filtered.objects.push_back(obj); } } @@ -101,21 +127,10 @@ void filterObjectsByPosition( void filterObjectsByClass( PredictedObjects & objects, const ObjectTypesToCheck & target_object_types) { - using autoware_auto_perception_msgs::msg::ObjectClassification; - PredictedObjects filtered_objects; for (auto & object : objects.objects) { - const auto t = utils::getHighestProbLabel(object.classification); - const auto is_object_type = - ((t == ObjectClassification::CAR && target_object_types.check_car) || - (t == ObjectClassification::TRUCK && target_object_types.check_truck) || - (t == ObjectClassification::BUS && target_object_types.check_bus) || - (t == ObjectClassification::TRAILER && target_object_types.check_trailer) || - (t == ObjectClassification::UNKNOWN && target_object_types.check_unknown) || - (t == ObjectClassification::BICYCLE && target_object_types.check_bicycle) || - (t == ObjectClassification::MOTORCYCLE && target_object_types.check_motorcycle) || - (t == ObjectClassification::PEDESTRIAN && target_object_types.check_pedestrian)); + const auto is_object_type = isTargetObjectType(object, target_object_types); // If the object type matches any of the target types, add it to the filtered list if (is_object_type) { @@ -125,12 +140,11 @@ void filterObjectsByClass( // Replace the original objects with the filtered list objects = std::move(filtered_objects); - - return; } std::pair, std::vector> separateObjectIndicesByLanelets( - const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets) + const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets, + const std::function & condition) { if (target_lanelets.empty()) { return {}; @@ -140,25 +154,9 @@ std::pair, std::vector> separateObjectIndicesByLanel std::vector other_indices; for (size_t i = 0; i < objects.objects.size(); i++) { - // create object polygon - const auto & obj = objects.objects.at(i); - // create object polygon - const auto obj_polygon = tier4_autoware_utils::toPolygon2d(obj); bool is_filtered_object = false; for (const auto & llt : target_lanelets) { - // create lanelet polygon - const auto polygon2d = llt.polygon2d().basicPolygon(); - if (polygon2d.empty()) { - // no lanelet polygon - continue; - } - Polygon2d lanelet_polygon; - for (const auto & lanelet_point : polygon2d) { - lanelet_polygon.outer().emplace_back(lanelet_point.x(), lanelet_point.y()); - } - lanelet_polygon.outer().push_back(lanelet_polygon.outer().front()); - // check the object does not intersect the lanelet - if (!boost::geometry::disjoint(lanelet_polygon, obj_polygon)) { + if (condition(objects.objects.at(i), llt)) { target_indices.push_back(i); is_filtered_object = true; break; @@ -174,13 +172,14 @@ std::pair, std::vector> separateObjectIndicesByLanel } std::pair separateObjectsByLanelets( - const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets) + const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets, + const std::function & condition) { PredictedObjects target_objects; PredictedObjects other_objects; const auto [target_indices, other_indices] = - separateObjectIndicesByLanelets(objects, target_lanelets); + separateObjectIndicesByLanelets(objects, target_lanelets, condition); target_objects.objects.reserve(target_indices.size()); other_objects.objects.reserve(other_indices.size()); @@ -211,28 +210,44 @@ std::vector getPredictedPathFromObj( return obj.predicted_paths; } -// TODO(Sugahara): should consider delay before departure std::vector createPredictedPath( const std::shared_ptr & ego_predicted_path_params, const std::vector & path_points, - const geometry_msgs::msg::Pose & vehicle_pose, const double current_velocity, size_t ego_seg_idx) + const geometry_msgs::msg::Pose & vehicle_pose, const double current_velocity, + const size_t ego_seg_idx, const bool is_object_front, const bool limit_to_max_velocity) { if (path_points.empty()) { return {}; } - const double min_slow_down_speed = ego_predicted_path_params->min_slow_speed; + const double min_velocity = ego_predicted_path_params->min_velocity; const double acceleration = ego_predicted_path_params->acceleration; - const double time_horizon = ego_predicted_path_params->time_horizon; + const double max_velocity = limit_to_max_velocity ? ego_predicted_path_params->max_velocity + : std::numeric_limits::infinity(); + const double time_horizon = is_object_front + ? ego_predicted_path_params->time_horizon_for_front_object + : ego_predicted_path_params->time_horizon_for_rear_object; const double time_resolution = ego_predicted_path_params->time_resolution; + const double delay_until_departure = ego_predicted_path_params->delay_until_departure; std::vector predicted_path; const auto vehicle_pose_frenet = convertToFrenetPoint(path_points, vehicle_pose.position, ego_seg_idx); - for (double t = 0.0; t < time_horizon + 1e-3; t += time_resolution) { - const double velocity = std::max(current_velocity + acceleration * t, min_slow_down_speed); - const double length = current_velocity * t + 0.5 * acceleration * t * t; + for (double t = 0.0; t < time_horizon; t += time_resolution) { + double velocity = 0.0; + double length = 0.0; + + // If t < delay_until_departure, it means ego have not depart yet, therefore the velocity is + // 0 and there's no change in position. + if (t >= delay_until_departure) { + // Adjust time to consider the delay. + double t_with_delay = t - delay_until_departure; + velocity = + std::clamp(current_velocity + acceleration * t_with_delay, min_velocity, max_velocity); + length = current_velocity * t_with_delay + 0.5 * acceleration * t_with_delay * t_with_delay; + } + const auto pose = motion_utils::calcInterpolatedPose(path_points, vehicle_pose_frenet.length + length); predicted_path.emplace_back(t, pose, velocity); @@ -337,17 +352,32 @@ TargetObjectsOnLane createTargetObjectsOnLane( }; // TODO(Sugahara): Consider shoulder and other lane objects - if (object_lane_configuration.check_current_lane) { + if (object_lane_configuration.check_current_lane && !current_lanes.empty()) { append_objects_on_lane(target_objects_on_lane.on_current_lane, current_lanes); } - if (object_lane_configuration.check_left_lane) { + if (object_lane_configuration.check_left_lane && !all_left_lanelets.empty()) { append_objects_on_lane(target_objects_on_lane.on_left_lane, all_left_lanelets); } - if (object_lane_configuration.check_right_lane) { + if (object_lane_configuration.check_right_lane && !all_right_lanelets.empty()) { append_objects_on_lane(target_objects_on_lane.on_right_lane, all_right_lanelets); } return target_objects_on_lane; } +bool isTargetObjectType( + const PredictedObject & object, const ObjectTypesToCheck & target_object_types) +{ + using autoware_auto_perception_msgs::msg::ObjectClassification; + const auto t = utils::getHighestProbLabel(object.classification); + return ( + (t == ObjectClassification::CAR && target_object_types.check_car) || + (t == ObjectClassification::TRUCK && target_object_types.check_truck) || + (t == ObjectClassification::BUS && target_object_types.check_bus) || + (t == ObjectClassification::TRAILER && target_object_types.check_trailer) || + (t == ObjectClassification::UNKNOWN && target_object_types.check_unknown) || + (t == ObjectClassification::BICYCLE && target_object_types.check_bicycle) || + (t == ObjectClassification::MOTORCYCLE && target_object_types.check_motorcycle) || + (t == ObjectClassification::PEDESTRIAN && target_object_types.check_pedestrian)); +} } // namespace behavior_path_planner::utils::path_safety_checker diff --git a/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp b/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp index c6a68e108e7e1..40aa184105edb 100644 --- a/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp +++ b/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp @@ -14,13 +14,19 @@ #include "behavior_path_planner/utils/path_safety_checker/safety_check.hpp" -#include "behavior_path_planner/marker_utils/utils.hpp" #include "interpolation/linear_interpolation.hpp" #include "motion_utils/trajectory/trajectory.hpp" -#include "object_recognition_utils/predicted_path_utils.hpp" +#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" + +#include +#include +#include namespace behavior_path_planner::utils::path_safety_checker { + +namespace bg = boost::geometry; + void appendPointToPolygon(Polygon2d & polygon, const geometry_msgs::msg::Point & geom_point) { Point2d point; @@ -30,6 +36,32 @@ void appendPointToPolygon(Polygon2d & polygon, const geometry_msgs::msg::Point & bg::append(polygon.outer(), point); } +bool isTargetObjectOncoming( + const geometry_msgs::msg::Pose & vehicle_pose, const geometry_msgs::msg::Pose & object_pose) +{ + return std::abs(calcYawDeviation(vehicle_pose, object_pose)) > M_PI_2; +} + +bool isTargetObjectFront( + const geometry_msgs::msg::Pose & ego_pose, const Polygon2d & obj_polygon, + const vehicle_info_util::VehicleInfo & vehicle_info) +{ + const double base_to_front = vehicle_info.max_longitudinal_offset_m; + const auto ego_offset_pose = + tier4_autoware_utils::calcOffsetPose(ego_pose, base_to_front, 0.0, 0.0); + + // check all edges in the polygon + const auto obj_polygon_outer = obj_polygon.outer(); + for (const auto & obj_edge : obj_polygon_outer) { + const auto obj_point = tier4_autoware_utils::createPoint(obj_edge.x(), obj_edge.y(), 0.0); + if (tier4_autoware_utils::calcLongitudinalDeviation(ego_offset_pose, obj_point) > 0.0) { + return true; + } + } + + return false; +} + bool isTargetObjectFront( const PathWithLaneId & path, const geometry_msgs::msg::Pose & ego_pose, const vehicle_info_util::VehicleInfo & vehicle_info, const Polygon2d & obj_polygon) @@ -39,7 +71,8 @@ bool isTargetObjectFront( tier4_autoware_utils::calcOffsetPose(ego_pose, base_to_front, 0.0, 0.0).position; // check all edges in the polygon - for (const auto & obj_edge : obj_polygon.outer()) { + const auto obj_polygon_outer = obj_polygon.outer(); + for (const auto & obj_edge : obj_polygon_outer) { const auto obj_point = tier4_autoware_utils::createPoint(obj_edge.x(), obj_edge.y(), 0.0); if (motion_utils::isTargetPointFront(path.points, ego_point, obj_point)) { return true; @@ -51,28 +84,33 @@ bool isTargetObjectFront( Polygon2d createExtendedPolygon( const Pose & base_link_pose, const vehicle_info_util::VehicleInfo & vehicle_info, - const double lon_length, const double lat_margin, CollisionCheckDebug & debug) + const double lon_length, const double lat_margin, const double is_stopped_obj, + CollisionCheckDebug & debug) { const double & base_to_front = vehicle_info.max_longitudinal_offset_m; const double & width = vehicle_info.vehicle_width_m; const double & base_to_rear = vehicle_info.rear_overhang_m; - const double lon_offset = std::max(lon_length + base_to_front, base_to_front); - + // if stationary object, extend forward and backward by the half of lon length + const double forward_lon_offset = base_to_front + (is_stopped_obj ? lon_length / 2 : lon_length); + const double backward_lon_offset = + -base_to_rear - (is_stopped_obj ? lon_length / 2 : 0); // minus value const double lat_offset = width / 2.0 + lat_margin; { - debug.longitudinal_offset = lon_offset; - debug.lateral_offset = lat_offset; + debug.forward_lon_offset = forward_lon_offset; + debug.backward_lon_offset = backward_lon_offset; + debug.lat_offset = lat_offset; } - const auto p1 = tier4_autoware_utils::calcOffsetPose(base_link_pose, lon_offset, lat_offset, 0.0); + const auto p1 = + tier4_autoware_utils::calcOffsetPose(base_link_pose, forward_lon_offset, lat_offset, 0.0); const auto p2 = - tier4_autoware_utils::calcOffsetPose(base_link_pose, lon_offset, -lat_offset, 0.0); + tier4_autoware_utils::calcOffsetPose(base_link_pose, forward_lon_offset, -lat_offset, 0.0); const auto p3 = - tier4_autoware_utils::calcOffsetPose(base_link_pose, -base_to_rear, -lat_offset, 0.0); + tier4_autoware_utils::calcOffsetPose(base_link_pose, backward_lon_offset, -lat_offset, 0.0); const auto p4 = - tier4_autoware_utils::calcOffsetPose(base_link_pose, -base_to_rear, lat_offset, 0.0); + tier4_autoware_utils::calcOffsetPose(base_link_pose, backward_lon_offset, lat_offset, 0.0); Polygon2d polygon; appendPointToPolygon(polygon, p1.position); @@ -87,7 +125,7 @@ Polygon2d createExtendedPolygon( Polygon2d createExtendedPolygon( const Pose & obj_pose, const Shape & shape, const double lon_length, const double lat_margin, - CollisionCheckDebug & debug) + const double is_stopped_obj, CollisionCheckDebug & debug) { const auto obj_polygon = tier4_autoware_utils::toPolygon2d(obj_pose, shape); if (obj_polygon.outer().empty()) { @@ -98,7 +136,8 @@ Polygon2d createExtendedPolygon( double min_x = std::numeric_limits::max(); double max_y = std::numeric_limits::lowest(); double min_y = std::numeric_limits::max(); - for (const auto & polygon_p : obj_polygon.outer()) { + const auto obj_polygon_outer = obj_polygon.outer(); + for (const auto & polygon_p : obj_polygon_outer) { const auto obj_p = tier4_autoware_utils::createPoint(polygon_p.x(), polygon_p.y(), 0.0); const auto transformed_p = tier4_autoware_utils::inverseTransformPoint(obj_p, obj_pose); @@ -108,19 +147,27 @@ Polygon2d createExtendedPolygon( min_y = std::min(transformed_p.y, min_y); } - const double lon_offset = max_x + lon_length; + // if stationary object, extend forward and backward by the half of lon length + const double forward_lon_offset = max_x + (is_stopped_obj ? lon_length / 2 : lon_length); + const double backward_lon_offset = min_x - (is_stopped_obj ? lon_length / 2 : 0); // minus value + const double left_lat_offset = max_y + lat_margin; const double right_lat_offset = min_y - lat_margin; { - debug.longitudinal_offset = lon_offset; - debug.lateral_offset = (left_lat_offset + right_lat_offset) / 2; + debug.forward_lon_offset = forward_lon_offset; + debug.backward_lon_offset = backward_lon_offset; + debug.lat_offset = std::max(std::abs(left_lat_offset), std::abs(right_lat_offset)); } - const auto p1 = tier4_autoware_utils::calcOffsetPose(obj_pose, lon_offset, left_lat_offset, 0.0); - const auto p2 = tier4_autoware_utils::calcOffsetPose(obj_pose, lon_offset, right_lat_offset, 0.0); - const auto p3 = tier4_autoware_utils::calcOffsetPose(obj_pose, min_x, right_lat_offset, 0.0); - const auto p4 = tier4_autoware_utils::calcOffsetPose(obj_pose, min_x, left_lat_offset, 0.0); + const auto p1 = + tier4_autoware_utils::calcOffsetPose(obj_pose, forward_lon_offset, left_lat_offset, 0.0); + const auto p2 = + tier4_autoware_utils::calcOffsetPose(obj_pose, forward_lon_offset, right_lat_offset, 0.0); + const auto p3 = + tier4_autoware_utils::calcOffsetPose(obj_pose, backward_lon_offset, right_lat_offset, 0.0); + const auto p4 = + tier4_autoware_utils::calcOffsetPose(obj_pose, backward_lon_offset, left_lat_offset, 0.0); Polygon2d polygon; appendPointToPolygon(polygon, p1.position); @@ -133,6 +180,19 @@ Polygon2d createExtendedPolygon( : tier4_autoware_utils::inverseClockwise(polygon); } +PredictedPath convertToPredictedPath( + const std::vector & path, const double time_resolution) +{ + PredictedPath predicted_path; + predicted_path.time_step = rclcpp::Duration::from_seconds(time_resolution); + predicted_path.path.resize(path.size()); + + for (size_t i = 0; i < path.size(); ++i) { + predicted_path.path.at(i) = path.at(i).pose; + } + return predicted_path; +} + double calcRssDistance( const double front_object_velocity, const double rear_object_velocity, const RSSparams & rss_params) @@ -219,10 +279,30 @@ bool checkCollision( const ExtendedPredictedObject & target_object, const PredictedPathWithPolygon & target_object_path, const BehaviorPathPlannerParameters & common_parameters, const RSSparams & rss_parameters, - CollisionCheckDebug & debug) + const double hysteresis_factor, CollisionCheckDebug & debug) { - debug.lerped_path.reserve(target_object_path.path.size()); + const auto collided_polygons = getCollidedPolygons( + planned_path, predicted_ego_path, target_object, target_object_path, common_parameters, + rss_parameters, hysteresis_factor, debug); + return collided_polygons.empty(); +} +std::vector getCollidedPolygons( + [[maybe_unused]] const PathWithLaneId & planned_path, + const std::vector & predicted_ego_path, + const ExtendedPredictedObject & target_object, + const PredictedPathWithPolygon & target_object_path, + const BehaviorPathPlannerParameters & common_parameters, const RSSparams & rss_parameters, + double hysteresis_factor, CollisionCheckDebug & debug) +{ + { + debug.ego_predicted_path = predicted_ego_path; + debug.obj_predicted_path = target_object_path.path; + debug.current_obj_pose = target_object.initial_pose.pose; + } + + std::vector collided_polygons{}; + collided_polygons.reserve(target_object_path.path.size()); for (const auto & obj_pose_with_poly : target_object_path.path) { const auto & current_time = obj_pose_with_poly.time; @@ -244,23 +324,20 @@ bool checkCollision( const auto & ego_polygon = interpolated_data->poly; const auto & ego_velocity = interpolated_data->velocity; - { - debug.lerped_path.push_back(ego_pose); - debug.expected_ego_pose = ego_pose; - debug.expected_obj_pose = obj_pose; - debug.ego_polygon = ego_polygon; - debug.obj_polygon = obj_polygon; - } - // check overlap if (boost::geometry::overlaps(ego_polygon, obj_polygon)) { - debug.failed_reason = "overlap_polygon"; - return false; + debug.unsafe_reason = "overlap_polygon"; + collided_polygons.push_back(obj_polygon); + + debug.expected_ego_pose = ego_pose; + debug.expected_obj_pose = obj_pose; + debug.extended_ego_polygon = ego_polygon; + debug.extended_obj_polygon = obj_polygon; + continue; } // compute which one is at the front of the other - const bool is_object_front = - isTargetObjectFront(planned_path, ego_pose, ego_vehicle_info, obj_polygon); + const bool is_object_front = isTargetObjectFront(ego_pose, obj_polygon, ego_vehicle_info); const auto & [front_object_velocity, rear_object_velocity] = is_object_front ? std::make_pair(object_velocity, ego_velocity) : std::make_pair(ego_velocity, object_velocity); @@ -273,56 +350,46 @@ bool checkCollision( const auto min_lon_length = calcMinimumLongitudinalLength(front_object_velocity, rear_object_velocity, rss_parameters); - const auto & lon_offset = std::max(rss_dist, min_lon_length); - const auto & lat_margin = rss_parameters.lateral_distance_max_threshold; - const auto & extended_ego_polygon = - is_object_front - ? createExtendedPolygon(ego_pose, ego_vehicle_info, lon_offset, lat_margin, debug) - : ego_polygon; + const auto & lon_offset = std::max(rss_dist, min_lon_length) * hysteresis_factor; + const auto & lat_margin = rss_parameters.lateral_distance_max_threshold * hysteresis_factor; + // TODO(watanabe) fix hard coding value + const bool is_stopped_object = object_velocity < 0.3; + const auto & extended_ego_polygon = is_object_front ? createExtendedPolygon( + ego_pose, ego_vehicle_info, lon_offset, + lat_margin, is_stopped_object, debug) + : ego_polygon; const auto & extended_obj_polygon = is_object_front ? obj_polygon - : createExtendedPolygon(obj_pose, target_object.shape, lon_offset, lat_margin, debug); - - { - debug.rss_longitudinal = rss_dist; - debug.ego_to_obj_margin = min_lon_length; - debug.ego_polygon = extended_ego_polygon; - debug.obj_polygon = extended_obj_polygon; - debug.is_front = is_object_front; - } + : createExtendedPolygon( + obj_pose, target_object.shape, lon_offset, lat_margin, is_stopped_object, debug); // check overlap with extended polygon if (boost::geometry::overlaps(extended_ego_polygon, extended_obj_polygon)) { - debug.failed_reason = "overlap_extended_polygon"; - return false; + debug.unsafe_reason = "overlap_extended_polygon"; + collided_polygons.push_back(obj_polygon); + + debug.rss_longitudinal = rss_dist; + debug.inter_vehicle_distance = min_lon_length; + debug.extended_ego_polygon = extended_ego_polygon; + debug.extended_obj_polygon = extended_obj_polygon; + debug.is_front = is_object_front; } } - return true; + return collided_polygons; } -bool checkCollisionWithExtraStoppingMargin( - const PathWithLaneId & ego_path, const PredictedObjects & dynamic_objects, - const double base_to_front, const double base_to_rear, const double width, - const double maximum_deceleration, const double collision_check_margin, - const double max_extra_stopping_margin) +bool checkPolygonsIntersects( + const std::vector & polys_1, const std::vector & polys_2) { - for (const auto & p : ego_path.points) { - const double extra_stopping_margin = std::min( - std::pow(p.point.longitudinal_velocity_mps, 2) * 0.5 / maximum_deceleration, - max_extra_stopping_margin); - - const auto ego_polygon = tier4_autoware_utils::toFootprint( - p.point.pose, base_to_front + extra_stopping_margin, base_to_rear, width); - - for (const auto & object : dynamic_objects.objects) { - const auto obj_polygon = tier4_autoware_utils::toPolygon2d(object); - const double distance = boost::geometry::distance(obj_polygon, ego_polygon); - if (distance < collision_check_margin) return true; + for (const auto & poly_1 : polys_1) { + for (const auto & poly_2 : polys_2) { + if (boost::geometry::intersects(poly_1, poly_2)) { + return true; + } } } - return false; } } // namespace behavior_path_planner::utils::path_safety_checker diff --git a/planning/behavior_path_planner/src/utils/path_shifter/path_shifter.cpp b/planning/behavior_path_planner/src/utils/path_shifter/path_shifter.cpp index 9ca083af35c92..df2d82b1072d4 100644 --- a/planning/behavior_path_planner/src/utils/path_shifter/path_shifter.cpp +++ b/planning/behavior_path_planner/src/utils/path_shifter/path_shifter.cpp @@ -15,13 +15,11 @@ #include "behavior_path_planner/utils/path_shifter/path_shifter.hpp" #include "behavior_path_planner/utils/path_utils.hpp" -#include "behavior_path_planner/utils/utils.hpp" #include #include -#include +#include -#include #include #include #include @@ -56,6 +54,7 @@ namespace behavior_path_planner using motion_utils::findNearestIndex; using motion_utils::insertOrientation; using motion_utils::removeInvalidOrientationPoints; +using motion_utils::removeOverlapPoints; void PathShifter::setPath(const PathWithLaneId & path) { @@ -147,9 +146,18 @@ bool PathShifter::generate( type == SHIFT_TYPE::SPLINE ? applySplineShifter(shifted_path, offset_back) : applyLinearShifter(shifted_path); - const bool is_driving_forward = true; - insertOrientation(shifted_path->path.points, is_driving_forward); + shifted_path->path.points = removeOverlapPoints(shifted_path->path.points); + // Use orientation before shift to remove points in reverse order + // before setting wrong azimuth orientation removeInvalidOrientationPoints(shifted_path->path.points); + size_t previous_size{shifted_path->path.points.size()}; + do { + previous_size = shifted_path->path.points.size(); + // Set the azimuth orientation to the next point at each point + insertOrientation(shifted_path->path.points, true); + // Use azimuth orientation to remove points in reverse order + removeInvalidOrientationPoints(shifted_path->path.points); + } while (previous_size != shifted_path->path.points.size()); // DEBUG RCLCPP_DEBUG_STREAM_THROTTLE( diff --git a/planning/behavior_path_planner/src/utils/path_utils.cpp b/planning/behavior_path_planner/src/utils/path_utils.cpp index ec14a064bf51b..3ac3c09ba0d97 100644 --- a/planning/behavior_path_planner/src/utils/path_utils.cpp +++ b/planning/behavior_path_planner/src/utils/path_utils.cpp @@ -19,7 +19,8 @@ #include #include #include -#include +#include +#include #include @@ -223,7 +224,7 @@ std::pair getPathTurnSignal( turn_signal.command = TurnIndicatorsCommand::NO_COMMAND; const double max_time = std::numeric_limits::max(); const double max_distance = std::numeric_limits::max(); - if (path.shift_length.empty()) { + if (path.shift_length.size() < shift_line.end_idx + 1) { return std::make_pair(turn_signal, max_distance); } const auto base_link2front = common_parameter.base_link2front; @@ -418,6 +419,7 @@ void correctDividedPathVelocity(std::vector & divided_paths) { for (auto & path : divided_paths) { const auto is_driving_forward = motion_utils::isDrivingForward(path.points); + // If the number of points in the path is less than 2, don't correct the velocity if (!is_driving_forward) { continue; } @@ -557,4 +559,25 @@ PathWithLaneId calcCenterLinePath( return centerline_path; } + +PathWithLaneId combinePath(const PathWithLaneId & path1, const PathWithLaneId & path2) +{ + if (path1.points.empty()) { + return path2; + } + if (path2.points.empty()) { + return path1; + } + + PathWithLaneId path{}; + path.points.insert(path.points.end(), path1.points.begin(), path1.points.end()); + + // skip overlapping point + path.points.insert(path.points.end(), next(path2.points.begin()), path2.points.end()); + + PathWithLaneId filtered_path = path; + filtered_path.points = motion_utils::removeOverlapPoints(filtered_path.points); + return filtered_path; +} + } // namespace behavior_path_planner::utils diff --git a/planning/behavior_path_planner/src/utils/start_goal_planner_common/utils.cpp b/planning/behavior_path_planner/src/utils/start_goal_planner_common/utils.cpp new file mode 100644 index 0000000000000..1eaea06c023f9 --- /dev/null +++ b/planning/behavior_path_planner/src/utils/start_goal_planner_common/utils.cpp @@ -0,0 +1,202 @@ +// Copyright 2023 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/utils/start_goal_planner_common/utils.hpp" + +namespace behavior_path_planner::utils::start_goal_planner_common +{ + +using motion_utils::calcDecelDistWithJerkAndAccConstraints; + +boost::optional calcFeasibleDecelDistance( + std::shared_ptr planner_data, const double acc_lim, const double jerk_lim, + const double target_velocity) +{ + const auto v_now = planner_data->self_odometry->twist.twist.linear.x; + const auto a_now = planner_data->self_acceleration->accel.accel.linear.x; + + if (v_now < target_velocity) { + return 0.0; + } + + auto min_stop_distance = calcDecelDistWithJerkAndAccConstraints( + v_now, target_velocity, a_now, -acc_lim, jerk_lim, -1.0 * jerk_lim); + + if (!min_stop_distance) { + return {}; + } + + min_stop_distance = std::max(*min_stop_distance, 0.0); + + return min_stop_distance; +} + +void modifyVelocityByDirection( + std::vector & paths, + std::vector> & terminal_vel_acc_pairs, const double target_velocity, + const double acceleration) +{ + assert(paths.size() == terminal_vel_acc_pairs.size()); + + auto path_itr = std::begin(paths); + auto pair_itr = std::begin(terminal_vel_acc_pairs); + + for (; path_itr != std::end(paths); ++path_itr, ++pair_itr) { + const auto is_driving_forward = motion_utils::isDrivingForward(path_itr->points); + + // If the number of points in the path is less than 2, don't insert stop velocity and + // set pairs_terminal_velocity_and_accel to 0 + if (!is_driving_forward) { + *pair_itr = std::make_pair(0.0, 0.0); + continue; + } + + if (*is_driving_forward) { + for (auto & point : path_itr->points) { + // TODO(Sugahara): velocity calculation can be improved by considering the acceleration + point.point.longitudinal_velocity_mps = std::abs(point.point.longitudinal_velocity_mps); + } + // TODO(Sugahara): Consider the calculation of the target velocity and acceleration for ego's + // predicted path when ego will stop at the end of the path + *pair_itr = std::make_pair(target_velocity, acceleration); + } else { + for (auto & point : path_itr->points) { + point.point.longitudinal_velocity_mps = -std::abs(point.point.longitudinal_velocity_mps); + } + *pair_itr = std::make_pair(-target_velocity, -acceleration); + } + path_itr->points.back().point.longitudinal_velocity_mps = 0.0; + } +} + +void updateEgoPredictedPathParams( + std::shared_ptr & ego_predicted_path_params, + const std::shared_ptr & start_planner_params) +{ + ego_predicted_path_params = + std::make_shared(start_planner_params->ego_predicted_path_params); +} +void updateEgoPredictedPathParams( + std::shared_ptr & ego_predicted_path_params, + const std::shared_ptr & goal_planner_params) +{ + ego_predicted_path_params = + std::make_shared(goal_planner_params->ego_predicted_path_params); +} + +void updateSafetyCheckParams( + std::shared_ptr & safety_check_params, + const std::shared_ptr & start_planner_params) +{ + safety_check_params = + std::make_shared(start_planner_params->safety_check_params); +} + +void updateSafetyCheckParams( + std::shared_ptr & safety_check_params, + const std::shared_ptr & goal_planner_params) +{ + safety_check_params = + std::make_shared(goal_planner_params->safety_check_params); +} + +void updateObjectsFilteringParams( + std::shared_ptr & objects_filtering_params, + const std::shared_ptr & start_planner_params) +{ + objects_filtering_params = + std::make_shared(start_planner_params->objects_filtering_params); +} + +void updateObjectsFilteringParams( + std::shared_ptr & objects_filtering_params, + const std::shared_ptr & goal_planner_params) +{ + objects_filtering_params = + std::make_shared(goal_planner_params->objects_filtering_params); +} + +void updatePathProperty( + std::shared_ptr & ego_predicted_path_params, + const std::pair & pairs_terminal_velocity_and_accel) +{ + // If acceleration is close to 0, the ego predicted path will be too short, so a minimum value is + // necessary to ensure a reasonable path length. + // TODO(Sugahara): define them as parameter + const double min_accel_for_ego_predicted_path = 1.0; + const double acceleration = + std::max(pairs_terminal_velocity_and_accel.second, min_accel_for_ego_predicted_path); + + ego_predicted_path_params->max_velocity = pairs_terminal_velocity_and_accel.first; + ego_predicted_path_params->acceleration = acceleration; +} + +void initializeCollisionCheckDebugMap(CollisionCheckDebugMap & collision_check_debug_map) +{ + collision_check_debug_map.clear(); +} + +void updateSafetyCheckTargetObjectsData( + StartGoalPlannerData & data, const PredictedObjects & filtered_objects, + const TargetObjectsOnLane & target_objects_on_lane, + const std::vector & ego_predicted_path) +{ + data.filtered_objects = filtered_objects; + data.target_objects_on_lane = target_objects_on_lane; + data.ego_predicted_path = ego_predicted_path; +} + +std::pair getPairsTerminalVelocityAndAccel( + const std::vector> & pairs_terminal_velocity_and_accel, + const size_t current_path_idx) +{ + if (pairs_terminal_velocity_and_accel.size() <= current_path_idx) { + return std::make_pair(0.0, 0.0); + } + return pairs_terminal_velocity_and_accel.at(current_path_idx); +} + +std::optional generateFeasibleStopPath( + PathWithLaneId & current_path, std::shared_ptr planner_data, + geometry_msgs::msg::Pose & stop_pose, const double maximum_deceleration, + const double maximum_jerk) +{ + if (current_path.points.empty()) { + return {}; + } + + // try to insert stop point in current_path after approval + // but if can't stop with constraints(maximum deceleration, maximum jerk), don't insert stop point + const auto min_stop_distance = + behavior_path_planner::utils::start_goal_planner_common::calcFeasibleDecelDistance( + planner_data, maximum_deceleration, maximum_jerk, 0.0); + + if (!min_stop_distance) { + return {}; + } + + // set stop point + const auto stop_idx = motion_utils::insertStopPoint( + planner_data->self_odometry->pose.pose, *min_stop_distance, current_path.points); + + if (!stop_idx) { + return {}; + } + + stop_pose = current_path.points.at(*stop_idx).point.pose; + + return current_path; +} + +} // namespace behavior_path_planner::utils::start_goal_planner_common diff --git a/planning/behavior_path_planner/src/utils/start_planner/freespace_pull_out.cpp b/planning/behavior_path_planner/src/utils/start_planner/freespace_pull_out.cpp index ba34901d9df6a..945c386668dee 100644 --- a/planning/behavior_path_planner/src/utils/start_planner/freespace_pull_out.cpp +++ b/planning/behavior_path_planner/src/utils/start_planner/freespace_pull_out.cpp @@ -44,7 +44,7 @@ FreespacePullOut::FreespacePullOut( } } -boost::optional FreespacePullOut::plan(const Pose start_pose, const Pose end_pose) +boost::optional FreespacePullOut::plan(const Pose & start_pose, const Pose & end_pose) { const auto & route_handler = planner_data_->route_handler; const double backward_path_length = planner_data_->parameters.backward_path_length; @@ -86,7 +86,7 @@ boost::optional FreespacePullOut::plan(const Pose start_pose, const } // push back generate road lane path between end pose and goal pose to last path - const auto & goal_pose = route_handler->getGoalPose(); + const Pose goal_pose = route_handler->getGoalPose(); constexpr double offset_from_end_pose = 1.0; const auto arc_position_end = lanelet::utils::getArcCoordinates(road_lanes, end_pose); const double s_start = std::max(arc_position_end.length + offset_from_end_pose, 0.0); @@ -96,7 +96,8 @@ boost::optional FreespacePullOut::plan(const Pose start_pose, const const bool path_terminal_is_goal = path_end_info.second; const auto road_center_line_path = route_handler->getCenterLinePath(road_lanes, s_start, s_end); - last_path = start_planner_utils::combineReferencePath(last_path, road_center_line_path); + last_path = utils::resamplePathWithSpline( + utils::combinePath(last_path, road_center_line_path), parameters_.center_line_path_interval); const double original_terminal_velocity = last_path.points.back().point.longitudinal_velocity_mps; utils::correctDividedPathVelocity(partial_paths); diff --git a/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp b/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp index 45d36d51d69b4..3cf42dbb5da26 100644 --- a/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp +++ b/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp @@ -15,8 +15,10 @@ #include "behavior_path_planner/utils/start_planner/geometric_pull_out.hpp" #include "behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp" +#include "behavior_path_planner/utils/path_utils.hpp" #include "behavior_path_planner/utils/start_planner/util.hpp" #include "behavior_path_planner/utils/utils.hpp" +#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" #include @@ -26,7 +28,6 @@ using tier4_autoware_utils::calcDistance2d; using tier4_autoware_utils::calcOffsetPose; namespace behavior_path_planner { -using start_planner_utils::combineReferencePath; using start_planner_utils::getPullOutLanes; GeometricPullOut::GeometricPullOut(rclcpp::Node & node, const StartPlannerParameters & parameters) @@ -36,7 +37,7 @@ GeometricPullOut::GeometricPullOut(rclcpp::Node & node, const StartPlannerParame planner_.setParameters(parallel_parking_parameters_); } -boost::optional GeometricPullOut::plan(Pose start_pose, Pose goal_pose) +boost::optional GeometricPullOut::plan(const Pose & start_pose, const Pose & goal_pose) { PullOutPath output; @@ -47,33 +48,26 @@ boost::optional GeometricPullOut::plan(Pose start_pose, Pose goal_p planner_data_, backward_path_length, std::numeric_limits::max(), /*forward_only_in_route*/ true); const auto pull_out_lanes = getPullOutLanes(planner_data_, backward_path_length); - auto lanes = road_lanes; - for (const auto & pull_out_lane : pull_out_lanes) { - auto it = std::find_if( - lanes.begin(), lanes.end(), [&pull_out_lane](const lanelet::ConstLanelet & lane) { - return lane.id() == pull_out_lane.id(); - }); - if (it == lanes.end()) { - lanes.push_back(pull_out_lane); - } - } + + // check if the ego is at left or right side of road lane center + const bool left_side_start = 0 < getArcCoordinates(road_lanes, start_pose).distance; planner_.setTurningRadius( planner_data_->parameters, parallel_parking_parameters_.pull_out_max_steer_angle); planner_.setPlannerData(planner_data_); const bool found_valid_path = - planner_.planPullOut(start_pose, goal_pose, road_lanes, pull_out_lanes); + planner_.planPullOut(start_pose, goal_pose, road_lanes, pull_out_lanes, left_side_start); if (!found_valid_path) { return {}; } // collision check with stop objects in pull out lanes const auto arc_path = planner_.getArcPath(); - const auto [pull_out_lane_objects, others] = + const auto & stop_objects = utils::path_safety_checker::filterObjectsByVelocity( + *(planner_data_->dynamic_object), parameters_.th_moving_object_velocity); + const auto [pull_out_lane_stop_objects, others] = utils::path_safety_checker::separateObjectsByLanelets( - *(planner_data_->dynamic_object), pull_out_lanes); - const auto pull_out_lane_stop_objects = utils::path_safety_checker::filterObjectsByVelocity( - pull_out_lane_objects, parameters_.th_moving_object_velocity); + stop_objects, pull_out_lanes, utils::path_safety_checker::isPolygonOverlapLanelet); if (utils::checkCollisionBetweenPathFootprintsAndObjects( vehicle_footprint_, arc_path, pull_out_lane_stop_objects, @@ -114,7 +108,7 @@ boost::optional GeometricPullOut::plan(Pose start_pose, Pose goal_p std::make_pair(velocity, velocity * velocity / (2 * arc_length_on_second_arc_path))); } else { const auto partial_paths = planner_.getPaths(); - const auto combined_path = combineReferencePath(partial_paths.at(0), partial_paths.at(1)); + const auto combined_path = utils::combinePath(partial_paths.at(0), partial_paths.at(1)); output.partial_paths.push_back(combined_path); // Calculate the acceleration required to reach the forward parking velocity at the center of @@ -123,6 +117,7 @@ boost::optional GeometricPullOut::plan(Pose start_pose, Pose goal_p output.pairs_terminal_velocity_and_accel.push_back( std::make_pair(velocity, velocity * velocity / 2 * arc_length_on_path)); } + output.start_pose = planner_.getArcPaths().at(0).points.front().point.pose; output.end_pose = planner_.getArcPaths().at(1).points.back().point.pose; diff --git a/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp b/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp index 7352821e140be..a430e18c330e1 100644 --- a/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp +++ b/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp @@ -16,8 +16,11 @@ #include "behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp" #include "behavior_path_planner/utils/path_utils.hpp" +#include "behavior_path_planner/utils/start_goal_planner_common/utils.hpp" #include "behavior_path_planner/utils/start_planner/util.hpp" #include "behavior_path_planner/utils/utils.hpp" +#include "motion_utils/trajectory/path_with_lane_id.hpp" +#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" #include @@ -30,7 +33,6 @@ using tier4_autoware_utils::calcDistance2d; using tier4_autoware_utils::calcOffsetPose; namespace behavior_path_planner { -using start_planner_utils::combineReferencePath; using start_planner_utils::getPullOutLanes; ShiftPullOut::ShiftPullOut( @@ -40,7 +42,7 @@ ShiftPullOut::ShiftPullOut( { } -boost::optional ShiftPullOut::plan(Pose start_pose, Pose goal_pose) +boost::optional ShiftPullOut::plan(const Pose & start_pose, const Pose & goal_pose) { const auto & route_handler = planner_data_->route_handler; const auto & common_parameters = planner_data_->parameters; @@ -57,15 +59,15 @@ boost::optional ShiftPullOut::plan(Pose start_pose, Pose goal_pose) planner_data_, backward_path_length, std::numeric_limits::max(), /*forward_only_in_route*/ true); // find candidate paths - auto pull_out_paths = calcPullOutPaths( - *route_handler, road_lanes, start_pose, goal_pose, common_parameters, parameters_); + auto pull_out_paths = calcPullOutPaths(*route_handler, road_lanes, start_pose, goal_pose); if (pull_out_paths.empty()) { return boost::none; } // extract stop objects in pull out lane for collision check const auto [pull_out_lane_objects, others] = - utils::path_safety_checker::separateObjectsByLanelets(*dynamic_objects, pull_out_lanes); + utils::path_safety_checker::separateObjectsByLanelets( + *dynamic_objects, pull_out_lanes, utils::path_safety_checker::isPolygonOverlapLanelet); const auto pull_out_lane_stop_objects = utils::path_safety_checker::filterObjectsByVelocity( pull_out_lane_objects, parameters_.th_moving_object_velocity); @@ -96,17 +98,48 @@ boost::optional ShiftPullOut::plan(Pose start_pose, Pose goal_pose) shift_path.points.begin() + collision_check_end_idx + 1); } - // check lane departure + // extract shoulder lanes from pull out lanes + lanelet::ConstLanelets shoulder_lanes; + std::copy_if( + pull_out_lanes.begin(), pull_out_lanes.end(), std::back_inserter(shoulder_lanes), + [&route_handler](const auto & pull_out_lane) { + return route_handler->isShoulderLanelet(pull_out_lane); + }); const auto drivable_lanes = - utils::generateDrivableLanesWithShoulderLanes(road_lanes, pull_out_lanes); + utils::generateDrivableLanesWithShoulderLanes(road_lanes, shoulder_lanes); const auto & dp = planner_data_->drivable_area_expansion_parameters; - const auto expanded_lanes = utils::expandLanelets( + const auto expanded_lanes = utils::transformToLanelets(utils::expandLanelets( drivable_lanes, dp.drivable_area_left_bound_offset, dp.drivable_area_right_bound_offset, - dp.drivable_area_types_to_skip); + dp.drivable_area_types_to_skip)); + + // crop backward path + // removes points which are out of lanes up to the start pose. + // this ensures that the backward_path stays within the drivable area when starting from a + // narrow place. + const size_t start_segment_idx = motion_utils::findFirstNearestIndexWithSoftConstraints( + shift_path.points, start_pose, common_parameters.ego_nearest_dist_threshold, + common_parameters.ego_nearest_yaw_threshold); + PathWithLaneId cropped_path{}; + for (size_t i = 0; i < shift_path.points.size(); ++i) { + const Pose pose = shift_path.points.at(i).point.pose; + const auto transformed_vehicle_footprint = + transformVector(vehicle_footprint_, tier4_autoware_utils::pose2transform(pose)); + const bool is_out_of_lane = + LaneDepartureChecker::isOutOfLane(expanded_lanes, transformed_vehicle_footprint); + if (i <= start_segment_idx) { + if (!is_out_of_lane) { + cropped_path.points.push_back(shift_path.points.at(i)); + } + } else { + cropped_path.points.push_back(shift_path.points.at(i)); + } + } + shift_path.points = cropped_path.points; + + // check lane departure if ( parameters_.check_shift_path_lane_departure && - lane_departure_checker_->checkPathWillLeaveLane( - utils::transformToLanelets(expanded_lanes), path_start_to_end)) { + lane_departure_checker_->checkPathWillLeaveLane(expanded_lanes, path_start_to_end)) { continue; } @@ -127,8 +160,7 @@ boost::optional ShiftPullOut::plan(Pose start_pose, Pose goal_pose) std::vector ShiftPullOut::calcPullOutPaths( const RouteHandler & route_handler, const lanelet::ConstLanelets & road_lanes, - const Pose & start_pose, const Pose & goal_pose, - const BehaviorPathPlannerParameters & common_parameter, const StartPlannerParameters & parameter) + const Pose & start_pose, const Pose & goal_pose) { std::vector candidate_paths{}; @@ -137,12 +169,16 @@ std::vector ShiftPullOut::calcPullOutPaths( } // rename parameter - const double forward_path_length = common_parameter.forward_path_length; - const double backward_path_length = common_parameter.backward_path_length; - const double lateral_jerk = parameter.lateral_jerk; - const double minimum_lateral_acc = parameter.minimum_lateral_acc; - const double maximum_lateral_acc = parameter.maximum_lateral_acc; - const int lateral_acceleration_sampling_num = parameter.lateral_acceleration_sampling_num; + const auto & common_parameters = planner_data_->parameters; + const double forward_path_length = common_parameters.forward_path_length; + const double backward_path_length = common_parameters.backward_path_length; + const double lateral_jerk = parameters_.lateral_jerk; + const double minimum_lateral_acc = parameters_.minimum_lateral_acc; + const double maximum_lateral_acc = parameters_.maximum_lateral_acc; + const double maximum_curvature = parameters_.maximum_curvature; + const double minimum_shift_pull_out_distance = parameters_.minimum_shift_pull_out_distance; + const int lateral_acceleration_sampling_num = parameters_.lateral_acceleration_sampling_num; + // set minimum acc for breaking loop when sampling num is 1 const double acc_resolution = std::max( std::abs(maximum_lateral_acc - minimum_lateral_acc) / lateral_acceleration_sampling_num, @@ -156,9 +192,9 @@ std::vector ShiftPullOut::calcPullOutPaths( const double s_end = path_end_info.first; const bool path_terminal_is_goal = path_end_info.second; - constexpr double RESAMPLE_INTERVAL = 1.0; PathWithLaneId road_lane_reference_path = utils::resamplePathWithSpline( - route_handler.getCenterLinePath(road_lanes, s_start, s_end), RESAMPLE_INTERVAL); + route_handler.getCenterLinePath(road_lanes, s_start, s_end), + parameters_.center_line_path_interval); // non_shifted_path for when shift length or pull out distance is too short const PullOutPath non_shifted_path = std::invoke([&]() { @@ -197,8 +233,8 @@ std::vector ShiftPullOut::calcPullOutPaths( PathShifter::calcShiftTimeFromJerk(shift_length, lateral_jerk, lateral_acc); const double longitudinal_acc = std::clamp(road_velocity / shift_time, 0.0, /* max acc */ 1.0); const auto pull_out_distance = calcPullOutLongitudinalDistance( - longitudinal_acc, shift_time, shift_length, parameter.maximum_curvature, - parameter.minimum_shift_pull_out_distance); + longitudinal_acc, shift_time, shift_length, maximum_curvature, + minimum_shift_pull_out_distance); const double terminal_velocity = longitudinal_acc * shift_time; // clip from ego pose @@ -220,7 +256,9 @@ std::vector ShiftPullOut::calcPullOutPaths( std::max(pull_out_distance, pull_out_distance_converted); // if before_shifted_pull_out_distance is too short, shifting path fails, so add non shifted - if (before_shifted_pull_out_distance < RESAMPLE_INTERVAL && !has_non_shifted_path) { + if ( + before_shifted_pull_out_distance < parameters_.center_line_path_interval && + !has_non_shifted_path) { candidate_paths.push_back(non_shifted_path); has_non_shifted_path = true; continue; diff --git a/planning/behavior_path_planner/src/utils/start_planner/util.cpp b/planning/behavior_path_planner/src/utils/start_planner/util.cpp index 9ee0491ca017e..40b100a1bc070 100644 --- a/planning/behavior_path_planner/src/utils/start_planner/util.cpp +++ b/planning/behavior_path_planner/src/utils/start_planner/util.cpp @@ -20,6 +20,7 @@ #include "behavior_path_planner/utils/utils.hpp" #include +#include #include #include @@ -37,19 +38,6 @@ namespace behavior_path_planner::start_planner_utils { -PathWithLaneId combineReferencePath(const PathWithLaneId path1, const PathWithLaneId path2) -{ - PathWithLaneId path; - path.points.insert(path.points.end(), path1.points.begin(), path1.points.end()); - - // skip overlapping point - path.points.insert(path.points.end(), next(path2.points.begin()), path2.points.end()); - - PathWithLaneId filtered_path = path; - filtered_path.points = motion_utils::removeOverlapPoints(filtered_path.points); - return filtered_path; -} - PathWithLaneId getBackwardPath( const RouteHandler & route_handler, const lanelet::ConstLanelets & shoulder_lanes, const Pose & current_pose, const Pose & backed_pose, const double velocity) diff --git a/planning/behavior_path_planner/src/utils/utils.cpp b/planning/behavior_path_planner/src/utils/utils.cpp index eb2859ff700af..35dd7675ae3fc 100644 --- a/planning/behavior_path_planner/src/utils/utils.cpp +++ b/planning/behavior_path_planner/src/utils/utils.cpp @@ -15,16 +15,22 @@ #include "behavior_path_planner/utils/utils.hpp" #include "behavior_path_planner/utils/drivable_area_expansion/drivable_area_expansion.hpp" +#include "behavior_path_planner/utils/path_shifter/path_shifter.hpp" #include "motion_utils/trajectory/path_with_lane_id.hpp" +#include "object_recognition_utils/predicted_path_utils.hpp" #include #include #include +#include +#include +#include -#include "autoware_auto_perception_msgs/msg/predicted_object.hpp" -#include "autoware_auto_perception_msgs/msg/predicted_path.hpp" +#include -#include +#include +#include +#include #include #include @@ -1532,44 +1538,23 @@ void generateDrivableArea( } const auto & expansion_params = planner_data->drivable_area_expansion_parameters; if (expansion_params.enabled) { - drivable_area_expansion::expandDrivableArea( - path, expansion_params, *planner_data->dynamic_object, *planner_data->route_handler, - transformed_lanes); + drivable_area_expansion::expand_drivable_area(path, planner_data); } // make bound longitudinally monotonic // TODO(Murooka) Fix makeBoundLongitudinallyMonotonic - if (enable_expanding_hatched_road_markings || enable_expanding_intersection_areas) { + if ( + is_driving_forward && + (enable_expanding_hatched_road_markings || enable_expanding_intersection_areas)) { makeBoundLongitudinallyMonotonic(path, planner_data, true); // for left bound makeBoundLongitudinallyMonotonic(path, planner_data, false); // for right bound } } -// calculate bounds from drivable lanes and hatched road markings -std::vector calcBound( - const std::shared_ptr route_handler, - const std::vector & drivable_lanes, - const bool enable_expanding_hatched_road_markings, const bool enable_expanding_intersection_areas, - const bool is_left) +std::vector getBoundWithHatchedRoadMarkings( + const std::vector & original_bound, + const std::shared_ptr & route_handler) { - // a function to convert drivable lanes to points without duplicated points - const auto convert_to_points = [&](const std::vector & drivable_lanes) { - constexpr double overlap_threshold = 0.01; - - std::vector points; - for (const auto & drivable_lane : drivable_lanes) { - const auto bound = - is_left ? drivable_lane.left_lane.leftBound3d() : drivable_lane.right_lane.rightBound3d(); - for (const auto & point : bound) { - if ( - points.empty() || - overlap_threshold < (points.back().basicPoint2d() - point.basicPoint2d()).norm()) { - points.push_back(point); - } - } - } - return points; - }; // a function to get polygon with a designated point id const auto get_corresponding_polygon_index = [&](const auto & polygon, const auto & target_point_id) { @@ -1582,18 +1567,89 @@ std::vector calcBound( // This means calculation has some errors. return polygon.size() - 1; }; + const auto mod = [&](const int a, const int b) { return (a + b) % b; // NOTE: consider negative value }; + + std::vector expanded_bound{}; + + std::optional current_polygon{std::nullopt}; + std::vector current_polygon_border_indices; + // expand drivable area by hatched road markings. + for (size_t bound_point_idx = 0; bound_point_idx < original_bound.size(); ++bound_point_idx) { + const auto & bound_point = original_bound[bound_point_idx]; + const auto polygon = getPolygonByPoint(route_handler, bound_point, "hatched_road_markings"); + + bool will_close_polygon{false}; + if (!current_polygon) { + if (!polygon) { + expanded_bound.push_back(bound_point); + } else { + // There is a new additional polygon to expand + current_polygon = polygon; + current_polygon_border_indices.push_back( + get_corresponding_polygon_index(*current_polygon, bound_point.id())); + } + } else { + if (!polygon) { + will_close_polygon = true; + } else { + current_polygon_border_indices.push_back( + get_corresponding_polygon_index(*current_polygon, bound_point.id())); + } + } + + if (bound_point_idx == original_bound.size() - 1 && current_polygon) { + // If drivable lanes ends earlier than polygon, close the polygon + will_close_polygon = true; + } + + if (will_close_polygon) { + // The current additional polygon ends to expand + if (current_polygon_border_indices.size() == 1) { + expanded_bound.push_back((*current_polygon)[current_polygon_border_indices.front()]); + } else { + const size_t current_polygon_points_num = current_polygon->size(); + const bool is_polygon_opposite_direction = [&]() { + const size_t modulo_diff = mod( + static_cast(current_polygon_border_indices[1]) - + static_cast(current_polygon_border_indices[0]), + current_polygon_points_num); + return modulo_diff == 1; + }(); + + const int target_points_num = + current_polygon_points_num - current_polygon_border_indices.size() + 1; + for (int poly_idx = 0; poly_idx <= target_points_num; ++poly_idx) { + const int target_poly_idx = current_polygon_border_indices.front() + + poly_idx * (is_polygon_opposite_direction ? -1 : 1); + expanded_bound.push_back( + (*current_polygon)[mod(target_poly_idx, current_polygon_points_num)]); + } + } + current_polygon = std::nullopt; + current_polygon_border_indices.clear(); + } + } + + return expanded_bound; +} + +std::vector getBoundWithIntersectionAreas( + const std::vector & original_bound, + const std::shared_ptr & route_handler, + const std::vector & drivable_lanes, const bool is_left) +{ const auto extract_bound_from_polygon = [](const auto & polygon, const auto start_idx, const auto end_idx, const auto clockwise) { - std::vector ret; + std::vector ret{}; for (size_t i = start_idx; i != end_idx; i = clockwise ? i + 1 : i - 1) { - ret.push_back(lanelet::utils::conversion::toGeomMsgPt(polygon[i])); + ret.push_back(polygon[i]); if (i + 1 == polygon.size() && clockwise) { if (end_idx == 0) { - ret.push_back(lanelet::utils::conversion::toGeomMsgPt(polygon[end_idx])); + ret.push_back(polygon[end_idx]); return ret; } i = 0; @@ -1602,7 +1658,7 @@ std::vector calcBound( if (i == 0 && !clockwise) { if (end_idx == polygon.size() - 1) { - ret.push_back(lanelet::utils::conversion::toGeomMsgPt(polygon[end_idx])); + ret.push_back(polygon[end_idx]); return ret; } i = polygon.size() - 1; @@ -1610,34 +1666,30 @@ std::vector calcBound( } } + ret.push_back(polygon[end_idx]); + return ret; }; - // If no need to expand with polygons, return here. - std::vector output_points; - const auto bound_points = convert_to_points(drivable_lanes); - if (!enable_expanding_hatched_road_markings && !enable_expanding_intersection_areas) { - for (const auto & point : bound_points) { - output_points.push_back(lanelet::utils::conversion::toGeomMsgPt(point)); - } - return motion_utils::removeOverlapPoints(output_points); - } + std::vector expanded_bound = original_bound; - std::optional current_polygon{std::nullopt}; - std::vector current_polygon_border_indices; + // expand drivable area by using intersection area. for (const auto & drivable_lane : drivable_lanes) { - // extract target lane and bound. - const auto bound_lane = is_left ? drivable_lane.left_lane : drivable_lane.right_lane; - const auto bound = is_left ? bound_lane.leftBound3d() : bound_lane.rightBound3d(); + const auto edge_lanelet = is_left ? drivable_lane.left_lane : drivable_lane.right_lane; + const auto lanelet_bound = is_left ? edge_lanelet.leftBound3d() : edge_lanelet.rightBound3d(); - if (bound.size() < 2) { + if (lanelet_bound.size() < 2) { continue; } - // expand drivable area by intersection areas. - const std::string id = bound_lane.attributeOr("intersection_area", "else"); - const auto use_intersection_area = enable_expanding_intersection_areas && id != "else"; - if (use_intersection_area) { + const std::string id = edge_lanelet.attributeOr("intersection_area", "else"); + if (id == "else") { + continue; + } + + // Step1. extract intersection partial bound. + std::vector intersection_bound{}; + { const auto polygon = route_handler->getLaneletMapPtr()->polygonLayer.get(std::atoi(id.c_str())); @@ -1645,89 +1697,144 @@ std::vector calcBound( boost::geometry::is_valid(lanelet::utils::to2D(polygon.basicPolygon())); const auto is_clockwise_iteration = is_clockwise_polygon ? is_left : !is_left; - const auto start_itr = std::find_if(polygon.begin(), polygon.end(), [&bound](const auto & p) { - return p.id() == bound.front().id(); - }); + const auto intersection_bound_itr_init = std::find_if( + polygon.begin(), polygon.end(), + [&lanelet_bound](const auto & p) { return p.id() == lanelet_bound.front().id(); }); - const auto end_itr = std::find_if(polygon.begin(), polygon.end(), [&bound](const auto & p) { - return p.id() == bound.back().id(); - }); + const auto intersection_bound_itr_last = std::find_if( + polygon.begin(), polygon.end(), + [&lanelet_bound](const auto & p) { return p.id() == lanelet_bound.back().id(); }); - if (start_itr == polygon.end() || end_itr == polygon.end()) { + if ( + intersection_bound_itr_init == polygon.end() || + intersection_bound_itr_last == polygon.end()) { continue; } // extract line strings between start_idx and end_idx. - const size_t start_idx = std::distance(polygon.begin(), start_itr); - const size_t end_idx = std::distance(polygon.begin(), end_itr); - for (const auto & point : - extract_bound_from_polygon(polygon, start_idx, end_idx, is_clockwise_iteration)) { - output_points.push_back(point); - } + const size_t start_idx = std::distance(polygon.begin(), intersection_bound_itr_init); + const size_t end_idx = std::distance(polygon.begin(), intersection_bound_itr_last); + + intersection_bound = + extract_bound_from_polygon(polygon, start_idx, end_idx, is_clockwise_iteration); + } + + // Step2. check shared bound point. + const auto shared_point_itr_init = + std::find_if(expanded_bound.begin(), expanded_bound.end(), [&](const auto & p) { + return std::any_of( + intersection_bound.begin(), intersection_bound.end(), + [&](const auto & point) { return point.id() == p.id(); }); + }); + + const auto shared_point_itr_last = + std::find_if(expanded_bound.rbegin(), expanded_bound.rend(), [&](const auto & p) { + return std::any_of( + intersection_bound.rbegin(), intersection_bound.rend(), + [&](const auto & point) { return point.id() == p.id(); }); + }); + if ( + shared_point_itr_init == expanded_bound.end() || + shared_point_itr_last == expanded_bound.rend()) { continue; } - // expand drivable area by hatched road markings. - for (size_t bound_point_idx = 0; bound_point_idx < bound.size(); ++bound_point_idx) { - const auto & bound_point = bound[bound_point_idx]; - const auto polygon = getPolygonByPoint(route_handler, bound_point, "hatched_road_markings"); - - bool will_close_polygon{false}; - if (!current_polygon) { - if (!polygon) { - output_points.push_back(lanelet::utils::conversion::toGeomMsgPt(bound_point)); - } else { - // There is a new additional polygon to expand - current_polygon = polygon; - current_polygon_border_indices.push_back( - get_corresponding_polygon_index(*current_polygon, bound_point.id())); - } - } else { - if (!polygon) { - will_close_polygon = true; - } else { - current_polygon_border_indices.push_back( - get_corresponding_polygon_index(*current_polygon, bound_point.id())); - } + // Step3. overwrite duplicate drivable bound by intersection bound. + { + const auto trim_point_itr_init = std::find_if( + intersection_bound.begin(), intersection_bound.end(), + [&](const auto & p) { return p.id() == shared_point_itr_init->id(); }); + + const auto trim_point_itr_last = std::find_if( + intersection_bound.begin(), intersection_bound.end(), + [&](const auto & p) { return p.id() == shared_point_itr_last->id(); }); + + if ( + trim_point_itr_init == intersection_bound.end() || + trim_point_itr_last == intersection_bound.end()) { + continue; } - if (bound_point_idx == bound_points.size() - 1 && current_polygon) { - // If drivable lanes ends earlier than polygon, close the polygon - will_close_polygon = true; + // TODO(Satoshi OTA): remove this guard. + if ( + std::distance(intersection_bound.begin(), trim_point_itr_last) < + std::distance(intersection_bound.begin(), trim_point_itr_init)) { + continue; } - if (will_close_polygon) { - // The current additional polygon ends to expand - if (current_polygon_border_indices.size() == 1) { - output_points.push_back(lanelet::utils::conversion::toGeomMsgPt( - (*current_polygon)[current_polygon_border_indices.front()])); - } else { - const size_t current_polygon_points_num = current_polygon->size(); - const bool is_polygon_opposite_direction = [&]() { - const size_t modulo_diff = mod( - static_cast(current_polygon_border_indices[1]) - - static_cast(current_polygon_border_indices[0]), - current_polygon_points_num); - return modulo_diff == 1; - }(); + std::vector tmp_bound{}; - const int target_points_num = - current_polygon_points_num - current_polygon_border_indices.size() + 1; - for (int poly_idx = 0; poly_idx <= target_points_num; ++poly_idx) { - const int target_poly_idx = current_polygon_border_indices.front() + - poly_idx * (is_polygon_opposite_direction ? -1 : 1); - output_points.push_back(lanelet::utils::conversion::toGeomMsgPt( - (*current_polygon)[mod(target_poly_idx, current_polygon_points_num)])); - } + tmp_bound.insert(tmp_bound.end(), expanded_bound.begin(), shared_point_itr_init); + tmp_bound.insert(tmp_bound.end(), trim_point_itr_init, trim_point_itr_last); + tmp_bound.insert( + tmp_bound.end(), std::next(shared_point_itr_last).base(), expanded_bound.end()); + + expanded_bound = tmp_bound; + } + } + + return expanded_bound; +} + +// calculate bounds from drivable lanes and hatched road markings +std::vector calcBound( + const std::shared_ptr route_handler, + const std::vector & drivable_lanes, + const bool enable_expanding_hatched_road_markings, const bool enable_expanding_intersection_areas, + const bool is_left) +{ + // a function to convert drivable lanes to points without duplicated points + const auto convert_to_points = [&](const std::vector & drivable_lanes) { + constexpr double overlap_threshold = 0.01; + + std::vector points; + for (const auto & drivable_lane : drivable_lanes) { + const auto bound = + is_left ? drivable_lane.left_lane.leftBound3d() : drivable_lane.right_lane.rightBound3d(); + for (const auto & point : bound) { + if ( + points.empty() || + overlap_threshold < (points.back().basicPoint2d() - point.basicPoint2d()).norm()) { + points.push_back(point); } - current_polygon = std::nullopt; - current_polygon_border_indices.clear(); } } + return points; + }; + + const auto to_ros_point = [](const std::vector & bound) { + std::vector ret{}; + std::for_each(bound.begin(), bound.end(), [&](const auto & p) { + ret.push_back(lanelet::utils::conversion::toGeomMsgPt(p)); + }); + return ret; + }; + + // Step1. create drivable bound from drivable lanes. + std::vector bound_points = convert_to_points(drivable_lanes); + + // Step2. if there is no drivable area defined by polygon, return original drivable bound. + if (!enable_expanding_hatched_road_markings && !enable_expanding_intersection_areas) { + return motion_utils::removeOverlapPoints(to_ros_point(bound_points)); + } + + // Step3. if there are hatched road markings, expand drivable bound with the polygon. + if (enable_expanding_hatched_road_markings) { + bound_points = getBoundWithHatchedRoadMarkings(bound_points, route_handler); + } + + if (!enable_expanding_intersection_areas) { + return motion_utils::removeOverlapPoints(to_ros_point(bound_points)); + } + + // Step4. if there are intersection areas, expand drivable bound with the polygon. + { + bound_points = + getBoundWithIntersectionAreas(bound_points, route_handler, drivable_lanes, is_left); } - return motion_utils::removeOverlapPoints(output_points); + return motion_utils::removeOverlapPoints(to_ros_point(bound_points)); } void makeBoundLongitudinallyMonotonic( @@ -1869,6 +1976,7 @@ void makeBoundLongitudinallyMonotonic( if (intersect_point) { Pose pose; pose.position = *intersect_point; + pose.position.z = bound_with_pose.at(i).position.z; const auto yaw = calcAzimuthAngle(*intersect_point, bound_with_pose.at(i + 1).position); pose.orientation = createQuaternionFromRPY(0.0, 0.0, yaw); monotonic_bound.push_back(pose); @@ -1900,20 +2008,42 @@ void makeBoundLongitudinallyMonotonic( return bound; } - std::vector ret; + std::vector ret = bound; + auto itr = std::next(ret.begin()); + while (std::next(itr) != ret.end()) { + if (itr == ret.begin()) { + itr++; + continue; + } + + const auto p1 = *std::prev(itr); + const auto p2 = *itr; + const auto p3 = *std::next(itr); - ret.push_back(bound.front()); + const std::vector vec_1to2 = {p2.x - p1.x, p2.y - p1.y, p2.z - p1.z}; + const std::vector vec_3to2 = {p2.x - p3.x, p2.y - p3.y, p2.z - p3.z}; + const auto product = + std::inner_product(vec_1to2.begin(), vec_1to2.end(), vec_3to2.begin(), 0.0); - for (size_t i = 0; i < bound.size() - 2; i++) { - try { - motion_utils::validateNonSharpAngle(bound.at(i), bound.at(i + 1), bound.at(i + 2)); - ret.push_back(bound.at(i + 1)); - } catch (const std::exception & e) { + const auto dist_1to2 = tier4_autoware_utils::calcDistance3d(p1, p2); + const auto dist_3to2 = tier4_autoware_utils::calcDistance3d(p3, p2); + + constexpr double epsilon = 1e-3; + + // Remove overlapped point. + if (dist_1to2 < epsilon || dist_3to2 < epsilon) { + itr = std::prev(ret.erase(itr)); continue; } - } - ret.push_back(bound.back()); + // If the angle between the points is sharper than 45 degrees, remove the middle point. + if (std::cos(M_PI_4) < product / dist_1to2 / dist_3to2 + epsilon) { + itr = std::prev(ret.erase(itr)); + continue; + } + + itr++; + } return ret; }; @@ -2406,64 +2536,128 @@ double getSignedDistanceFromBoundary( } std::optional getSignedDistanceFromBoundary( - const lanelet::ConstLanelets & lanelets, const LinearRing2d & footprint, - const Pose & vehicle_pose, const bool left_side) -{ - double min_distance = std::numeric_limits::max(); - - const auto transformed_footprint = - transformVector(footprint, tier4_autoware_utils::pose2transform(vehicle_pose)); - bool found_neighbor_bound = false; - for (const auto & vehicle_corner_point : transformed_footprint) { - // convert point of footprint to pose + const lanelet::ConstLanelets & lanelets, const double vehicle_width, const double base_link2front, + const double base_link2rear, const Pose & vehicle_pose, const bool left_side) +{ + // Depending on which side is selected, calculate the transformed coordinates of the front and + // rear vehicle corners + Point rear_corner_point, front_corner_point; + if (left_side) { + Point front_left, rear_left; + rear_left.x = -base_link2rear; + rear_left.y = vehicle_width / 2; + front_left.x = base_link2front; + front_left.y = vehicle_width / 2; + rear_corner_point = tier4_autoware_utils::transformPoint(rear_left, vehicle_pose); + front_corner_point = tier4_autoware_utils::transformPoint(front_left, vehicle_pose); + } else { + Point front_right, rear_right; + rear_right.x = -base_link2rear; + rear_right.y = -vehicle_width / 2; + front_right.x = base_link2front; + front_right.y = -vehicle_width / 2; + rear_corner_point = tier4_autoware_utils::transformPoint(rear_right, vehicle_pose); + front_corner_point = tier4_autoware_utils::transformPoint(front_right, vehicle_pose); + } + + const auto combined_lane = lanelet::utils::combineLaneletsShape(lanelets); + const auto & bound_line_2d = left_side ? lanelet::utils::to2D(combined_lane.leftBound3d()) + : lanelet::utils::to2D(combined_lane.rightBound3d()); + + // Find the closest bound segment that contains the corner point in the X-direction + // and calculate the lateral distance from that segment. + const auto calcLateralDistanceFromBound = + [&](const Point & vehicle_corner_point) -> boost::optional> { Pose vehicle_corner_pose{}; - vehicle_corner_pose.position = tier4_autoware_utils::toMsg(vehicle_corner_point.to_3d()); + vehicle_corner_pose.position = vehicle_corner_point; vehicle_corner_pose.orientation = vehicle_pose.orientation; - // calculate distance to the bound directly next to footprint points - lanelet::ConstLanelet closest_lanelet{}; - if (lanelet::utils::query::getClosestLanelet(lanelets, vehicle_corner_pose, &closest_lanelet)) { - const auto & bound_line_2d = left_side ? lanelet::utils::to2D(closest_lanelet.leftBound3d()) - : lanelet::utils::to2D(closest_lanelet.rightBound3d()); - - for (size_t i = 1; i < bound_line_2d.size(); ++i) { - const Point p_front = lanelet::utils::conversion::toGeomMsgPt(bound_line_2d[i - 1]); - const Point p_back = lanelet::utils::conversion::toGeomMsgPt(bound_line_2d[i]); - - const Point inverse_p_front = - tier4_autoware_utils::inverseTransformPoint(p_front, vehicle_corner_pose); - const Point inverse_p_back = - tier4_autoware_utils::inverseTransformPoint(p_back, vehicle_corner_pose); - - const double dy_front = inverse_p_front.y; - const double dy_back = inverse_p_back.y; - const double dx_front = inverse_p_front.x; - const double dx_back = inverse_p_back.x; - // is in segment - if (dx_front < 0 && dx_back > 0) { - const double lateral_distance_from_pose_to_segment = - (dy_front * dx_back + dy_back * -dx_front) / (dx_back - dx_front); - - if (std::abs(lateral_distance_from_pose_to_segment) < std::abs(min_distance)) { - min_distance = lateral_distance_from_pose_to_segment; - } - - found_neighbor_bound = true; - break; - } + boost::optional> lateral_distance_with_idx{}; + + // Euclidean distance to find the closest segment containing the corner point. + double min_distance = std::numeric_limits::max(); + + for (size_t i = 0; i < bound_line_2d.size() - 1; i++) { + const Point p1 = lanelet::utils::conversion::toGeomMsgPt(bound_line_2d[i]); + const Point p2 = lanelet::utils::conversion::toGeomMsgPt(bound_line_2d[i + 1]); + + const Point inverse_p1 = tier4_autoware_utils::inverseTransformPoint(p1, vehicle_corner_pose); + const Point inverse_p2 = tier4_autoware_utils::inverseTransformPoint(p2, vehicle_corner_pose); + const double dx_p1 = inverse_p1.x; + const double dx_p2 = inverse_p2.x; + const double dy_p1 = inverse_p1.y; + const double dy_p2 = inverse_p2.y; + + // Calculate the Euclidean distances between vehicle's corner and the current and next points. + const double distance1 = tier4_autoware_utils::calcDistance2d(p1, vehicle_corner_point); + const double distance2 = tier4_autoware_utils::calcDistance2d(p2, vehicle_corner_point); + + // If one of the bound points is behind and the other is in front of the vehicle corner point + // and any of these points is closer than the current minimum distance, + // then update minimum distance, lateral distance and the segment index. + if (dx_p1 < 0 && dx_p2 > 0 && (distance1 < min_distance || distance2 < min_distance)) { + min_distance = std::min(distance1, distance2); + // Update lateral distance using the formula derived from similar triangles in the lateral + // cross-section view. + lateral_distance_with_idx = + std::make_pair(-1.0 * (dy_p1 * dx_p2 + dy_p2 * -dx_p1) / (dx_p2 - dx_p1), i); } } - } + if (lateral_distance_with_idx) { + return lateral_distance_with_idx; + } + return boost::optional>{}; + }; - if (!found_neighbor_bound) { - RCLCPP_ERROR_STREAM( - rclcpp::get_logger("behavior_path_planner").get_child("utils"), - "neighbor shoulder bound to footprint is not found."); + // Calculate the lateral distance for both the rear and front corners of the vehicle. + const boost::optional> rear_lateral_distance_with_idx = + calcLateralDistanceFromBound(rear_corner_point); + const boost::optional> front_lateral_distance_with_idx = + calcLateralDistanceFromBound(front_corner_point); + + // If no closest bound segment was found for both corners, return an empty optional. + if (!rear_lateral_distance_with_idx && !front_lateral_distance_with_idx) { return {}; } - - // min_distance is the distance from corner_pose to bound, so reverse this value - return -min_distance; + // If only one of them found the closest bound, return the found lateral distance. + if (!rear_lateral_distance_with_idx) { + return front_lateral_distance_with_idx.get().first; + } else if (!front_lateral_distance_with_idx) { + return rear_lateral_distance_with_idx.get().first; + } + // If both corners found their closest bound, return the maximum (for left side) or the minimum + // (for right side) lateral distance. + double lateral_distance = + left_side + ? std::max( + rear_lateral_distance_with_idx.get().first, front_lateral_distance_with_idx.get().first) + : std::min( + rear_lateral_distance_with_idx.get().first, front_lateral_distance_with_idx.get().first); + + // Iterate through all segments between the segments closest to the rear and front corners. + // Update the lateral distance in case any of these inner segments are closer to the vehicle. + for (size_t i = rear_lateral_distance_with_idx.get().second + 1; + i < front_lateral_distance_with_idx.get().second; i++) { + Pose bound_pose; + bound_pose.position = lanelet::utils::conversion::toGeomMsgPt(bound_line_2d[i]); + bound_pose.orientation = vehicle_pose.orientation; + + const Point inverse_rear_point = + tier4_autoware_utils::inverseTransformPoint(rear_corner_point, bound_pose); + const Point inverse_front_point = + tier4_autoware_utils::inverseTransformPoint(front_corner_point, bound_pose); + const double dx_rear = inverse_rear_point.x; + const double dx_front = inverse_front_point.x; + const double dy_rear = inverse_rear_point.y; + const double dy_front = inverse_front_point.y; + + const double current_lateral_distance = + (dy_rear * dx_front + dy_front * -dx_rear) / (dx_front - dx_rear); + lateral_distance = left_side ? std::max(lateral_distance, current_lateral_distance) + : std::min(lateral_distance, current_lateral_distance); + } + + return lateral_distance; } double getArcLengthToTargetLanelet( @@ -2501,6 +2695,19 @@ double getArcLengthToTargetLanelet( std::min(arc_front.length - arc_pose.length, arc_back.length - arc_pose.length), 0.0); } +Polygon2d toPolygon2d(const lanelet::ConstLanelet & lanelet) +{ + Polygon2d polygon; + for (const auto & p : lanelet.polygon2d().basicPolygon()) { + polygon.outer().emplace_back(p.x(), p.y()); + } + polygon.outer().push_back(polygon.outer().front()); + + return tier4_autoware_utils::isClockwise(polygon) + ? polygon + : tier4_autoware_utils::inverseClockwise(polygon); +} + std::vector getTargetLaneletPolygons( const lanelet::PolygonLayer & map_polygons, lanelet::ConstLanelets & lanelets, const Pose & pose, const double check_length, const std::string & target_type) @@ -2787,7 +2994,7 @@ BehaviorModuleOutput getReferencePath( // clip backward length // NOTE: In order to keep backward_path_length at least, resampling interval is added to the // backward. - const size_t current_seg_idx = motion_utils::findFirstNearestIndexWithSoftConstraints( + const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( reference_path.points, no_shift_pose, p.ego_nearest_dist_threshold, p.ego_nearest_yaw_threshold); reference_path.points = motion_utils::cropPoints( @@ -2874,9 +3081,24 @@ lanelet::ConstLanelets getCurrentLanesFromPath( lanelet::ConstLanelet current_lane; lanelet::utils::query::getClosestLanelet(reference_lanes, current_pose, ¤t_lane); - - return route_handler->getLaneletSequence( + auto current_lanes = route_handler->getLaneletSequence( current_lane, current_pose, p.backward_path_length, p.forward_path_length); + + // Extend the 'current_lanes' with previous lanes until it contains 'front_lane_ids'. + const auto front_lane_ids = path.points.front().lane_ids; + auto have_front_lanes = [front_lane_ids](const auto & lanes) { + return std::any_of(lanes.begin(), lanes.end(), [&](const auto & lane) { + return std::find(front_lane_ids.begin(), front_lane_ids.end(), lane.id()) != + front_lane_ids.end(); + }); + }; + while (!have_front_lanes(current_lanes)) { + const auto extended_lanes = extendPrevLane(route_handler, current_lanes); + if (extended_lanes.size() == current_lanes.size()) break; + current_lanes = extended_lanes; + } + + return current_lanes; } lanelet::ConstLanelets extendNextLane( @@ -3077,15 +3299,12 @@ double calcMinimumLaneChangeLength( const double & max_lateral_acc = lat_acc.second; const double & lateral_jerk = common_param.lane_changing_lateral_jerk; const double & finish_judge_buffer = common_param.lane_change_finish_judge_buffer; - const auto prepare_length = 0.5 * common_param.max_acc * - common_param.lane_change_prepare_duration * - common_param.lane_change_prepare_duration; double accumulated_length = length_to_intersection; for (const auto & shift_interval : shift_intervals) { const double t = PathShifter::calcShiftTimeFromJerk(shift_interval, lateral_jerk, max_lateral_acc); - accumulated_length += vel * t + prepare_length + finish_judge_buffer; + accumulated_length += vel * t + finish_judge_buffer; } accumulated_length += common_param.backward_length_buffer_for_end_of_lane * (shift_intervals.size() - 1.0); diff --git a/planning/behavior_path_planner/test/input.hpp b/planning/behavior_path_planner/test/input.hpp index de167bc8b4bcb..ededb32f78be0 100644 --- a/planning/behavior_path_planner/test/input.hpp +++ b/planning/behavior_path_planner/test/input.hpp @@ -16,9 +16,11 @@ #define INPUT_HPP_ #endif // INPUT_HPP_ -#include "behavior_path_planner/utils/utils.hpp" #include "autoware_auto_planning_msgs/msg/path_point.hpp" +#include "autoware_auto_planning_msgs/msg/path_point_with_lane_id.hpp" +#include "autoware_auto_planning_msgs/msg/path_with_lane_id.hpp" +#include "geometry_msgs/msg/pose.hpp" #include "geometry_msgs/msg/twist.hpp" #include @@ -26,6 +28,9 @@ namespace behavior_path_planner { using autoware_auto_planning_msgs::msg::PathPoint; +using autoware_auto_planning_msgs::msg::PathPointWithLaneId; +using autoware_auto_planning_msgs::msg::PathWithLaneId; +using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; PathWithLaneId generateStraightSamplePathWithLaneId( float initial_pose_value, float pose_increment, size_t point_sample); diff --git a/planning/behavior_path_planner/test/test_drivable_area_expansion.cpp b/planning/behavior_path_planner/test/test_drivable_area_expansion.cpp index 605b3b96ee9b9..99cd5cac2b9d9 100644 --- a/planning/behavior_path_planner/test/test_drivable_area_expansion.cpp +++ b/planning/behavior_path_planner/test/test_drivable_area_expansion.cpp @@ -12,17 +12,18 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "behavior_path_planner/data_manager.hpp" #include "behavior_path_planner/utils/drivable_area_expansion/drivable_area_expansion.hpp" -#include "behavior_path_planner/utils/drivable_area_expansion/expansion.hpp" #include "behavior_path_planner/utils/drivable_area_expansion/path_projection.hpp" #include "behavior_path_planner/utils/drivable_area_expansion/types.hpp" #include "lanelet2_extension/utility/message_conversion.hpp" #include +#include -using drivable_area_expansion::linestring_t; -using drivable_area_expansion::point_t; -using drivable_area_expansion::segment_t; +using drivable_area_expansion::LineString2d; +using drivable_area_expansion::Point2d; +using drivable_area_expansion::Segment2d; constexpr auto eps = 1e-9; TEST(DrivableAreaExpansionProjection, PointToSegment) @@ -30,56 +31,56 @@ TEST(DrivableAreaExpansionProjection, PointToSegment) using drivable_area_expansion::point_to_segment_projection; { - point_t query(1.0, 1.0); - segment_t segment(point_t(0.0, 0.0), point_t(10.0, 0.0)); + Point2d query(1.0, 1.0); + Segment2d segment(Point2d(0.0, 0.0), Point2d(10.0, 0.0)); const auto projection = point_to_segment_projection(query, segment.first, segment.second); EXPECT_NEAR(projection.distance, 1.0, eps); EXPECT_NEAR(projection.point.x(), 1.0, eps); EXPECT_NEAR(projection.point.y(), 0.0, eps); } { - point_t query(-1.0, 1.0); - segment_t segment(point_t(0.0, 0.0), point_t(10.0, 0.0)); + Point2d query(-1.0, 1.0); + Segment2d segment(Point2d(0.0, 0.0), Point2d(10.0, 0.0)); const auto projection = point_to_segment_projection(query, segment.first, segment.second); EXPECT_NEAR(projection.distance, std::sqrt(2), eps); EXPECT_NEAR(projection.point.x(), 0.0, eps); EXPECT_NEAR(projection.point.y(), 0.0, eps); } { - point_t query(11.0, 1.0); - segment_t segment(point_t(0.0, 0.0), point_t(10.0, 0.0)); + Point2d query(11.0, 1.0); + Segment2d segment(Point2d(0.0, 0.0), Point2d(10.0, 0.0)); const auto projection = point_to_segment_projection(query, segment.first, segment.second); EXPECT_NEAR(projection.distance, std::sqrt(2), eps); EXPECT_NEAR(projection.point.x(), 10.0, eps); EXPECT_NEAR(projection.point.y(), 0.0, eps); } { - point_t query(5.0, -5.0); - segment_t segment(point_t(0.0, 0.0), point_t(10.0, 0.0)); + Point2d query(5.0, -5.0); + Segment2d segment(Point2d(0.0, 0.0), Point2d(10.0, 0.0)); const auto projection = point_to_segment_projection(query, segment.first, segment.second); EXPECT_NEAR(projection.distance, -5.0, eps); EXPECT_NEAR(projection.point.x(), 5.0, eps); EXPECT_NEAR(projection.point.y(), 0.0, eps); } { - point_t query(5.0, -5.0); - segment_t segment(point_t(0.0, 0.0), point_t(0.0, -10.0)); + Point2d query(5.0, -5.0); + Segment2d segment(Point2d(0.0, 0.0), Point2d(0.0, -10.0)); const auto projection = point_to_segment_projection(query, segment.first, segment.second); EXPECT_NEAR(projection.distance, 5.0, eps); EXPECT_NEAR(projection.point.x(), 0.0, eps); EXPECT_NEAR(projection.point.y(), -5.0, eps); } { - point_t query(5.0, 5.0); - segment_t segment(point_t(2.5, 7.5), point_t(7.5, 2.5)); + Point2d query(5.0, 5.0); + Segment2d segment(Point2d(2.5, 7.5), Point2d(7.5, 2.5)); const auto projection = point_to_segment_projection(query, segment.first, segment.second); EXPECT_NEAR(projection.distance, 0.0, eps); EXPECT_NEAR(projection.point.x(), 5.0, eps); EXPECT_NEAR(projection.point.y(), 5.0, eps); } { - point_t query(0.0, 0.0); - segment_t segment(point_t(2.5, 7.5), point_t(7.5, 2.5)); + Point2d query(0.0, 0.0); + Segment2d segment(Point2d(2.5, 7.5), Point2d(7.5, 2.5)); const auto projection = point_to_segment_projection(query, segment.first, segment.second); EXPECT_NEAR(projection.distance, -std::sqrt(50), eps); EXPECT_NEAR(projection.point.x(), 5.0, eps); @@ -91,11 +92,11 @@ TEST(DrivableAreaExpansionProjection, PointToLinestring) { using drivable_area_expansion::point_to_linestring_projection; - linestring_t ls = { - point_t(0.0, 0.0), point_t(10.0, 0.0), point_t(10.0, 10.0), point_t(0.0, 10.0), - point_t(5.0, 5.0)}; + LineString2d ls = { + Point2d(0.0, 0.0), Point2d(10.0, 0.0), Point2d(10.0, 10.0), Point2d(0.0, 10.0), + Point2d(5.0, 5.0)}; { - point_t query(0.0, 0.0); + Point2d query(0.0, 0.0); const auto projection = point_to_linestring_projection(query, ls); EXPECT_NEAR(projection.arc_length, 0.0, eps); EXPECT_NEAR(projection.distance, 0.0, eps); @@ -103,7 +104,7 @@ TEST(DrivableAreaExpansionProjection, PointToLinestring) EXPECT_NEAR(projection.projected_point.y(), 0.0, eps); } { - point_t query(2.0, 1.0); + Point2d query(2.0, 1.0); const auto projection = point_to_linestring_projection(query, ls); EXPECT_NEAR(projection.arc_length, 2.0, eps); EXPECT_NEAR(projection.distance, 1.0, eps); @@ -111,7 +112,7 @@ TEST(DrivableAreaExpansionProjection, PointToLinestring) EXPECT_NEAR(projection.projected_point.y(), 0.0, eps); } { - point_t query(0.0, 5.0); + Point2d query(0.0, 5.0); const auto projection = point_to_linestring_projection(query, ls); EXPECT_NEAR(projection.arc_length, 30.0 + std::sqrt(2.5 * 2.5 * 2), eps); EXPECT_NEAR(projection.distance, -std::sqrt(2.5 * 2.5 * 2), eps); @@ -124,9 +125,9 @@ TEST(DrivableAreaExpansionProjection, LinestringToPoint) { using drivable_area_expansion::linestring_to_point_projection; - linestring_t ls = { - point_t(0.0, 0.0), point_t(10.0, 0.0), point_t(10.0, 10.0), point_t(0.0, 10.0), - point_t(5.0, 5.0)}; + LineString2d ls = { + Point2d(0.0, 0.0), Point2d(10.0, 0.0), Point2d(10.0, 10.0), Point2d(0.0, 10.0), + Point2d(5.0, 5.0)}; for (auto arc_length = 0.0; arc_length <= 10.0; arc_length += 1.0) { const auto projection = linestring_to_point_projection(ls, arc_length, 0.0); EXPECT_NEAR(projection.first.x(), arc_length, eps); @@ -150,58 +151,18 @@ TEST(DrivableAreaExpansionProjection, LinestringToPoint) } } -TEST(DrivableAreaExpansionProjection, SubLinestring) -{ - using drivable_area_expansion::sub_linestring; - - const linestring_t ls = { - point_t{0.0, 0.0}, point_t{1.0, 0.0}, point_t{2.0, 0.0}, point_t{3.0, 0.0}, - point_t{4.0, 0.0}, point_t{5.0, 0.0}, point_t{6.0, 0.0}, - }; - { - // arc lengths equal to the original range: same linestring is returned - const auto sub = sub_linestring(ls, 0.0, 6.0); - ASSERT_EQ(ls.size(), sub.size()); - for (auto i = 0lu; i < ls.size(); ++i) EXPECT_TRUE(boost::geometry::equals(ls[i], sub[i])); - } - { - // arc lengths equal to existing point: sub-linestring with same points - const auto sub = sub_linestring(ls, 1.0, 5.0); - ASSERT_EQ(ls.size() - 2lu, sub.size()); - for (auto i = 0lu; i < sub.size(); ++i) EXPECT_TRUE(boost::geometry::equals(ls[i + 1], sub[i])); - } - { - // arc lengths inside the original: sub-linestring with some interpolated points - const auto sub = sub_linestring(ls, 1.5, 2.5); - ASSERT_EQ(sub.size(), 3lu); - EXPECT_NEAR(sub[0].x(), 1.5, eps); - EXPECT_NEAR(sub[1].x(), 2.0, eps); - EXPECT_NEAR(sub[2].x(), 2.5, eps); - for (const auto & p : sub) EXPECT_NEAR(p.y(), 0.0, eps); - } - { - // arc length outside of the original range: first & last point are replaced by interpolations - const auto sub = sub_linestring(ls, -0.5, 8.5); - ASSERT_EQ(sub.size(), ls.size()); - EXPECT_NEAR(sub.front().x(), -0.5, eps); - for (auto i = 1lu; i + 1 < ls.size(); ++i) EXPECT_TRUE(boost::geometry::equals(ls[i], sub[i])); - EXPECT_NEAR(sub.back().x(), 8.5, eps); - for (const auto & p : sub) EXPECT_NEAR(p.y(), 0.0, eps); - } -} - TEST(DrivableAreaExpansionProjection, InverseProjection) { using drivable_area_expansion::linestring_to_point_projection; using drivable_area_expansion::point_to_linestring_projection; - linestring_t ls = { - point_t(0.0, 0.0), point_t(10.0, 0.0), point_t(10.0, 10.0), point_t(0.0, 10.0), - point_t(5.0, 5.0)}; + LineString2d ls = { + Point2d(0.0, 0.0), Point2d(10.0, 0.0), Point2d(10.0, 10.0), Point2d(0.0, 10.0), + Point2d(5.0, 5.0)}; for (auto x = 0.0; x < 10.0; x += 0.1) { for (auto y = 0.0; x < 10.0; x += 0.1) { - point_t p(x, y); + Point2d p(x, y); const auto projection = point_to_linestring_projection(p, ls); const auto inverse = linestring_to_point_projection(ls, projection.arc_length, projection.distance); @@ -211,7 +172,7 @@ TEST(DrivableAreaExpansionProjection, InverseProjection) } } -TEST(DrivableAreaExpansionProjection, expandDrivableArea) +TEST(DrivableAreaExpansionProjection, expand_drivable_area) { drivable_area_expansion::DrivableAreaExpansionParameters params; drivable_area_expansion::PredictedObjects dynamic_objects; @@ -254,78 +215,59 @@ TEST(DrivableAreaExpansionProjection, expandDrivableArea) params.avoid_dynamic_objects = false; params.avoid_linestring_dist = 0.0; params.avoid_linestring_types = {}; - params.compensate_extra_dist = false; params.max_expansion_distance = 0.0; // means no limit params.max_path_arc_length = 0.0; // means no limit - params.extra_arc_length = 1.0; - params.expansion_method = "polygon"; + params.resample_interval = 1.0; // 2m x 4m ego footprint - params.ego_front_offset = 1.0; - params.ego_rear_offset = -1.0; - params.ego_left_offset = 2.0; - params.ego_right_offset = -2.0; + params.vehicle_info.front_overhang_m = 0.0; + params.vehicle_info.wheel_base_m = 2.0; + params.vehicle_info.vehicle_width_m = 2.0; } - // we expect the drivable area to be expanded by 1m on each side - drivable_area_expansion::expandDrivableArea( - path, params, dynamic_objects, route_handler, path_lanes); + behavior_path_planner::PlannerData planner_data; + planner_data.drivable_area_expansion_parameters = params; + planner_data.dynamic_object = + std::make_shared(dynamic_objects); + planner_data.self_odometry = std::make_shared(); + planner_data.route_handler = std::make_shared(route_handler); + drivable_area_expansion::expand_drivable_area( + path, std::make_shared(planner_data)); // unchanged path points ASSERT_EQ(path.points.size(), 3ul); for (auto i = 0.0; i < path.points.size(); ++i) { EXPECT_NEAR(path.points[i].point.pose.position.x, i, eps); EXPECT_NEAR(path.points[i].point.pose.position.y, 0.0, eps); } - + // straight path: no expansion // expanded left bound ASSERT_EQ(path.left_bound.size(), 3ul); EXPECT_NEAR(path.left_bound[0].x, 0.0, eps); EXPECT_NEAR(path.left_bound[0].y, 1.0, eps); - EXPECT_NEAR(path.left_bound[1].x, 0.0, eps); - EXPECT_NEAR(path.left_bound[1].y, 2.0, eps); + EXPECT_NEAR(path.left_bound[1].x, 1.0, eps); + EXPECT_NEAR(path.left_bound[1].y, 1.0, eps); EXPECT_NEAR(path.left_bound[2].x, 2.0, eps); - EXPECT_NEAR(path.left_bound[2].y, 2.0, eps); + EXPECT_NEAR(path.left_bound[2].y, 1.0, eps); // expanded right bound ASSERT_EQ(path.right_bound.size(), 3ul); EXPECT_NEAR(path.right_bound[0].x, 0.0, eps); - EXPECT_NEAR(path.right_bound[0].y, -2.0, eps); - EXPECT_NEAR(path.right_bound[1].x, 2.0, eps); - EXPECT_NEAR(path.right_bound[1].y, -2.0, eps); + EXPECT_NEAR(path.right_bound[0].y, -1.0, eps); + EXPECT_NEAR(path.right_bound[1].x, 1.0, eps); + EXPECT_NEAR(path.right_bound[1].y, -1.0, eps); EXPECT_NEAR(path.right_bound[2].x, 2.0, eps); EXPECT_NEAR(path.right_bound[2].y, -1.0, eps); -} -TEST(DrivableAreaExpansion, calculateDistanceLimit) -{ - using drivable_area_expansion::calculateDistanceLimit; - using drivable_area_expansion::linestring_t; - using drivable_area_expansion::multi_linestring_t; - using drivable_area_expansion::polygon_t; + // add some curvature + path.points[1].point.pose.position.y = 0.5; - { - const linestring_t base_ls = {{0.0, 0.0}, {10.0, 0.0}}; - const multi_linestring_t uncrossable_lines = {}; - const polygon_t expansion_polygon = { - {{0.0, -4.0}, {0.0, 4.0}, {10.0, 4.0}, {10.0, -4.0}, {10.0, -4.0}}, {}}; - const auto limit_distance = - calculateDistanceLimit(base_ls, expansion_polygon, uncrossable_lines); - EXPECT_NEAR(limit_distance, std::numeric_limits::max(), 1e-9); - } - { - const linestring_t base_ls = {{0.0, 0.0}, {10.0, 0.0}}; - const linestring_t uncrossable_line = {{0.0, 2.0}, {10.0, 2.0}}; - const polygon_t expansion_polygon = { - {{0.0, -4.0}, {0.0, 4.0}, {10.0, 4.0}, {10.0, -4.0}, {10.0, -4.0}}, {}}; - const auto limit_distance = - calculateDistanceLimit(base_ls, expansion_polygon, {uncrossable_line}); - EXPECT_NEAR(limit_distance, 2.0, 1e-9); - } - { - const linestring_t base_ls = {{0.0, 0.0}, {10.0, 0.0}}; - const multi_linestring_t uncrossable_lines = { - {{0.0, 2.0}, {10.0, 2.0}}, {{0.0, 1.5}, {10.0, 1.0}}}; - const polygon_t expansion_polygon = { - {{0.0, -4.0}, {0.0, 4.0}, {10.0, 4.0}, {10.0, -4.0}, {10.0, -4.0}}, {}}; - const auto limit_distance = - calculateDistanceLimit(base_ls, expansion_polygon, uncrossable_lines); - EXPECT_NEAR(limit_distance, 1.0, 1e-9); - } + drivable_area_expansion::expand_drivable_area( + path, std::make_shared(planner_data)); + // expanded left bound + ASSERT_EQ(path.left_bound.size(), 3ul); + EXPECT_GT(path.left_bound[0].y, 1.0); + EXPECT_GT(path.left_bound[1].y, 1.0); + EXPECT_GT(path.left_bound[2].y, 1.0); + // expanded right bound + ASSERT_EQ(path.right_bound.size(), 3ul); + EXPECT_LT(path.right_bound[0].y, -1.0); + EXPECT_LT(path.right_bound[1].y, -1.0); + EXPECT_LT(path.right_bound[2].y, -1.0); } diff --git a/planning/behavior_path_planner/test/test_lane_change_utils.cpp b/planning/behavior_path_planner/test/test_lane_change_utils.cpp index 3a809509641f4..ad64a7ae3feb5 100644 --- a/planning/behavior_path_planner/test/test_lane_change_utils.cpp +++ b/planning/behavior_path_planner/test/test_lane_change_utils.cpp @@ -12,7 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. #include "behavior_path_planner/utils/path_safety_checker/safety_check.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" + +#include +#include #include #include diff --git a/planning/behavior_path_planner/test/test_safety_check.cpp b/planning/behavior_path_planner/test/test_safety_check.cpp index e1b74636f5ed1..a5fc19d1ecbbe 100644 --- a/planning/behavior_path_planner/test/test_safety_check.cpp +++ b/planning/behavior_path_planner/test/test_safety_check.cpp @@ -15,7 +15,8 @@ #include "behavior_path_planner/marker_utils/utils.hpp" #include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include "behavior_path_planner/utils/path_safety_checker/safety_check.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" + +#include #include @@ -27,10 +28,10 @@ constexpr double epsilon = 1e-6; using autoware_auto_perception_msgs::msg::Shape; +using behavior_path_planner::utils::path_safety_checker::CollisionCheckDebug; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; -using marker_utils::CollisionCheckDebug; using tier4_autoware_utils::Point2d; using tier4_autoware_utils::Polygon2d; @@ -51,9 +52,10 @@ TEST(BehaviorPathPlanningSafetyUtilsTest, createExtendedEgoPolygon) const double lon_length = 10.0; const double lat_margin = 2.0; + const bool is_stopped_object = false; - const auto polygon = - createExtendedPolygon(ego_pose, vehicle_info, lon_length, lat_margin, debug); + const auto polygon = createExtendedPolygon( + ego_pose, vehicle_info, lon_length, lat_margin, is_stopped_object, debug); EXPECT_EQ(polygon.outer().size(), static_cast(5)); @@ -78,9 +80,10 @@ TEST(BehaviorPathPlanningSafetyUtilsTest, createExtendedEgoPolygon) const double lon_length = 10.0; const double lat_margin = 2.0; + const bool is_stopped_object = false; - const auto polygon = - createExtendedPolygon(ego_pose, vehicle_info, lon_length, lat_margin, debug); + const auto polygon = createExtendedPolygon( + ego_pose, vehicle_info, lon_length, lat_margin, is_stopped_object, debug); EXPECT_EQ(polygon.outer().size(), static_cast(5)); @@ -106,9 +109,10 @@ TEST(BehaviorPathPlanningSafetyUtilsTest, createExtendedEgoPolygon) const double lon_length = 10.0; const double lat_margin = 2.0; + const bool is_stopped_object = false; - const auto polygon = - createExtendedPolygon(ego_pose, vehicle_info, lon_length, lat_margin, debug); + const auto polygon = createExtendedPolygon( + ego_pose, vehicle_info, lon_length, lat_margin, is_stopped_object, debug); EXPECT_EQ(polygon.outer().size(), static_cast(5)); @@ -154,9 +158,11 @@ TEST(BehaviorPathPlanningSafetyUtilsTest, createExtendedObjPolygon) const double lon_length = 10.0; const double lat_margin = 2.0; + const bool is_stopped_object = false; CollisionCheckDebug debug; - const auto polygon = createExtendedPolygon(obj_pose, shape, lon_length, lat_margin, debug); + const auto polygon = + createExtendedPolygon(obj_pose, shape, lon_length, lat_margin, is_stopped_object, debug); EXPECT_EQ(polygon.outer().size(), static_cast(5)); diff --git a/planning/behavior_path_planner/test/test_turn_signal.cpp b/planning/behavior_path_planner/test/test_turn_signal.cpp index 8c6f8de7ee6a0..454be6b75fc73 100644 --- a/planning/behavior_path_planner/test/test_turn_signal.cpp +++ b/planning/behavior_path_planner/test/test_turn_signal.cpp @@ -12,8 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. #include "behavior_path_planner/turn_signal_decider.hpp" +#include "motion_utils/trajectory/trajectory.hpp" + +#include #include "autoware_auto_planning_msgs/msg/path_point.hpp" +#include #include #include diff --git a/planning/behavior_path_planner/test/test_utils.cpp b/planning/behavior_path_planner/test/test_utils.cpp index b9e2727ebaa13..4eefa27e88806 100644 --- a/planning/behavior_path_planner/test/test_utils.cpp +++ b/planning/behavior_path_planner/test/test_utils.cpp @@ -14,8 +14,10 @@ #include "behavior_path_planner/utils/utils.hpp" #include "input.hpp" #include "lanelet2_core/Attribute.h" +#include "lanelet2_core/geometry/LineString.h" #include "lanelet2_core/geometry/Point.h" #include "lanelet2_core/primitives/Lanelet.h" +#include "motion_utils/trajectory/path_with_lane_id.hpp" #include #include diff --git a/planning/behavior_velocity_blind_spot_module/config/blind_spot.param.yaml b/planning/behavior_velocity_blind_spot_module/config/blind_spot.param.yaml index 3aa8c82a5e556..f9ddd3ce61099 100644 --- a/planning/behavior_velocity_blind_spot_module/config/blind_spot.param.yaml +++ b/planning/behavior_velocity_blind_spot_module/config/blind_spot.param.yaml @@ -8,4 +8,4 @@ max_future_movement_time: 10.0 # [second] threshold_yaw_diff: 0.523 # [rad] adjacent_extend_width: 1.5 # [m] - enable_rtc: true # If set to true, the scene modules require approval from the rtc (request to cooperate) function. If set to false, the modules can be executed without requiring rtc approval + enable_rtc: false # If set to true, the scene modules require approval from the rtc (request to cooperate) function. If set to false, the modules can be executed without requiring rtc approval diff --git a/planning/behavior_velocity_blind_spot_module/src/debug.cpp b/planning/behavior_velocity_blind_spot_module/src/debug.cpp index dbd4764585d81..7da494bfd5231 100644 --- a/planning/behavior_velocity_blind_spot_module/src/debug.cpp +++ b/planning/behavior_velocity_blind_spot_module/src/debug.cpp @@ -16,7 +16,12 @@ #include #include -#include +#include +#include + +#include + +#include #include diff --git a/planning/behavior_velocity_blind_spot_module/src/manager.cpp b/planning/behavior_velocity_blind_spot_module/src/manager.cpp index 09fef7a496b09..d64f1ebc39a51 100644 --- a/planning/behavior_velocity_blind_spot_module/src/manager.cpp +++ b/planning/behavior_velocity_blind_spot_module/src/manager.cpp @@ -16,6 +16,7 @@ #include #include +#include #include @@ -27,22 +28,26 @@ namespace behavior_velocity_planner { +using tier4_autoware_utils::getOrDeclareParameter; + BlindSpotModuleManager::BlindSpotModuleManager(rclcpp::Node & node) : SceneModuleManagerInterfaceWithRTC( node, getModuleName(), - node.declare_parameter(std::string(getModuleName()) + ".enable_rtc")) + getOrDeclareParameter(node, std::string(getModuleName()) + ".enable_rtc")) { const std::string ns(getModuleName()); - planner_param_.use_pass_judge_line = node.declare_parameter(ns + ".use_pass_judge_line"); - planner_param_.stop_line_margin = node.declare_parameter(ns + ".stop_line_margin"); - planner_param_.backward_length = node.declare_parameter(ns + ".backward_length"); + planner_param_.use_pass_judge_line = + getOrDeclareParameter(node, ns + ".use_pass_judge_line"); + planner_param_.stop_line_margin = getOrDeclareParameter(node, ns + ".stop_line_margin"); + planner_param_.backward_length = getOrDeclareParameter(node, ns + ".backward_length"); planner_param_.ignore_width_from_center_line = - node.declare_parameter(ns + ".ignore_width_from_center_line"); + getOrDeclareParameter(node, ns + ".ignore_width_from_center_line"); planner_param_.max_future_movement_time = - node.declare_parameter(ns + ".max_future_movement_time"); - planner_param_.threshold_yaw_diff = node.declare_parameter(ns + ".threshold_yaw_diff"); + getOrDeclareParameter(node, ns + ".max_future_movement_time"); + planner_param_.threshold_yaw_diff = + getOrDeclareParameter(node, ns + ".threshold_yaw_diff"); planner_param_.adjacent_extend_width = - node.declare_parameter(ns + ".adjacent_extend_width"); + getOrDeclareParameter(node, ns + ".adjacent_extend_width"); } void BlindSpotModuleManager::launchNewModules( diff --git a/planning/behavior_velocity_blind_spot_module/src/scene.cpp b/planning/behavior_velocity_blind_spot_module/src/scene.cpp index 440c360e712a3..775f3b1c65973 100644 --- a/planning/behavior_velocity_blind_spot_module/src/scene.cpp +++ b/planning/behavior_velocity_blind_spot_module/src/scene.cpp @@ -20,7 +20,7 @@ #include #include #include -#include +#include #include #include diff --git a/planning/behavior_velocity_crosswalk_module/CMakeLists.txt b/planning/behavior_velocity_crosswalk_module/CMakeLists.txt index 118931780a92c..3217b3828b8d7 100644 --- a/planning/behavior_velocity_crosswalk_module/CMakeLists.txt +++ b/planning/behavior_velocity_crosswalk_module/CMakeLists.txt @@ -13,3 +13,8 @@ ament_auto_add_library(${PROJECT_NAME} SHARED ) ament_auto_package(INSTALL_TO_SHARE config) + +install(PROGRAMS + scripts/time_to_collision_plotter.py + DESTINATION lib/${PROJECT_NAME} +) diff --git a/planning/behavior_velocity_crosswalk_module/README.md b/planning/behavior_velocity_crosswalk_module/README.md index f85c472c6d856..5f82fe839fda6 100644 --- a/planning/behavior_velocity_crosswalk_module/README.md +++ b/planning/behavior_velocity_crosswalk_module/README.md @@ -218,6 +218,16 @@ When multiple crosswalks are nearby (such as intersection), this module may make ### Known Issues +### Debugging + +By `ros2 run behavior_velocity_crosswalk_module time_to_collision_plotter.py`, you can visualize the following figure of the ego and pedestrian's time to collision. +The label of each plot is `-`. + +
+ ![limitation](docs/time_to_collision_plot.png){width=1000} +
Plot of time to collision
+
+ ### References/External links [1] 佐藤 みなみ, 早坂 祥一, 清水 政行, 村野 隆彦, 横断歩行者に対するドライバのリスク回避行動のモデル化, 自動車技術会論文集, 2013, 44 巻, 3 号, p. 931-936. diff --git a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml b/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml index 4b0a1ffc4317b..a4f9e9d7ce23d 100644 --- a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml +++ b/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml @@ -5,7 +5,7 @@ show_processing_time: false # [-] whether to show processing time # param for input data traffic_light_state_timeout: 3.0 # [s] timeout threshold for traffic light signal - enable_rtc: true # if true, the scene modules should be approved by (request to cooperate)rtc function. if false, the module can be run without approval from rtc. + enable_rtc: false # if true, the scene modules should be approved by (request to cooperate)rtc function. if false, the module can be run without approval from rtc. # param for stop position stop_position: @@ -42,13 +42,21 @@ ego_pass_later_margin_x: [0.0] # [[s]] time to vehicle margin vector for object pass first situation (the module judges that ego don't have to stop at TTV + MARGIN < TTC condition) ego_pass_later_margin_y: [10.0] # [[s]] time to collision margin vector for object pass first situation (the module judges that ego don't have to stop at TTV + MARGIN < TTC condition) ego_pass_later_additional_margin: 0.5 # [s] additional time margin for object pass first situation to suppress chattering - max_offset_to_crosswalk_for_yield: 0.0 # [m] maximum offset from ego's front to crosswalk for yield. Positive value means in front of the crosswalk. + + no_stop_decision: + max_offset_to_crosswalk_for_yield: 0.0 # [m] maximum offset from ego's front to crosswalk for yield. Positive value means in front of the crosswalk. + min_acc: -1.0 # min acceleration [m/ss] + min_jerk: -1.0 # min jerk [m/sss] + max_jerk: 1.0 # max jerk [m/sss] + stop_object_velocity_threshold: 0.28 # [m/s] velocity threshold for the module to judge whether the objects is stopped (0.28 m/s = 1.0 kmph) min_object_velocity: 1.39 # [m/s] minimum object velocity (compare the estimated velocity by perception module with this parameter and adopt the larger one to calculate TTV. 1.39 m/s = 5.0 kmph) ## param for yielding - disable_stop_for_yield_cancel: false # for the crosswalk where there is a traffic signal - disable_yield_for_new_stopped_object: false # for the crosswalk where there is a traffic signal - timeout_no_intention_to_walk: 3.0 # [s] if the pedestrian does not move for X seconds after stopping before the crosswalk, the module judge that ego is able to pass first. + disable_stop_for_yield_cancel: true # for the crosswalk where there is a traffic signal + disable_yield_for_new_stopped_object: true # for the crosswalk where there is a traffic signal + # if the pedestrian does not move for X seconds after stopping before the crosswalk, the module judge that ego is able to pass first. + distance_map_for_no_intention_to_walk: [1.0, 5.0] # [m] ascending order + timeout_map_for_no_intention_to_walk: [3.0, 0.0] # [s] timeout_ego_stop_for_yield: 3.0 # [s] the amount of time which ego should be stopping to query whether it yields or not # param for target object filtering diff --git a/planning/behavior_velocity_crosswalk_module/docs/time_to_collision_plot.png b/planning/behavior_velocity_crosswalk_module/docs/time_to_collision_plot.png new file mode 100644 index 0000000000000..71ce642f382bc Binary files /dev/null and b/planning/behavior_velocity_crosswalk_module/docs/time_to_collision_plot.png differ diff --git a/planning/behavior_velocity_crosswalk_module/include/behavior_velocity_crosswalk_module/util.hpp b/planning/behavior_velocity_crosswalk_module/include/behavior_velocity_crosswalk_module/util.hpp index 578ae66e40006..6c70e7a7f6dc0 100644 --- a/planning/behavior_velocity_crosswalk_module/include/behavior_velocity_crosswalk_module/util.hpp +++ b/planning/behavior_velocity_crosswalk_module/include/behavior_velocity_crosswalk_module/util.hpp @@ -26,6 +26,7 @@ #include #include #include +#include #include #include @@ -42,8 +43,6 @@ namespace behavior_velocity_planner using autoware_auto_planning_msgs::msg::PathPointWithLaneId; using autoware_auto_planning_msgs::msg::PathWithLaneId; -using tier4_autoware_utils::createPoint; -using tier4_autoware_utils::Point2d; using tier4_planning_msgs::msg::StopFactor; enum class CollisionState { YIELD, EGO_PASS_FIRST, EGO_PASS_LATER, IGNORE }; @@ -73,7 +72,7 @@ struct DebugData std::optional range_near_point{std::nullopt}; std::optional range_far_point{std::nullopt}; - std::vector> collision_points; + std::vector> collision_points; std::vector stop_poses; std::vector slow_poses; diff --git a/planning/behavior_velocity_crosswalk_module/package.xml b/planning/behavior_velocity_crosswalk_module/package.xml index d5ef92a4c097d..3830aa9edddff 100644 --- a/planning/behavior_velocity_crosswalk_module/package.xml +++ b/planning/behavior_velocity_crosswalk_module/package.xml @@ -9,6 +9,7 @@ Tomoya Kimura Shumpei Wakabayashi Takayuki Murooka + Kyoichi Sugahara Apache License 2.0 @@ -21,6 +22,7 @@ autoware_auto_perception_msgs autoware_auto_planning_msgs autoware_auto_tf2 + autoware_perception_msgs behavior_velocity_planner_common eigen geometry_msgs @@ -34,6 +36,7 @@ sensor_msgs tier4_api_msgs tier4_autoware_utils + tier4_debug_msgs vehicle_info_util visualization_msgs diff --git a/planning/behavior_velocity_crosswalk_module/scripts/time_to_collision_plotter.py b/planning/behavior_velocity_crosswalk_module/scripts/time_to_collision_plotter.py new file mode 100755 index 0000000000000..8a9de1a563249 --- /dev/null +++ b/planning/behavior_velocity_crosswalk_module/scripts/time_to_collision_plotter.py @@ -0,0 +1,214 @@ +#!/usr/bin/env python3 + +# Copyright 2023 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 +from collections import deque +from copy import deepcopy + +from ament_index_python.packages import get_package_share_directory +import matplotlib.pyplot as plt +import rclpy +from rclpy.node import Node +from tier4_debug_msgs.msg import StringStamped +import yaml + + +def expand_margin(x_vec, y_vec, min_x=0.0, max_x=10.0): + expanded_x_vec = deepcopy(x_vec) + expanded_y_vec = deepcopy(y_vec) + + if min_x < expanded_x_vec[0]: + expanded_x_vec = [min_x] + expanded_x_vec + min_y = expanded_y_vec[0] + min_x - expanded_x_vec[0] + expanded_y_vec = [min_y] + expanded_y_vec + + if expanded_x_vec[-1] < max_x: + expanded_x_vec.append(max_x) + max_y = expanded_y_vec[-1] + max_x - expanded_x_vec[-2] + expanded_y_vec.append(max_y) + + return expanded_x_vec, expanded_y_vec + + +class CollisionInfo: + def __init__(self, module_id, obj_id, ttc, ttv, state): + self.module_id = module_id + self.obj_id = obj_id + self.ttc = ttc + self.ttv = ttv + self.state = state + + +class TimeToCollisionPlotter(Node): + def __init__(self, args): + super().__init__("time_to_collision_plotter") + + self.depth = args.depth + + self.sub_calculation_cost = self.create_subscription( + StringStamped, + "/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/collision_info", + self.on_collision_info, + 1, + ) + + self.time_to_collision_hist = {} + self.max_hist_size = args.depth + + crosswalk_config = ( + get_package_share_directory("autoware_launch") + + "/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/crosswalk.param.yaml" + ) + with open(crosswalk_config) as f: + crosswalk_config_obj = yaml.safe_load(f)["/**"]["ros__parameters"] + + ego_pass_first_x = crosswalk_config_obj["crosswalk"]["pass_judge"][ + "ego_pass_first_margin_x" + ] + ego_pass_first_y = crosswalk_config_obj["crosswalk"]["pass_judge"][ + "ego_pass_first_margin_y" + ] + self.ego_pass_first_margin = [ego_pass_first_x, ego_pass_first_y] + + ego_pass_later_x = crosswalk_config_obj["crosswalk"]["pass_judge"][ + "ego_pass_later_margin_x" + ] + ego_pass_later_y = crosswalk_config_obj["crosswalk"]["pass_judge"][ + "ego_pass_later_margin_y" + ] + self.ego_pass_later_margin = [ego_pass_later_x, ego_pass_later_y] + + plt.ion() + _, self.ax = plt.subplots() + plt.show() + plt.connect("key_press_event", self.on_key) + + self.timer = self.create_timer(0.3, self.on_timer) + + def on_key(self, event): + if event.key == "c": + self.time_to_collision_hist = {} + + def on_collision_info(self, msg): + collision_info_data_vec = [] + collision_info_str_vec = msg.data[:-1].split(",") + if len(collision_info_str_vec) < 5: + return + + for i in range(int(len(collision_info_str_vec) / 5)): + collision_info_data_vec.append( + CollisionInfo(*collision_info_str_vec[i * 5 : i * 5 + 5]) + ) + + # memorize data in the history dictionary + for collision_info_data in collision_info_data_vec: + uuid = collision_info_data.module_id + collision_info_data.obj_id + if uuid not in self.time_to_collision_hist: + self.time_to_collision_hist[uuid] = deque([]) + + self.time_to_collision_hist[uuid].append( + [float(collision_info_data.ttv), float(collision_info_data.ttc)] + ) + + def on_timer(self): + # make the size of history not exceed the maximum value + for uuid in self.time_to_collision_hist: + while self.max_hist_size < len(self.time_to_collision_hist[uuid]): + self.time_to_collision_hist[uuid].popleft() + + plt.cla() + + # plot "ego_pass_later" border + ego_pass_later_x = [] + ego_pass_later_y = [] + for i in range(len(self.ego_pass_later_margin[0])): + ego_pass_later_x.append(self.ego_pass_later_margin[0][i]) + ego_pass_later_y.append(self.ego_pass_later_margin[1][i]) + expanded_ego_pass_later_x, expanded_ego_pass_later_y = expand_margin( + ego_pass_later_x, ego_pass_later_y + ) + plt.fill_between( + expanded_ego_pass_later_x, + expanded_ego_pass_later_y, + [expanded_ego_pass_later_y[-1] for i in expanded_ego_pass_later_y], + alpha=0.2, + color="green", + ) + + # plot "ego_pass_first" border + ego_pass_first_x = [] + ego_pass_first_y = [] + for i in range(len(self.ego_pass_first_margin[0])): + ego_pass_first_x.append(self.ego_pass_first_margin[0][i]) + ego_pass_first_y.append(self.ego_pass_first_margin[1][i]) + expanded_ego_pass_first_x, expanded_ego_pass_first_y = expand_margin( + ego_pass_first_x, ego_pass_first_y + ) + plt.fill_between( + expanded_ego_pass_first_y, + [expanded_ego_pass_first_x[0] for i in expanded_ego_pass_first_x], + expanded_ego_pass_first_x, + alpha=0.2, + color="blue", + ) + plt.text( + expanded_ego_pass_later_x[0], + expanded_ego_pass_later_y[-1], + "Ego passes later.", + va="top", + ha="left", + color="green", + ) + plt.text( + expanded_ego_pass_first_y[-1], + expanded_ego_pass_first_x[0], + "Ego passes first.", + va="bottom", + ha="right", + color="blue", + ) + plt.text(0, 0, "Ego yields.", va="bottom", ha="left", color="red") + + # plot history + for uuid in self.time_to_collision_hist: + hist = self.time_to_collision_hist[uuid] + plt.plot( + [val[0] for val in hist], + [val[1] for val in hist], + label=uuid[0:4] + "-" + uuid[4:8], + ) + + plt.legend() + plt.draw() + plt.title("Time to collision") + plt.xlabel("Pedestrian's time to collision") + plt.ylabel("Ego's time to collision") + plt.pause(0.01) + + +if __name__ == "__main__": + parser = argparse.ArgumentParser() + parser.add_argument( + "-d", + "--depth", + type=float, + default=100, + ) + args = parser.parse_args() + + rclpy.init() + node = TimeToCollisionPlotter(args) + rclpy.spin(node) diff --git a/planning/behavior_velocity_crosswalk_module/src/debug.cpp b/planning/behavior_velocity_crosswalk_module/src/debug.cpp index 409898233cbbe..d6a2a3cb185a1 100644 --- a/planning/behavior_velocity_crosswalk_module/src/debug.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/debug.cpp @@ -15,8 +15,9 @@ #include "scene_crosswalk.hpp" #include -#include -#include +#include +#include +#include #include @@ -44,8 +45,8 @@ visualization_msgs::msg::MarkerArray createCrosswalkMarkers( { for (size_t i = 0; i < debug_data.collision_points.size(); ++i) { const auto & collision_point = debug_data.collision_points.at(i); - const auto & point = collision_point.first; - const auto & state = collision_point.second; + const auto & point = std::get<1>(collision_point); + const auto & state = std::get<2>(collision_point); auto marker = createDefaultMarker( "map", now, "collision point state", uid + i++, Marker::TEXT_VIEW_FACING, @@ -97,7 +98,7 @@ visualization_msgs::msg::MarkerArray createCrosswalkMarkers( for (size_t i = 0; i < debug_data.obj_polygons.size(); ++i) { const auto & points = debug_data.obj_polygons.at(i); auto marker = createDefaultMarker( - "map", now, "object polygon", uid + i++, Marker::LINE_STRIP, createMarkerScale(0.1, 0.0, 0.0), + "map", now, "object polygon", uid + i, Marker::LINE_STRIP, createMarkerScale(0.1, 0.0, 0.0), createMarkerColor(1.0, 0.0, 1.0, 0.999)); marker.points = points; // marker.points.push_back(marker.points.front()); @@ -107,8 +108,8 @@ visualization_msgs::msg::MarkerArray createCrosswalkMarkers( for (size_t i = 0; i < debug_data.ego_polygons.size(); ++i) { const auto & points = debug_data.ego_polygons.at(i); auto marker = createDefaultMarker( - "map", now, "vehicle polygon", uid + i++, Marker::LINE_STRIP, - createMarkerScale(0.1, 0.0, 0.0), createMarkerColor(1.0, 1.0, 0.0, 0.999)); + "map", now, "vehicle polygon", uid + i, Marker::LINE_STRIP, createMarkerScale(0.1, 0.0, 0.0), + createMarkerColor(1.0, 1.0, 0.0, 0.999)); marker.points = points; // marker.points.push_back(marker.points.front()); msg.markers.push_back(marker); @@ -134,7 +135,7 @@ visualization_msgs::msg::MarkerArray createCrosswalkMarkers( "map", now, "collision point", uid, Marker::POINTS, createMarkerScale(0.25, 0.25, 0.0), createMarkerColor(1.0, 0.0, 1.0, 0.999)); for (const auto & collision_point : debug_data.collision_points) { - marker.points.push_back(collision_point.first.collision_point); + marker.points.push_back(std::get<1>(collision_point).collision_point); } msg.markers.push_back(marker); } diff --git a/planning/behavior_velocity_crosswalk_module/src/manager.cpp b/planning/behavior_velocity_crosswalk_module/src/manager.cpp index 992d1c48eb9f4..dd8dc95a8ad3c 100644 --- a/planning/behavior_velocity_crosswalk_module/src/manager.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/manager.cpp @@ -15,6 +15,7 @@ #include "manager.hpp" #include +#include #include #include @@ -26,101 +27,110 @@ namespace behavior_velocity_planner { using lanelet::autoware::Crosswalk; +using tier4_autoware_utils::getOrDeclareParameter; CrosswalkModuleManager::CrosswalkModuleManager(rclcpp::Node & node) : SceneModuleManagerInterfaceWithRTC( node, getModuleName(), - node.declare_parameter(std::string(getModuleName()) + ".common.enable_rtc")) + getOrDeclareParameter(node, std::string(getModuleName()) + ".common.enable_rtc")) { const std::string ns(getModuleName()); // for crosswalk parameters auto & cp = crosswalk_planner_param_; - cp.show_processing_time = node.declare_parameter(ns + ".common.show_processing_time"); + cp.show_processing_time = getOrDeclareParameter(node, ns + ".common.show_processing_time"); // param for input data cp.traffic_light_state_timeout = - node.declare_parameter(ns + ".common.traffic_light_state_timeout"); + getOrDeclareParameter(node, ns + ".common.traffic_light_state_timeout"); // param for stop position cp.stop_distance_from_crosswalk = - node.declare_parameter(ns + ".stop_position.stop_distance_from_crosswalk"); + getOrDeclareParameter(node, ns + ".stop_position.stop_distance_from_crosswalk"); cp.stop_distance_from_object = - node.declare_parameter(ns + ".stop_position.stop_distance_from_object"); + getOrDeclareParameter(node, ns + ".stop_position.stop_distance_from_object"); cp.far_object_threshold = - node.declare_parameter(ns + ".stop_position.far_object_threshold"); + getOrDeclareParameter(node, ns + ".stop_position.far_object_threshold"); cp.stop_position_threshold = - node.declare_parameter(ns + ".stop_position.stop_position_threshold"); + getOrDeclareParameter(node, ns + ".stop_position.stop_position_threshold"); // param for ego velocity cp.min_slow_down_velocity = - node.declare_parameter(ns + ".slow_down.min_slow_down_velocity"); - cp.max_slow_down_jerk = node.declare_parameter(ns + ".slow_down.max_slow_down_jerk"); - cp.max_slow_down_accel = node.declare_parameter(ns + ".slow_down.max_slow_down_accel"); - cp.no_relax_velocity = node.declare_parameter(ns + ".slow_down.no_relax_velocity"); + getOrDeclareParameter(node, ns + ".slow_down.min_slow_down_velocity"); + cp.max_slow_down_jerk = getOrDeclareParameter(node, ns + ".slow_down.max_slow_down_jerk"); + cp.max_slow_down_accel = + getOrDeclareParameter(node, ns + ".slow_down.max_slow_down_accel"); + cp.no_relax_velocity = getOrDeclareParameter(node, ns + ".slow_down.no_relax_velocity"); // param for stuck vehicle cp.stuck_vehicle_velocity = - node.declare_parameter(ns + ".stuck_vehicle.stuck_vehicle_velocity"); + getOrDeclareParameter(node, ns + ".stuck_vehicle.stuck_vehicle_velocity"); cp.max_stuck_vehicle_lateral_offset = - node.declare_parameter(ns + ".stuck_vehicle.max_stuck_vehicle_lateral_offset"); + getOrDeclareParameter(node, ns + ".stuck_vehicle.max_stuck_vehicle_lateral_offset"); cp.stuck_vehicle_attention_range = - node.declare_parameter(ns + ".stuck_vehicle.stuck_vehicle_attention_range"); - cp.min_acc_for_stuck_vehicle = node.declare_parameter(ns + ".stuck_vehicle.min_acc"); - cp.max_jerk_for_stuck_vehicle = node.declare_parameter(ns + ".stuck_vehicle.max_jerk"); - cp.min_jerk_for_stuck_vehicle = node.declare_parameter(ns + ".stuck_vehicle.min_jerk"); + getOrDeclareParameter(node, ns + ".stuck_vehicle.stuck_vehicle_attention_range"); + cp.min_acc_for_stuck_vehicle = getOrDeclareParameter(node, ns + ".stuck_vehicle.min_acc"); + cp.max_jerk_for_stuck_vehicle = + getOrDeclareParameter(node, ns + ".stuck_vehicle.max_jerk"); + cp.min_jerk_for_stuck_vehicle = + getOrDeclareParameter(node, ns + ".stuck_vehicle.min_jerk"); // param for pass judge logic cp.ego_pass_first_margin_x = - node.declare_parameter>(ns + ".pass_judge.ego_pass_first_margin_x"); + getOrDeclareParameter>(node, ns + ".pass_judge.ego_pass_first_margin_x"); cp.ego_pass_first_margin_y = - node.declare_parameter>(ns + ".pass_judge.ego_pass_first_margin_y"); + getOrDeclareParameter>(node, ns + ".pass_judge.ego_pass_first_margin_y"); cp.ego_pass_first_additional_margin = - node.declare_parameter(ns + ".pass_judge.ego_pass_first_additional_margin"); + getOrDeclareParameter(node, ns + ".pass_judge.ego_pass_first_additional_margin"); cp.ego_pass_later_margin_x = - node.declare_parameter>(ns + ".pass_judge.ego_pass_later_margin_x"); + getOrDeclareParameter>(node, ns + ".pass_judge.ego_pass_later_margin_x"); cp.ego_pass_later_margin_y = - node.declare_parameter>(ns + ".pass_judge.ego_pass_later_margin_y"); + getOrDeclareParameter>(node, ns + ".pass_judge.ego_pass_later_margin_y"); cp.ego_pass_later_additional_margin = - node.declare_parameter(ns + ".pass_judge.ego_pass_later_additional_margin"); - cp.max_offset_to_crosswalk_for_yield = - node.declare_parameter(ns + ".pass_judge.max_offset_to_crosswalk_for_yield"); + getOrDeclareParameter(node, ns + ".pass_judge.ego_pass_later_additional_margin"); + cp.max_offset_to_crosswalk_for_yield = getOrDeclareParameter( + node, ns + ".pass_judge.no_stop_decision.max_offset_to_crosswalk_for_yield"); + cp.min_acc_for_no_stop_decision = + getOrDeclareParameter(node, ns + ".pass_judge.no_stop_decision.min_acc"); + cp.max_jerk_for_no_stop_decision = + getOrDeclareParameter(node, ns + ".pass_judge.no_stop_decision.max_jerk"); + cp.min_jerk_for_no_stop_decision = + getOrDeclareParameter(node, ns + ".pass_judge.no_stop_decision.min_jerk"); cp.stop_object_velocity = - node.declare_parameter(ns + ".pass_judge.stop_object_velocity_threshold"); - cp.min_object_velocity = node.declare_parameter(ns + ".pass_judge.min_object_velocity"); + getOrDeclareParameter(node, ns + ".pass_judge.stop_object_velocity_threshold"); + cp.min_object_velocity = + getOrDeclareParameter(node, ns + ".pass_judge.min_object_velocity"); cp.disable_stop_for_yield_cancel = - node.declare_parameter(ns + ".pass_judge.disable_stop_for_yield_cancel"); + getOrDeclareParameter(node, ns + ".pass_judge.disable_stop_for_yield_cancel"); cp.disable_yield_for_new_stopped_object = - node.declare_parameter(ns + ".pass_judge.disable_yield_for_new_stopped_object"); - cp.timeout_no_intention_to_walk = - node.declare_parameter(ns + ".pass_judge.timeout_no_intention_to_walk"); + getOrDeclareParameter(node, ns + ".pass_judge.disable_yield_for_new_stopped_object"); + cp.distance_map_for_no_intention_to_walk = getOrDeclareParameter>( + node, ns + ".pass_judge.distance_map_for_no_intention_to_walk"); + cp.timeout_map_for_no_intention_to_walk = getOrDeclareParameter>( + node, ns + ".pass_judge.timeout_map_for_no_intention_to_walk"); cp.timeout_ego_stop_for_yield = - node.declare_parameter(ns + ".pass_judge.timeout_ego_stop_for_yield"); + getOrDeclareParameter(node, ns + ".pass_judge.timeout_ego_stop_for_yield"); // param for target area & object cp.crosswalk_attention_range = - node.declare_parameter(ns + ".object_filtering.crosswalk_attention_range"); - cp.look_unknown = node.declare_parameter(ns + ".object_filtering.target_object.unknown"); - cp.look_bicycle = node.declare_parameter(ns + ".object_filtering.target_object.bicycle"); + getOrDeclareParameter(node, ns + ".object_filtering.crosswalk_attention_range"); + cp.look_unknown = + getOrDeclareParameter(node, ns + ".object_filtering.target_object.unknown"); + cp.look_bicycle = + getOrDeclareParameter(node, ns + ".object_filtering.target_object.bicycle"); cp.look_motorcycle = - node.declare_parameter(ns + ".object_filtering.target_object.motorcycle"); + getOrDeclareParameter(node, ns + ".object_filtering.target_object.motorcycle"); cp.look_pedestrian = - node.declare_parameter(ns + ".object_filtering.target_object.pedestrian"); + getOrDeclareParameter(node, ns + ".object_filtering.target_object.pedestrian"); } void CrosswalkModuleManager::launchNewModules(const PathWithLaneId & path) { const auto rh = planner_data_->route_handler_; - if (!opt_use_regulatory_element_) { - opt_use_regulatory_element_ = checkRegulatoryElementExistence(rh->getLaneletMapPtr()); - std::ostringstream string_stream; - string_stream << "use crosswalk regulatory element: "; - string_stream << std::boolalpha << *opt_use_regulatory_element_; - RCLCPP_INFO_STREAM(logger_, string_stream.str()); - } - const auto launch = [this, &path](const auto id) { - if (isModuleRegistered(id)) { + const auto launch = [this, &path]( + const auto lane_id, const std::optional & reg_elem_id) { + if (isModuleRegistered(lane_id)) { return; } @@ -128,27 +138,28 @@ void CrosswalkModuleManager::launchNewModules(const PathWithLaneId & path) const auto logger = logger_.get_child("crosswalk_module"); const auto lanelet_map_ptr = planner_data_->route_handler_->getLaneletMapPtr(); + // NOTE: module_id is always a lane id so that isModuleRegistered works correctly in the case + // where both regulatory element and non-regulatory element crosswalks exist. registerModule(std::make_shared( - id, lanelet_map_ptr, p, *opt_use_regulatory_element_, logger, clock_)); - generateUUID(id); - updateRTCStatus(getUUID(id), true, std::numeric_limits::lowest(), path.header.stamp); + node_, lane_id, reg_elem_id, lanelet_map_ptr, p, logger, clock_)); + generateUUID(lane_id); + updateRTCStatus( + getUUID(lane_id), true, std::numeric_limits::lowest(), path.header.stamp); }; - if (*opt_use_regulatory_element_) { - const auto crosswalk_leg_elem_map = planning_utils::getRegElemMapOnPath( - path, rh->getLaneletMapPtr(), planner_data_->current_odometry->pose); + const auto crosswalk_leg_elem_map = planning_utils::getRegElemMapOnPath( + path, rh->getLaneletMapPtr(), planner_data_->current_odometry->pose); - for (const auto & crosswalk : crosswalk_leg_elem_map) { - launch(crosswalk.first->id()); - } - } else { - const auto crosswalk_lanelets = getCrosswalksOnPath( - planner_data_->current_odometry->pose, path, rh->getLaneletMapPtr(), - rh->getOverallGraphPtr()); + for (const auto & crosswalk : crosswalk_leg_elem_map) { + // NOTE: The former id is a lane id, and the latter one is a regulatory element's id. + launch(crosswalk.first->crosswalkLanelet().id(), crosswalk.first->id()); + } - for (const auto & crosswalk : crosswalk_lanelets) { - launch(crosswalk.id()); - } + const auto crosswalk_lanelets = getCrosswalksOnPath( + planner_data_->current_odometry->pose, path, rh->getLaneletMapPtr(), rh->getOverallGraphPtr()); + + for (const auto & crosswalk : crosswalk_lanelets) { + launch(crosswalk.id(), std::nullopt); } } @@ -159,17 +170,14 @@ CrosswalkModuleManager::getModuleExpiredFunction(const PathWithLaneId & path) std::set crosswalk_id_set; - if (*opt_use_regulatory_element_) { - const auto crosswalk_leg_elem_map = planning_utils::getRegElemMapOnPath( - path, rh->getLaneletMapPtr(), planner_data_->current_odometry->pose); + crosswalk_id_set = getCrosswalkIdSetOnPath( + planner_data_->current_odometry->pose, path, rh->getLaneletMapPtr(), rh->getOverallGraphPtr()); - for (const auto & crosswalk : crosswalk_leg_elem_map) { - crosswalk_id_set.insert(crosswalk.first->id()); - } - } else { - crosswalk_id_set = getCrosswalkIdSetOnPath( - planner_data_->current_odometry->pose, path, rh->getLaneletMapPtr(), - rh->getOverallGraphPtr()); + const auto crosswalk_leg_elem_map = planning_utils::getRegElemMapOnPath( + path, rh->getLaneletMapPtr(), planner_data_->current_odometry->pose); + + for (const auto & crosswalk : crosswalk_leg_elem_map) { + crosswalk_id_set.insert(crosswalk.first->crosswalkLanelet().id()); } return [crosswalk_id_set](const std::shared_ptr & scene_module) { diff --git a/planning/behavior_velocity_crosswalk_module/src/manager.hpp b/planning/behavior_velocity_crosswalk_module/src/manager.hpp index 481a246af80dd..38bb63c121699 100644 --- a/planning/behavior_velocity_crosswalk_module/src/manager.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/manager.hpp @@ -51,8 +51,6 @@ class CrosswalkModuleManager : public SceneModuleManagerInterfaceWithRTC std::function &)> getModuleExpiredFunction( const PathWithLaneId & path) override; - - std::optional opt_use_regulatory_element_{std::nullopt}; }; class CrosswalkModulePlugin : public PluginWrapper diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index 0edfa6167e999..56f5072a2ade9 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -18,13 +18,21 @@ #include #include #include +#include +#include #include -#include +#include +#include +#include + +#include +#include #include #include #include #include +#include #include namespace behavior_velocity_planner @@ -36,6 +44,7 @@ using motion_utils::calcLateralOffset; using motion_utils::calcLongitudinalOffsetPoint; using motion_utils::calcLongitudinalOffsetPose; using motion_utils::calcSignedArcLength; +using motion_utils::calcSignedArcLengthPartialSum; using motion_utils::findNearestSegmentIndex; using motion_utils::insertTargetPoint; using motion_utils::resamplePath; @@ -67,39 +76,34 @@ std::vector toGeometryPointVector( return points; } -Polygon2d createOneStepPolygon( - const geometry_msgs::msg::Pose & p_front, const geometry_msgs::msg::Pose & p_back, - const geometry_msgs::msg::Polygon & base_polygon) +void offsetPolygon2d( + const geometry_msgs::msg::Pose & origin_point, const geometry_msgs::msg::Polygon & polygon, + Polygon2d & offset_polygon) { - Polygon2d one_step_polygon{}; - - { - geometry_msgs::msg::Polygon out_polygon{}; - geometry_msgs::msg::TransformStamped geometry_tf{}; - geometry_tf.transform = pose2transform(p_front); - tf2::doTransform(base_polygon, out_polygon, geometry_tf); - - for (const auto & p : out_polygon.points) { - one_step_polygon.outer().push_back(Point2d(p.x, p.y)); - } + for (const auto & polygon_point : polygon.points) { + const auto offset_pos = + tier4_autoware_utils::calcOffsetPose(origin_point, polygon_point.x, polygon_point.y, 0.0) + .position; + offset_polygon.outer().push_back(Point2d(offset_pos.x, offset_pos.y)); } +} - { - geometry_msgs::msg::Polygon out_polygon{}; - geometry_msgs::msg::TransformStamped geometry_tf{}; - geometry_tf.transform = pose2transform(p_back); - tf2::doTransform(base_polygon, out_polygon, geometry_tf); - - for (const auto & p : out_polygon.points) { - one_step_polygon.outer().push_back(Point2d(p.x, p.y)); - } +template +Polygon2d createMultiStepPolygon( + const T & obj_path_points, const geometry_msgs::msg::Polygon & polygon, const size_t start_idx, + const size_t end_idx) +{ + Polygon2d multi_step_polygon{}; + for (size_t i = start_idx; i <= end_idx; ++i) { + offsetPolygon2d( + tier4_autoware_utils::getPose(obj_path_points.at(i)), polygon, multi_step_polygon); } - Polygon2d hull_polygon{}; - bg::convex_hull(one_step_polygon, hull_polygon); - bg::correct(hull_polygon); + Polygon2d hull_multi_step_polygon{}; + bg::convex_hull(multi_step_polygon, hull_multi_step_polygon); + bg::correct(hull_multi_step_polygon); - return hull_polygon; + return hull_multi_step_polygon; } void sortCrosswalksByDistance( @@ -126,24 +130,12 @@ void sortCrosswalksByDistance( std::sort(crosswalks.begin(), crosswalks.end(), compare); } -std::vector calcOverlappingPoints(const Polygon2d & polygon1, const Polygon2d & polygon2) +std::vector calcOverlappingPoints(const Polygon2d & polygon1, const Polygon2d & polygon2) { // NOTE: If one polygon is fully inside the other polygon, the result is empty. - std::vector intersection{}; - bg::intersection(polygon1, polygon2, intersection); - - if (bg::within(polygon1, polygon2)) { - for (const auto & p : polygon1.outer()) { - intersection.push_back(Point2d{p.x(), p.y()}); - } - } - if (bg::within(polygon2, polygon1)) { - for (const auto & p : polygon2.outer()) { - intersection.push_back(Point2d{p.x(), p.y()}); - } - } - - return intersection; + std::vector intersection_polygons{}; + bg::intersection(polygon1, polygon2, intersection_polygons); + return intersection_polygons; } StopFactor createStopFactor( @@ -164,23 +156,40 @@ std::optional toStdOptional( } return std::nullopt; } + +tier4_debug_msgs::msg::StringStamped createStringStampedMessage( + const rclcpp::Time & now, const int64_t module_id_, + const std::vector> & collision_points) +{ + tier4_debug_msgs::msg::StringStamped msg; + msg.stamp = now; + for (const auto & collision_point : collision_points) { + std::stringstream ss; + ss << module_id_ << "," << std::get<0>(collision_point).substr(0, 4) << "," + << std::get<1>(collision_point).time_to_collision << "," + << std::get<1>(collision_point).time_to_vehicle << "," + << static_cast(std::get<2>(collision_point)) << ","; + msg.data += ss.str(); + } + return msg; +} } // namespace CrosswalkModule::CrosswalkModule( - const int64_t module_id, const lanelet::LaneletMapPtr & lanelet_map_ptr, - const PlannerParam & planner_param, const bool use_regulatory_element, + rclcpp::Node & node, const int64_t module_id, const std::optional & reg_elem_id, + const lanelet::LaneletMapPtr & lanelet_map_ptr, const PlannerParam & planner_param, const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock) : SceneModuleInterface(module_id, logger, clock), module_id_(module_id), planner_param_(planner_param), - use_regulatory_element_(use_regulatory_element) + use_regulatory_element_(reg_elem_id) { velocity_factor_.init(VelocityFactor::CROSSWALK); passed_safety_slow_point_ = false; if (use_regulatory_element_) { const auto reg_elem_ptr = std::dynamic_pointer_cast( - lanelet_map_ptr->regulatoryElementLayer.get(module_id)); + lanelet_map_ptr->regulatoryElementLayer.get(*reg_elem_id)); stop_lines_ = reg_elem_ptr->stopLines(); crosswalk_ = reg_elem_ptr->crosswalkLanelet(); } else { @@ -190,6 +199,9 @@ CrosswalkModule::CrosswalkModule( } crosswalk_ = lanelet_map_ptr->laneletLayer.get(module_id); } + + collision_info_pub_ = + node.create_publisher("~/debug/collision_info", 1); } bool CrosswalkModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) @@ -227,9 +239,12 @@ bool CrosswalkModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto // Calculate stop point with margin const auto p_stop_line = getStopPointWithMargin(*path, path_intersects); - // TODO(murooka) add a guard of p_stop_line - const auto default_stop_pose = toStdOptional( - calcLongitudinalOffsetPose(path->points, p_stop_line->first, p_stop_line->second)); + + std::optional default_stop_pose = std::nullopt; + if (p_stop_line.has_value()) { + default_stop_pose = toStdOptional( + calcLongitudinalOffsetPose(path->points, p_stop_line->first, p_stop_line->second)); + } // Resample path sparsely for less computation cost constexpr double resample_interval = 4.0; @@ -264,6 +279,10 @@ bool CrosswalkModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto } recordTime(4); + const auto collision_info_msg = + createStringStampedMessage(clock_->now(), module_id_, debug_data_.collision_points); + collision_info_pub_->publish(collision_info_msg); + return true; } @@ -280,8 +299,7 @@ std::optional> CrosswalkModule::get const auto p_stop_lines = getLinestringIntersects( ego_path, lanelet::utils::to2D(stop_line).basicLineString(), ego_pos, 2); if (!p_stop_lines.empty()) { - return std::make_pair( - p_stop_lines.front(), -planner_param_.stop_distance_from_object - base_link2front); + return std::make_pair(p_stop_lines.front(), -base_link2front); } } @@ -301,6 +319,9 @@ std::optional CrosswalkModule::checkStopForCrosswalkUsers( const std::optional & default_stop_pose) { const auto & ego_pos = planner_data_->current_odometry->pose.position; + const double ego_vel = planner_data_->current_velocity->twist.linear.x; + const double ego_acc = planner_data_->current_acceleration->accel.accel.linear.x; + const auto base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; const auto dist_ego_to_stop = calcSignedArcLength(ego_path.points, ego_pos, p_stop_line->first) + p_stop_line->second; @@ -316,11 +337,18 @@ std::optional CrosswalkModule::checkStopForCrosswalkUsers( dist_ego_to_stop, sparse_resample_path, crosswalk_attention_range, attention_area); // Check if ego moves forward enough to ignore yield. + const auto & p = planner_param_; if (!path_intersects.empty()) { const double base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; const double dist_ego2crosswalk = calcSignedArcLength(ego_path.points, ego_pos, path_intersects.front()); - if (dist_ego2crosswalk - base_link2front < planner_param_.max_offset_to_crosswalk_for_yield) { + const auto braking_distance_opt = motion_utils::calcDecelDistWithJerkAndAccConstraints( + ego_vel, 0.0, ego_acc, p.min_acc_for_no_stop_decision, p.max_jerk_for_no_stop_decision, + p.min_jerk_for_no_stop_decision); + const double braking_distance = braking_distance_opt ? *braking_distance_opt : 0.0; + if ( + dist_ego2crosswalk - base_link2front - braking_distance < + p.max_offset_to_crosswalk_for_yield) { return {}; } } @@ -577,37 +605,56 @@ std::optional CrosswalkModule::getCollisionPoint( double minimum_stop_dist = std::numeric_limits::max(); std::optional nearest_collision_point{std::nullopt}; for (const auto & obj_path : object.kinematics.predicted_paths) { - for (size_t i = 0; i < obj_path.path.size() - 1; ++i) { - const auto & p_obj_front = obj_path.path.at(i); - const auto & p_obj_back = obj_path.path.at(i + 1); - const auto obj_one_step_polygon = createOneStepPolygon(p_obj_front, p_obj_back, obj_polygon); + size_t start_idx{0}; + bool is_start_idx_initialized{false}; + for (size_t i = 0; i < obj_path.path.size(); ++i) { + // For effective computation, the point and polygon intersection is calculated first. + const auto obj_one_step_polygon = createMultiStepPolygon(obj_path.path, obj_polygon, i, i); + const auto one_step_intersection_polygons = + calcOverlappingPoints(obj_one_step_polygon, attention_area); + if (!one_step_intersection_polygons.empty()) { + if (!is_start_idx_initialized) { + start_idx = i; + is_start_idx_initialized = true; + } + if (i != obj_path.path.size() - 1) { + // NOTE: Even if the object path does not fully cross the ego path, the path should be + // considered. + continue; + } + } + + if (!is_start_idx_initialized) { + continue; + } + + // Calculate multi-step object polygon, and reset start_idx + const size_t start_idx_with_margin = std::max(static_cast(start_idx) - 1, 0); + const size_t end_idx_with_margin = std::min(i + 1, obj_path.path.size() - 1); + const auto obj_multi_step_polygon = createMultiStepPolygon( + obj_path.path, obj_polygon, start_idx_with_margin, end_idx_with_margin); + is_start_idx_initialized = false; // Calculate intersection points between object and attention area - const auto tmp_intersection = calcOverlappingPoints(obj_one_step_polygon, attention_area); - if (tmp_intersection.empty()) { + const auto multi_step_intersection_polygons = + calcOverlappingPoints(obj_multi_step_polygon, attention_area); + if (multi_step_intersection_polygons.empty()) { continue; } // Calculate nearest collision point among collisions with a predicted path - double local_minimum_stop_dist = std::numeric_limits::max(); - geometry_msgs::msg::Point local_nearest_collision_point{}; - for (const auto & p : tmp_intersection) { - const auto cp = createPoint(p.x(), p.y(), ego_pos.z); - const auto dist_ego2cp = calcSignedArcLength(ego_path.points, ego_pos, cp); - - if (dist_ego2cp < local_minimum_stop_dist) { - local_minimum_stop_dist = dist_ego2cp; - local_nearest_collision_point = cp; - } - } + Point2d boost_intersection_center_point; + bg::centroid(multi_step_intersection_polygons.front(), boost_intersection_center_point); + const auto intersection_center_point = createPoint( + boost_intersection_center_point.x(), boost_intersection_center_point.y(), ego_pos.z); const auto dist_ego2cp = - calcSignedArcLength(ego_path.points, ego_pos, local_nearest_collision_point); + calcSignedArcLength(ego_path.points, ego_pos, intersection_center_point); constexpr double eps = 1e-3; const auto dist_obj2cp = calcArcLength(obj_path.path) < eps ? 0.0 - : calcSignedArcLength(obj_path.path, size_t(0), local_nearest_collision_point); + : calcSignedArcLength(obj_path.path, size_t(0), intersection_center_point); if ( dist_ego2cp < crosswalk_attention_range.first || @@ -619,8 +666,9 @@ std::optional CrosswalkModule::getCollisionPoint( if (dist_ego2cp < minimum_stop_dist) { minimum_stop_dist = dist_ego2cp; nearest_collision_point = createCollisionPoint( - local_nearest_collision_point, dist_ego2cp, dist_obj2cp, ego_vel, obj_vel); - debug_data_.obj_polygons.push_back(toGeometryPointVector(obj_one_step_polygon, ego_pos.z)); + intersection_center_point, dist_ego2cp, dist_obj2cp, ego_vel, obj_vel); + debug_data_.obj_polygons.push_back( + toGeometryPointVector(obj_multi_step_polygon, ego_pos.z)); } break; @@ -710,15 +758,15 @@ Polygon2d CrosswalkModule::getAttentionArea( { const auto & ego_pos = planner_data_->current_odometry->pose.position; const auto ego_polygon = createVehiclePolygon(planner_data_->vehicle_info_); + const auto backward_path_length = + calcSignedArcLength(sparse_resample_path.points, size_t(0), ego_pos); + const auto length_sum = calcSignedArcLengthPartialSum( + sparse_resample_path.points, size_t(0), sparse_resample_path.points.size()); Polygon2d attention_area; for (size_t j = 0; j < sparse_resample_path.points.size() - 1; ++j) { - const auto & p_ego_front = sparse_resample_path.points.at(j).point.pose; - const auto & p_ego_back = sparse_resample_path.points.at(j + 1).point.pose; - const auto front_length = - calcSignedArcLength(sparse_resample_path.points, ego_pos, p_ego_front.position); - const auto back_length = - calcSignedArcLength(sparse_resample_path.points, ego_pos, p_ego_back.position); + const auto front_length = length_sum.at(j) - backward_path_length; + const auto back_length = length_sum.at(j + 1) - backward_path_length; if (back_length < crosswalk_attention_range.first) { continue; @@ -728,7 +776,8 @@ Polygon2d CrosswalkModule::getAttentionArea( break; } - const auto ego_one_step_polygon = createOneStepPolygon(p_ego_front, p_ego_back, ego_polygon); + const auto ego_one_step_polygon = + createMultiStepPolygon(sparse_resample_path.points, ego_polygon, j, j + 1); debug_data_.ego_polygons.push_back(toGeometryPointVector(ego_one_step_polygon, ego_pos.z)); @@ -878,11 +927,12 @@ void CrosswalkModule::updateObjectState( getCollisionPoint(sparse_resample_path, object, crosswalk_attention_range, attention_area); object_info_manager_.update( obj_uuid, obj_pos, std::hypot(obj_vel.x, obj_vel.y), clock_->now(), is_ego_yielding, - has_traffic_light, collision_point, planner_param_); + has_traffic_light, collision_point, planner_param_, crosswalk_.polygon2d().basicPolygon()); if (collision_point) { const auto collision_state = object_info_manager_.getCollisionState(obj_uuid); - debug_data_.collision_points.push_back(std::make_pair(*collision_point, collision_state)); + debug_data_.collision_points.push_back( + std::make_tuple(obj_uuid, *collision_point, collision_state)); } } object_info_manager_.finalize(); @@ -894,28 +944,27 @@ bool CrosswalkModule::isRedSignalForPedestrians() const crosswalk_.regulatoryElementsAs(); for (const auto & traffic_lights_reg_elem : traffic_lights_reg_elems) { - lanelet::ConstLineStringsOrPolygons3d traffic_lights = traffic_lights_reg_elem->trafficLights(); - for (const auto & traffic_light : traffic_lights) { - const auto ll_traffic_light = static_cast(traffic_light); - const auto traffic_signal_stamped = planner_data_->getTrafficSignal(ll_traffic_light.id()); - if (!traffic_signal_stamped) { - continue; - } + const auto traffic_signal_stamped = + planner_data_->getTrafficSignal(traffic_lights_reg_elem->id()); + if (!traffic_signal_stamped) { + continue; + } - if ( - planner_param_.traffic_light_state_timeout < - (clock_->now() - traffic_signal_stamped->header.stamp).seconds()) { - continue; - } + if ( + planner_param_.traffic_light_state_timeout < + (clock_->now() - traffic_signal_stamped->stamp).seconds()) { + continue; + } - const auto & lights = traffic_signal_stamped->signal.lights; - if (lights.empty()) { - continue; - } + const auto & lights = traffic_signal_stamped->signal.elements; + if (lights.empty()) { + continue; + } - if (lights.front().color == TrafficLight::RED) { + for (const auto & element : lights) { + if ( + element.color == TrafficSignalElement::RED && element.shape == TrafficSignalElement::CIRCLE) return true; - } } } @@ -1028,6 +1077,8 @@ void CrosswalkModule::setDistanceToStop( const auto & ego_pos = planner_data_->current_odometry->pose.position; const double dist_ego2stop = calcSignedArcLength(ego_path.points, ego_pos, *stop_pos); setDistance(dist_ego2stop); + } else { + setDistance(std::numeric_limits::lowest()); } } diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp index 961126bf4bca1..0768101179857 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp @@ -19,24 +19,26 @@ #include #include -#include -#include #include -#include +#include +#include #include #include +#include #include #include -#include -#include -#include +#include +#include +#include +#include #include #include #include +#include #include #include #include @@ -46,11 +48,12 @@ namespace behavior_velocity_planner { +namespace bg = boost::geometry; using autoware_auto_perception_msgs::msg::ObjectClassification; using autoware_auto_perception_msgs::msg::PredictedObject; using autoware_auto_perception_msgs::msg::PredictedObjects; -using autoware_auto_perception_msgs::msg::TrafficLight; using autoware_auto_planning_msgs::msg::PathWithLaneId; +using autoware_perception_msgs::msg::TrafficSignalElement; using lanelet::autoware::Crosswalk; using tier4_api_msgs::msg::CrosswalkStatus; using tier4_autoware_utils::Polygon2d; @@ -72,6 +75,33 @@ double interpolateEgoPassMargin( } return y_vec.back(); } + +double InterpolateMap( + const std::vector & key_map, const std::vector & value_map, const double query) +{ + // If the speed is out of range of the key_map, apply zero-order hold. + if (query <= key_map.front()) { + return value_map.front(); + } + if (query >= key_map.back()) { + return value_map.back(); + } + + // Apply linear interpolation + for (size_t i = 0; i < key_map.size() - 1; ++i) { + if (key_map.at(i) <= query && query <= key_map.at(i + 1)) { + auto ratio = (query - key_map.at(i)) / std::max(key_map.at(i + 1) - key_map.at(i), 1.0e-5); + ratio = std::clamp(ratio, 0.0, 1.0); + const auto interp = value_map.at(i) + ratio * (value_map.at(i + 1) - value_map.at(i)); + return interp; + } + } + + std::cerr << "Crosswalk's InterpolateMap interpolation logic is broken." + "Please check the code." + << std::endl; + return value_map.back(); +} } // namespace class CrosswalkModule : public SceneModuleInterface @@ -105,11 +135,15 @@ class CrosswalkModule : public SceneModuleInterface std::vector ego_pass_later_margin_y; double ego_pass_later_additional_margin; double max_offset_to_crosswalk_for_yield; + double min_acc_for_no_stop_decision; + double max_jerk_for_no_stop_decision; + double min_jerk_for_no_stop_decision; double stop_object_velocity; double min_object_velocity; bool disable_stop_for_yield_cancel; bool disable_yield_for_new_stopped_object; - double timeout_no_intention_to_walk; + std::vector distance_map_for_no_intention_to_walk; + std::vector timeout_map_for_no_intention_to_walk; double timeout_ego_stop_for_yield; // param for input data double traffic_light_state_timeout; @@ -130,9 +164,10 @@ class CrosswalkModule : public SceneModuleInterface std::optional collision_point{}; void transitState( - const rclcpp::Time & now, const double vel, const bool is_ego_yielding, - const bool has_traffic_light, const std::optional & collision_point, - const PlannerParam & planner_param) + const rclcpp::Time & now, const geometry_msgs::msg::Point & position, const double vel, + const bool is_ego_yielding, const bool has_traffic_light, + const std::optional & collision_point, const PlannerParam & planner_param, + const lanelet::BasicPolygon2d & crosswalk_polygon) { const bool is_stopped = vel < planner_param.stop_object_velocity; @@ -145,8 +180,13 @@ class CrosswalkModule : public SceneModuleInterface if (!time_to_start_stopped) { time_to_start_stopped = now; } + const double distance_to_crosswalk = + bg::distance(crosswalk_polygon, lanelet::BasicPoint2d(position.x, position.y)); + const double timeout_no_intention_to_walk = InterpolateMap( + planner_param.distance_map_for_no_intention_to_walk, + planner_param.timeout_map_for_no_intention_to_walk, distance_to_crosswalk); const bool intent_to_cross = - (now - *time_to_start_stopped).seconds() < planner_param.timeout_no_intention_to_walk; + (now - *time_to_start_stopped).seconds() < timeout_no_intention_to_walk; if ( (is_ego_yielding || (has_traffic_light && planner_param.disable_stop_for_yield_cancel)) && !intent_to_cross) { @@ -201,7 +241,8 @@ class CrosswalkModule : public SceneModuleInterface void update( const std::string & uuid, const geometry_msgs::msg::Point & position, const double vel, const rclcpp::Time & now, const bool is_ego_yielding, const bool has_traffic_light, - const std::optional & collision_point, const PlannerParam & planner_param) + const std::optional & collision_point, const PlannerParam & planner_param, + const lanelet::BasicPolygon2d & crosswalk_polygon) { // update current uuids current_uuids_.push_back(uuid); @@ -219,7 +260,8 @@ class CrosswalkModule : public SceneModuleInterface // update object state objects.at(uuid).transitState( - now, vel, is_ego_yielding, has_traffic_light, collision_point, planner_param); + now, position, vel, is_ego_yielding, has_traffic_light, collision_point, planner_param, + crosswalk_polygon); objects.at(uuid).collision_point = collision_point; objects.at(uuid).position = position; } @@ -257,8 +299,8 @@ class CrosswalkModule : public SceneModuleInterface }; CrosswalkModule( - const int64_t module_id, const lanelet::LaneletMapPtr & lanelet_map_ptr, - const PlannerParam & planner_param, const bool use_regulatory_element, + rclcpp::Node & node, const int64_t module_id, const std::optional & reg_elem_id, + const lanelet::LaneletMapPtr & lanelet_map_ptr, const PlannerParam & planner_param, const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock); bool modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) override; @@ -360,6 +402,8 @@ class CrosswalkModule : public SceneModuleInterface const int64_t module_id_; + rclcpp::Publisher::SharedPtr collision_info_pub_; + lanelet::ConstLanelet crosswalk_; lanelet::ConstLineStrings3d stop_lines_; diff --git a/planning/behavior_velocity_crosswalk_module/src/util.cpp b/planning/behavior_velocity_crosswalk_module/src/util.cpp index 72c091cd8bc85..47bbef7111da0 100644 --- a/planning/behavior_velocity_crosswalk_module/src/util.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/util.cpp @@ -15,7 +15,11 @@ #include "behavior_velocity_crosswalk_module/util.hpp" #include -#include +#include +#include +#include +#include +#include #include @@ -25,6 +29,8 @@ #include #include +#include + #include #include #include @@ -35,9 +41,8 @@ #include #include #include -#include -#include +#include #include namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_detection_area_module/config/detection_area.param.yaml b/planning/behavior_velocity_detection_area_module/config/detection_area.param.yaml index b5d075a4d6493..62b5f2458336f 100644 --- a/planning/behavior_velocity_detection_area_module/config/detection_area.param.yaml +++ b/planning/behavior_velocity_detection_area_module/config/detection_area.param.yaml @@ -8,4 +8,4 @@ state_clear_time: 2.0 hold_stop_margin_distance: 0.0 distance_to_judge_over_stop_line: 0.5 - enable_rtc: true # If set to true, the scene modules require approval from the rtc (request to cooperate) function. If set to false, the modules can be executed without requiring rtc approval + enable_rtc: false # If set to true, the scene modules require approval from the rtc (request to cooperate) function. If set to false, the modules can be executed without requiring rtc approval diff --git a/planning/behavior_velocity_detection_area_module/src/debug.cpp b/planning/behavior_velocity_detection_area_module/src/debug.cpp index 60479d5adfd88..ee2af54e5ea2a 100644 --- a/planning/behavior_velocity_detection_area_module/src/debug.cpp +++ b/planning/behavior_velocity_detection_area_module/src/debug.cpp @@ -16,7 +16,9 @@ #include #include -#include +#include +#include +#include #ifdef ROS_DISTRO_GALACTIC #include diff --git a/planning/behavior_velocity_detection_area_module/src/manager.cpp b/planning/behavior_velocity_detection_area_module/src/manager.cpp index 99888d0789559..e7ec6b37f7f20 100644 --- a/planning/behavior_velocity_detection_area_module/src/manager.cpp +++ b/planning/behavior_velocity_detection_area_module/src/manager.cpp @@ -14,7 +14,9 @@ #include "manager.hpp" +#include #include +#include #include @@ -29,22 +31,24 @@ namespace behavior_velocity_planner { using lanelet::autoware::DetectionArea; +using tier4_autoware_utils::getOrDeclareParameter; DetectionAreaModuleManager::DetectionAreaModuleManager(rclcpp::Node & node) : SceneModuleManagerInterfaceWithRTC( node, getModuleName(), - node.declare_parameter(std::string(getModuleName()) + ".enable_rtc")) + getOrDeclareParameter(node, std::string(getModuleName()) + ".enable_rtc")) { const std::string ns(getModuleName()); - planner_param_.stop_margin = node.declare_parameter(ns + ".stop_margin"); - planner_param_.use_dead_line = node.declare_parameter(ns + ".use_dead_line"); - planner_param_.dead_line_margin = node.declare_parameter(ns + ".dead_line_margin"); - planner_param_.use_pass_judge_line = node.declare_parameter(ns + ".use_pass_judge_line"); - planner_param_.state_clear_time = node.declare_parameter(ns + ".state_clear_time"); + planner_param_.stop_margin = getOrDeclareParameter(node, ns + ".stop_margin"); + planner_param_.use_dead_line = getOrDeclareParameter(node, ns + ".use_dead_line"); + planner_param_.dead_line_margin = getOrDeclareParameter(node, ns + ".dead_line_margin"); + planner_param_.use_pass_judge_line = + getOrDeclareParameter(node, ns + ".use_pass_judge_line"); + planner_param_.state_clear_time = getOrDeclareParameter(node, ns + ".state_clear_time"); planner_param_.hold_stop_margin_distance = - node.declare_parameter(ns + ".hold_stop_margin_distance"); + getOrDeclareParameter(node, ns + ".hold_stop_margin_distance"); planner_param_.distance_to_judge_over_stop_line = - node.declare_parameter(ns + ".distance_to_judge_over_stop_line"); + getOrDeclareParameter(node, ns + ".distance_to_judge_over_stop_line"); } void DetectionAreaModuleManager::launchNewModules( diff --git a/planning/behavior_velocity_detection_area_module/src/scene.cpp b/planning/behavior_velocity_detection_area_module/src/scene.cpp index 53a6deb6f26a4..4d635d3d6d641 100644 --- a/planning/behavior_velocity_detection_area_module/src/scene.cpp +++ b/planning/behavior_velocity_detection_area_module/src/scene.cpp @@ -17,13 +17,14 @@ #include #include #include - #ifdef ROS_DISTRO_GALACTIC #include #else #include #endif +#include + #include #include #include diff --git a/planning/behavior_velocity_intersection_module/README.md b/planning/behavior_velocity_intersection_module/README.md index 3e62ab75d23cd..16459bb64074b 100644 --- a/planning/behavior_velocity_intersection_module/README.md +++ b/planning/behavior_velocity_intersection_module/README.md @@ -24,7 +24,7 @@ This module is activated when the path contains the lanes with `turn_direction` ### Attention area -The `Attention Area` in the intersection are defined as the set of lanes that are conflicting with ego vehicle's path and their preceding lanes up to `attention_area_length` meters. `RightOfWay` tag is used to rule out the lanes that each lane has priority given the traffic light relation and `turn_direction` priority. +The `attention area` in the intersection are defined as the set of lanes that are conflicting with ego vehicle's path and their preceding lanes up to `common.attention_area_length` meters. `RightOfWay` tag is used to rule out the lanes that each lane has priority given the traffic light relation and `turn_direction` priority(`yield lane`). `Intersection Area`, which is supposed to be defined on the HDMap, is an area converting the entire intersection. @@ -54,45 +54,115 @@ For [stuck vehicle detection](#stuck-vehicle-detection) and [collision detection Objects that satisfy all of the following conditions are considered as target objects (possible collision objects): -- The center of mass of the object is **within a certain distance** from the attention lane (threshold = `attention_area_margin`) . +- The center of mass of the object is **within a certain distance** from the attention lane (threshold = `common.attention_area_margin`) . - (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 = `attention_area_angle_threshold`). +- The posture of object is **the same direction as the attention lane** (threshold = `common.attention_area_angle_threshold`). - Not being **in the adjacent lanes of the ego vehicle**. #### Stuck Vehicle Detection -If there is any object in a certain distance (`stuck_vehicle_ignore_dist` and `stuck_vehicle_detect_dist`) before and after the exit of the intersection lane and the object velocity is less than a threshold (=`stuck_vehicle_vel_thr`), the object is regarded as a stuck vehicle. If stuck vehicles exist, this module inserts a stopline a certain distance (=`stop_line_margin`) before the overlapped region with other lanes. The stuck vehicle detection area is generated based on the vehicle path, so the stuck vehicle stopline is not inserted if the upstream module generated avoidance path +If there is any object on the path in inside the intersection and at the exit of the intersection (up to `stuck_vehicle_detect_dist`) lane and its velocity is less than a threshold (`stuck_vehicle.stuck_vehicle_vel_thr`), the object is regarded as a stuck vehicle. If stuck vehicles exist, this module inserts a stopline a certain distance (=`stop_line_margin`) before the overlapped region with other lanes. The stuck vehicle detection area is generated based on the vehicle path, so the stuck vehicle stopline is not inserted if the upstream module generated avoidance path ![stuck_vehicle_detection](./docs/stuck-vehicle.drawio.svg) #### Collision detection -The following process is performed for the targets objects to determine whether the ego vehicle can pass the intersection safely. If it is judged that the ego vehicle cannot pass through the intersection with enough _margin_, it will insert a stopline on the _path_. +The following process is performed for the targets objects to determine whether the ego vehicle can pass the intersection safely. If it is judged that the ego vehicle cannot pass through the intersection with enough margin, this module inserts a stopline on the path. 1. calculate the time interval that the ego vehicle is in the intersection. This time is set as $t_s$ ~ $t_e$ -2. extract the predicted path of the target object whose confidence is greater than `min_predicted_path_confidence`. +2. extract the predicted path of the target object whose confidence is greater than `collision_detection.min_predicted_path_confidence`. 3. detect collision between the extracted predicted path and ego's predicted path in the following process. 1. obtain the passing area of the ego vehicle $A_{ego}$ in $t_s$ ~ $t_e$. - 2. calculate the passing area of the target object $A_{target}$ at $t_s$ - `collision_start_margin_time` ~ $t_e$ + `collision_end_margin_time` for each predicted path (\*1). + 2. calculate the passing area of the target object $A_{target}$ at $t_s$ - `collision_detection.collision_start_margin_time` ~ $t_e$ + `collision_detection.collision_end_margin_time` for each predicted path (\*1). 3. check if $A_{ego}$ and $A_{target}$ polygons are overlapped (has collision). 4. when a collision is detected, the module inserts a stopline. 5. If ego is over the `pass_judge_line`, collision checking is not processed to avoid sudden braking and/or unnecessary stop in the middle of the intersection. -The parameters `collision_start_margin_time` and `collision_end_margin_time` can be interpreted as follows: +The parameters `collision_detection.collision_start_margin_time` and `collision_detection.collision_end_margin_time` can be interpreted as follows: -- If the ego vehicle was to pass through the intersection earlier than the target object, collision would be detected if the time difference between the two was less than `collision_start_margin_time`. -- If the ego vehicle was to pass through the intersection later than the target object, collision would be detected if the time difference between the two was less than `collision_end_margin_time`. +- If the ego vehicle was to pass through the intersection earlier than the target object, collision would be detected if the time difference between the two was less than `collision_detection.collision_start_margin_time`. +- If the ego vehicle was to pass through the intersection later than the target object, collision would be detected if the time difference between the two was less than `collision_detection.collision_end_margin_time`. -If collision is detected, the state transits to "STOP" immediately. On the other hand, the state does not transit to "GO" unless _safe_ judgement continues for a certain period to prevent the chattering of decisions. +If collision is detected, the state transits to "STOP" immediately. On the other hand, the state does not transit to "GO" unless safe judgement continues for a certain period `collision_detection.state_transit_margin` to prevent the chattering of decisions. #### Stop Line Automatic Generation -If a stopline is associated with the intersection lane, that line is used as the stopline for collision detection. Otherwise the path is interpolated at a certain intervals (=`path_interpolation_ds`), and the point which is `stop_line_margin` meters behind the attention lane is defined as the position of the stop line for the vehicle front. +If a stopline is associated with the intersection lane on the map, that line is used as the stopline for collision detection. Otherwise the path is interpolated at a certain intervals (=`common.path_interpolation_ds`), and the point which is `stop_line_margin` meters behind the attention area is defined as the position of the stop line for the vehicle front. #### Pass Judge Line -To avoid a rapid braking, if deceleration and jerk more than a threshold (`behavior_velocity_planner.max_accel` and `behavior_velocity_planner.max_jerk`) are needed to stop just in front of the attention area, this module does not insert stopline after passing the default stopline position. +To avoid sudden braking, if deceleration and jerk more than a threshold (`behavior_velocity_planner.max_accel` and `behavior_velocity_planner.max_jerk`) is required to stop just in front of the attention area, this module does not insert stopline after passing the default stopline position. + +### Occlusion detection + +If the flag `occlusion.enable` is true this module checks if there is sufficient field of view (FOV) on the attention area up to `occlusion.occlusion_attention_area_length`. If FOV is not clear enough the ego vehicle will once stop at the _default stop line_ for `occlusion.before_creep_stop_time`, and then slowly creep toward _occlusion peeking stop line_ at the speed of `occlusion.occlusion_creep_velocity` if `occlusion.enable_creeping` is true. During the creeping if collision is detected this module inserts a stop line instantly, and if the FOV gets sufficiently clear the _intersection occlusion_ wall will disappear. If occlusion is cleared and no collision is detected the ego vehicle will pass the intersection. + +The occlusion is detected as the common area of occlusion attention area(which is partially the same as the normal attention area) and the unknown cells of the occupancy grid map. The occupancy grid map is denoised using morphology with the window size of `occlusion.denoise_kernel`. The occlusion attention area lanes are discretized to line strings and they are used to generate a grid whose each cell represents the distance from the ego path along the lane as shown below. + +![occlusion_detection](./docs/occlusion_grid.drawio.svg) + +If the nearest occlusion cell value is below the threshold, occlusion is detected. It is expected that the occlusion gets cleared as the vehicle approaches the occlusion peeking stop line. + +In there are no traffic lights associated with the lane, the ego vehicle will make a brief stop at the _default stop line_ and the position where the vehicle heading touches the attention area for the first time(which is denoted as _first attention stop line_). After stopping at the _first attention area stop line_ this module inserts `occlusion.absence_traffic_light.creep_velocity` velocity between ego and the position which is `occlusion.absence_traffic_light.maximum_peeking_distance` ahead of _first attention area stop line_ while occlusion is not cleared. If collision is detected, ego will instantly stop. Once the occlusion is cleared or ego passed `occlusion.absence_traffic_light.maximum_peeking_distance` this module does not detect collision and occlusion because ego vehicle is already inside the intersection. + +![occlusion_detection](./docs/occlusion-without-tl.drawio.svg) + +### Data Structure + +#### `IntersectionLanelets` + +```plantuml +@startuml +entity IntersectionLanelets { + * conflicting lanes/area + -- + * first conflicting area + The conflicting lane area which the path intersects first + -- + * attention lanes/area + -- + * first attention lane area + The attention lane area which the path intersects first + -- + * occlusion attention lanes/area + Part of attention lanes/area for occlusion detection + -- + * is_priortized: bool + If ego vehicle has priority in current traffic light context +} +@enduml +``` + +#### `IntersectionStopLines` + +Each stop lines are generated from interpolated path points to obtain precise positions. + +```plantuml +@startuml +entity IntersectionStopLines { + * closest_idx: size_t + closest path point index for ego vehicle + -- + * stuck_stop_line: size_t + stop line index on stuck vehicle detection + -- + * default_stop_line: size_t + If defined on the map, its index on the path. Otherwise generated before first_attention_stop_line + -- + * first_attention_stop_line + The index of the first path point which is inside the attention area + -- + * occlusion_peeking_stop_line + The index of the path point for the peeking limit position + -- + * pass_judge_line + The index of the path point which is before first_attention_stop_line/occlusion_peeking_stop_line by braking distance +} +@enduml +``` + +![data structure](./docs/data-structure.drawio.svg) ### Module Parameters @@ -105,9 +175,7 @@ To avoid a rapid braking, if deceleration and jerk more than a threshold (`behav | `common.intersection_velocity` | double | [m/s] velocity profile for pass judge calculation | | `common.intersection_max_accel` | double | [m/s^2] acceleration profile for pass judge calculation | | `common.stop_overshoot_margin` | double | [m] margin for the overshoot from stopline | -| `common.path_interpolation_ds` | double | [m] path interpolation interval | | `stuck_vehicle.stuck_vehicle_detect_dist` | double | [m] length toward from the exit of intersection for stuck vehicle detection | -| `stuck_vehicle.stuck_vehicle_ignore_dist` | double | [m] length behind the exit of intersection for stuck vehicle detection | | `stuck_vehicle.stuck_vehicle_vel_thr` | double | [m/s] velocity threshold for stuck vehicle detection | | `collision_detection.state_transit_margin_time` | double | [m] time margin to change state | | `collision_detection.min_predicted_path_confidence` | double | [-] minimum confidence value of predicted path to use for collision detection | @@ -119,7 +187,13 @@ To avoid a rapid braking, if deceleration and jerk more than a threshold (`behav | `occlusion.peeking_offset` | double | [m] the offset of the front of the vehicle into the attention area for peeking to occlusion | | `occlusion.min_vehicle_brake_for_rss` | double | [m/s] assumed minimum brake of the vehicle running from behind the occlusion | | `occlusion.max_vehicle_velocity_for_rss` | double | [m/s] assumed maximum velocity of the vehicle running from behind the occlusion | -| `occlusion.denoise_kernel` | double | [m] the window size of morphology process for clearing noisy occlusion | + +#### For developers only + +| Parameter | Type | Description | +| ------------------------------ | ------ | ---------------------------------------------------------------------- | +| `common.path_interpolation_ds` | double | [m] path interpolation interval | +| `occlusion.denoise_kernel` | double | [m] the window size of morphology process for clearing noisy occlusion | ### How to turn parameters diff --git a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml index 2b3ca11bbe750..82a5f65622d0b 100644 --- a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml +++ b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml @@ -2,7 +2,7 @@ ros__parameters: intersection: common: - attention_area_margin: 0.5 # [m] + attention_area_margin: 0.75 # [m] attention_area_length: 200.0 # [m] attention_area_angle_threshold: 0.785 # [rad] stop_line_margin: 3.0 @@ -12,26 +12,52 @@ use_intersection_area: false path_interpolation_ds: 0.1 # [m] consider_wrong_direction_vehicle: false + max_accel: -2.8 + max_jerk: -5.0 + delay_response_time: 0.5 stuck_vehicle: + turn_direction: + left: true + right: true + straight: true use_stuck_stopline: true # stopline generated before the first conflicting area - stuck_vehicle_detect_dist: 3.0 # this should be the length between cars when they are stopped. The actual stuck vehicle detection length will be this value + vehicle_length. - stuck_vehicle_ignore_dist: 10.0 # obstacle stop max distance(5.0m) + stuck vehicle size / 2 (0.0m-) + stuck_vehicle_detect_dist: 5.0 # this should be the length between cars when they are stopped. The actual stuck vehicle detection length will be this value + vehicle_length. stuck_vehicle_vel_thr: 0.833 # 0.833m/s = 3.0km/h # enable_front_car_decel_prediction: false # By default this feature is disabled # assumed_front_car_decel: 1.0 # [m/ss] the expected deceleration of front car when front car as well as ego are turning timeout_private_area: 3.0 # [s] cancel stuck vehicle stop in private area + enable_private_area_stuck_disregard: false #In the intersections which labeled as "private area", the obstacle vehicles judged as "stuck" are neglected if this param is set as true. + yield_stuck: + turn_direction: + left: true + right: false + straight: false + distance_thr: 1.0 # [m] collision_detection: state_transit_margin_time: 1.0 min_predicted_path_confidence: 0.05 minimum_ego_predicted_velocity: 1.388 # [m/s] - normal: - collision_start_margin_time: 4.0 # [s] this + state_transit_margin_time should be higher to account for collision with fast/accelerating object - collision_end_margin_time: 6.0 # [s] this + state_transit_margin_time should be higher to account for collision with slow/decelerating object - relaxed: + fully_prioritized: collision_start_margin_time: 2.0 collision_end_margin_time: 0.0 + partially_prioritized: + collision_start_margin_time: 2.0 + collision_end_margin_time: 2.0 + not_prioritized: + collision_start_margin_time: 4.0 # [s] this + state_transit_margin_time should be higher to account for collision with fast/accelerating object + collision_end_margin_time: 6.0 # [s] this + state_transit_margin_time should be higher to account for collision with slow/decelerating object keep_detection_vel_thr: 0.833 # == 3.0km/h. keep detection if ego is ego.vel < keep_detection_vel_thr + use_upstream_velocity: true # flag to use the planned velocity profile from the upstream module + minimum_upstream_velocity: 0.01 # [m/s] minimum velocity to avoid null division for the stop line from the upstream velocity + yield_on_green_traffic_light: + distance_to_assigned_lanelet_start: 5.0 + duration: 2.0 + object_dist_to_stopline: 10.0 # [m] + ignore_on_amber_traffic_light: + object_expected_deceleration: 2.0 # [m/ss] + ignore_on_red_traffic_light: + object_margin_to_path: 2.0 occlusion: enable: false @@ -46,13 +72,22 @@ min_vehicle_brake_for_rss: -2.5 # [m/s^2] max_vehicle_velocity_for_rss: 16.66 # [m/s] == 60kmph denoise_kernel: 1.0 # [m] - possible_object_bbox: [1.0, 2.0] # [m x m] - ignore_parked_vehicle_speed_threshold: 0.333 # == 1.2km/h + possible_object_bbox: [1.5, 2.5] # [m x m] + ignore_parked_vehicle_speed_threshold: 0.8333 # == 3.0km/h + stop_release_margin_time: 1.5 # [s] + temporal_stop_before_attention_area: false + absence_traffic_light: + creep_velocity: 1.388 # [m/s] + maximum_peeking_distance: 6.0 # [m] + attention_lane_crop_curvature_threshold: 0.25 + attention_lane_curvature_calculation_ds: 0.5 + static_occlusion_with_traffic_light_timeout: 3.5 - enable_rtc: # If set to true, the scene modules require approval from the rtc (request to cooperate) function. If set to false, the modules can be executed without requiring rtc approval - intersection: true - intersection_to_occlusion: true + enable_rtc: + intersection: false # If set to true, the scene modules require approval from the rtc (request to cooperate) function. If set to false, the modules can be executed without requiring rtc approval + intersection_to_occlusion: false merge_from_private: stop_line_margin: 3.0 stop_duration_sec: 1.0 + stop_distance_threshold: 1.0 diff --git a/planning/behavior_velocity_intersection_module/docs/data-structure.drawio.svg b/planning/behavior_velocity_intersection_module/docs/data-structure.drawio.svg new file mode 100644 index 0000000000000..fb512902ef856 --- /dev/null +++ b/planning/behavior_velocity_intersection_module/docs/data-structure.drawio.svg @@ -0,0 +1,771 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + closest_idx + +
+
+
+ + closest_idx + + + + + + + + +
+
+
+ + stuck_stop_line + +
+
+
+ + stuck_stop_line + + + + + + + + + + + +
+
+
+ + default_stop_line + +
+
+
+ + default_stop_line + + + + + + + + +
+
+
+ + first_attention_stop_line + +
+
+
+ + first_attention_stop_line + + + + + + + + +
+
+
+ + + occlusion_peeking +
+ _stop_line + + +
+
+
+ + occlusion_peeking... + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + + next + + +
+
+
+ + next + + + + + + + + +
+
+
+ + + prev + + +
+
+
+ + prev + + + + + + + + +
+
+
+ + + ego_or_entry2exit + + +
+
+
+ + ego_or_entry2exit + + + + + + + + +
+
+
+ + + entry2ego + + +
+
+
+ + entry2ego + + + + + + + +
+
+
+ + IntersectionStopLines + +
+
+
+ + IntersectionStopLines + + + + + +
+
+
+ + PathLanelets + +
+
+
+ + PathLanelets + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_velocity_intersection_module/docs/intersection-attention-ll-rr.drawio.svg b/planning/behavior_velocity_intersection_module/docs/intersection-attention-ll-rr.drawio.svg index d4d709eee6b99..51e926fe266d6 100644 --- a/planning/behavior_velocity_intersection_module/docs/intersection-attention-ll-rr.drawio.svg +++ b/planning/behavior_velocity_intersection_module/docs/intersection-attention-ll-rr.drawio.svg @@ -8,12 +8,48 @@ width="3723px" height="2401px" viewBox="-0.5 -0.5 3723 2401" - content="<mxfile host="Electron" modified="2023-06-06T10:28:31.375Z" agent="5.0 (X11; Linux x86_64) AppleWebKit/537.36 (KHTML, like Gecko) draw.io/20.3.0 Chrome/104.0.5112.114 Electron/20.1.3 Safari/537.36" etag="M_P_fA_OZlKNZHzEOQxb" version="20.3.0" type="device"><diagram name="intersection" id="0L5whF3ImEvTl2DSWTjR">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</diagram></mxfile>" + content="<mxfile host="Electron" modified="2023-10-03T02:44:21.616Z" agent="5.0 (X11; Linux x86_64) AppleWebKit/537.36 (KHTML, like Gecko) draw.io/20.3.0 Chrome/104.0.5112.114 Electron/20.1.3 Safari/537.36" etag="wkqIuFIE5UyEHdVd8sjR" version="20.3.0" type="device"><diagram name="intersection" id="0L5whF3ImEvTl2DSWTjR">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</diagram></mxfile>" > + + + + + + + + + - + @@ -51,7 +87,7 @@ stroke-miterlimit="10" pointer-events="none" /> - + - + - + yield lane - - + + @@ -749,10 +785,10 @@ - - + + - + - attention area + attention lane - attention area + attention lane @@ -1141,12 +1177,12 @@
- attention area + attention lane
- attention area + attention lane @@ -1226,9 +1262,9 @@ stroke-miterlimit="10" pointer-events="none" /> - + + + + + + + + + + + + + + + + diff --git a/planning/behavior_velocity_intersection_module/docs/intersection-attention-lr-rl.drawio.svg b/planning/behavior_velocity_intersection_module/docs/intersection-attention-lr-rl.drawio.svg index 2d590b9fd51ca..25a7c6b519f96 100644 --- a/planning/behavior_velocity_intersection_module/docs/intersection-attention-lr-rl.drawio.svg +++ b/planning/behavior_velocity_intersection_module/docs/intersection-attention-lr-rl.drawio.svg @@ -5,10 +5,10 @@ xmlns="http://www.w3.org/2000/svg" xmlns:xlink="http://www.w3.org/1999/xlink" version="1.1" - width="3729px" + width="3727px" height="2651px" - viewBox="-0.5 -0.5 3729 2651" - content="<mxfile host="Electron" modified="2023-06-06T10:27:24.636Z" agent="5.0 (X11; Linux x86_64) AppleWebKit/537.36 (KHTML, like Gecko) draw.io/20.3.0 Chrome/104.0.5112.114 Electron/20.1.3 Safari/537.36" etag="EZYouzMJcjefpLDW8VVp" version="20.3.0" type="device"><diagram name="intersection" id="0L5whF3ImEvTl2DSWTjR">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</diagram></mxfile>" + viewBox="-0.5 -0.5 3727 2651" + content="<mxfile host="Electron" modified="2023-10-03T03:32:56.314Z" agent="5.0 (X11; Linux x86_64) AppleWebKit/537.36 (KHTML, like Gecko) draw.io/20.3.0 Chrome/104.0.5112.114 Electron/20.1.3 Safari/537.36" etag="aYRnTHIqVcSkLQHKGcXP" version="20.3.0" type="device"><diagram name="intersection" id="0L5whF3ImEvTl2DSWTjR">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</diagram></mxfile>" > @@ -25,13 +25,13 @@ transform="rotate(-130,954.7,670.3)" pointer-events="none" /> - - - + + + - + - - - + + + @@ -50,8 +50,7 @@ - - + @@ -88,9 +87,9 @@ stroke-miterlimit="10" pointer-events="none" /> - + - + - + - + - + - + - - - + + + - + - + - + - - - - - + + + + + - + - + - - - + + + - + - + - + - - + + - + - + - + - + @@ -436,8 +435,8 @@ ego lane - - + + @@ -474,16 +473,16 @@
- attention area + attention lane
- attention area + attention lane - - + + @@ -548,12 +547,12 @@ transform="rotate(225,2890.14,695.64)" pointer-events="none" /> - + - - + + - attention area + attention lane - attention area + attention lane - - + + - - - + + + - - + + - + @@ -753,13 +752,13 @@ transform="rotate(-130,954.7,1832.3)" pointer-events="none" /> - - - + + + - + - - - + + + @@ -779,7 +778,7 @@ - + - + - + - + - + - + - + - - - + + + - - - - - - - - + - - + + - - + + - + - + - - - + + + - + - + - + - - + + - + - + - + - + - + @@ -1150,8 +1118,8 @@ ego lane - - + + @@ -1186,16 +1154,16 @@
- attention area + attention lane
- attention area + attention lane - - + + @@ -1224,12 +1192,12 @@ transform="rotate(225,2890.14,1857.64)" pointer-events="none" /> - + - - + + - attention area + attention lane - attention area + attention lane - - - - - + + + + + - - + + - + @@ -1376,6 +1344,494 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/planning/behavior_velocity_intersection_module/docs/intersection-attention-straight.drawio.svg b/planning/behavior_velocity_intersection_module/docs/intersection-attention-straight.drawio.svg index d33674a257d9b..640eba618fa49 100644 --- a/planning/behavior_velocity_intersection_module/docs/intersection-attention-straight.drawio.svg +++ b/planning/behavior_velocity_intersection_module/docs/intersection-attention-straight.drawio.svg @@ -5,41 +5,42 @@ xmlns="http://www.w3.org/2000/svg" xmlns:xlink="http://www.w3.org/1999/xlink" version="1.1" - width="3389px" - height="1411px" - viewBox="-0.5 -0.5 3389 1411" - content="<mxfile host="Electron" modified="2023-06-06T10:31:30.247Z" agent="5.0 (X11; Linux x86_64) AppleWebKit/537.36 (KHTML, like Gecko) draw.io/20.3.0 Chrome/104.0.5112.114 Electron/20.1.3 Safari/537.36" etag="U1JUqxOsfTr30viw8iHe" version="20.3.0" type="device"><diagram name="intersection" id="0L5whF3ImEvTl2DSWTjR">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</diagram></mxfile>" + width="3390px" + height="1412px" + viewBox="-0.5 -0.5 3390 1412" + content="<mxfile host="Electron" modified="2023-10-03T04:48:10.725Z" agent="5.0 (X11; Linux x86_64) AppleWebKit/537.36 (KHTML, like Gecko) draw.io/20.3.0 Chrome/104.0.5112.114 Electron/20.1.3 Safari/537.36" etag="02kGXKyoYgRU_RdR3ghD" version="20.3.0" type="device"><diagram name="intersection" id="0L5whF3ImEvTl2DSWTjR">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</diagram></mxfile>" > + - + + - - + - + - + @@ -390,7 +391,7 @@ - + - + - + - + - + - + - - - + + + - + attention area - - - - + + + + + + + + + + + + + + + + + + + + + diff --git a/planning/behavior_velocity_intersection_module/docs/intersection-attention.drawio.svg b/planning/behavior_velocity_intersection_module/docs/intersection-attention.drawio.svg index 3e9826fd84ad6..57552f586e63b 100644 --- a/planning/behavior_velocity_intersection_module/docs/intersection-attention.drawio.svg +++ b/planning/behavior_velocity_intersection_module/docs/intersection-attention.drawio.svg @@ -6,9 +6,9 @@ xmlns:xlink="http://www.w3.org/1999/xlink" version="1.1" width="3707px" - height="2193px" - viewBox="-0.5 -0.5 3707 2193" - content="<mxfile host="Electron" modified="2023-06-06T08:17:01.045Z" agent="5.0 (X11; Linux x86_64) AppleWebKit/537.36 (KHTML, like Gecko) draw.io/20.3.0 Chrome/104.0.5112.114 Electron/20.1.3 Safari/537.36" etag="0BpzzxhIQKLXKpqq79kr" version="20.3.0" type="device"><diagram name="intersection" id="0L5whF3ImEvTl2DSWTjR">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</diagram></mxfile>" + height="2195px" + viewBox="-0.5 -0.5 3707 2195" + content="<mxfile host="Electron" modified="2023-10-03T05:15:42.124Z" agent="5.0 (X11; Linux x86_64) AppleWebKit/537.36 (KHTML, like Gecko) draw.io/20.3.0 Chrome/104.0.5112.114 Electron/20.1.3 Safari/537.36" etag="WBrLRHcQvF5FOJ4jcZAE" version="20.3.0" type="device"><diagram name="intersection" id="0L5whF3ImEvTl2DSWTjR">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</diagram></mxfile>" > @@ -23,7 +23,7 @@ pointer-events="none" /> - + - + - - + + - + - + - - - + + + @@ -139,7 +139,7 @@ - + @@ -176,9 +176,9 @@ stroke-miterlimit="10" pointer-events="none" /> - + - + - + - + - + - + - - - + + + - + - + - + - - + + - - + + - + - + - - - + + + - + - + - + + + + + + + @@ -589,16 +613,16 @@
- attention area + attention lane
- attention area + attention lane - - + + @@ -654,7 +678,7 @@ stroke-miterlimit="10" pointer-events="none" /> - + - + - - + + - attention area + attention lane - attention area + attention lane - - + + @@ -875,7 +899,7 @@ stroke-miterlimit="10" pointer-events="none" /> - + @@ -990,10 +1014,10 @@ stroke-miterlimit="10" pointer-events="none" /> - - + + - - + + - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ attention area +
+
+
+ + attention area + + + + + + + + +
+
+
+ attention area +
+
+
+ + attention area + + + + diff --git a/planning/behavior_velocity_intersection_module/docs/occlusion-without-tl.drawio.svg b/planning/behavior_velocity_intersection_module/docs/occlusion-without-tl.drawio.svg new file mode 100644 index 0000000000000..2fc22c8a4a401 --- /dev/null +++ b/planning/behavior_velocity_intersection_module/docs/occlusion-without-tl.drawio.svg @@ -0,0 +1,518 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + + + default stop +
+ line + +
+ + +
+
+
+ + default stop... + + + + + + + + +
+
+
+ + occlusion + +
+
+
+ + occlusion + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + + + first attention +
+ stop line + +
+ + +
+
+
+ + first attentio... + + + + + + + + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_velocity_intersection_module/docs/occlusion_grid.drawio.svg b/planning/behavior_velocity_intersection_module/docs/occlusion_grid.drawio.svg new file mode 100644 index 0000000000000..fc09a4212070a --- /dev/null +++ b/planning/behavior_velocity_intersection_module/docs/occlusion_grid.drawio.svg @@ -0,0 +1,2616 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + default stop line + +
+
+
+ + default stop line + + + + + + + + +
+
+
+ + occlusion peeking line + +
+
+
+ + occlusion peeking line + + + + + + + + + + + + + + + + + +
+
+
+ + ego + +
+
+
+ + ego + + + + + + + + +
+
+
+ occlusion attention area +
+
+
+ + occlusion attent... + + + + + + + + + + + +
+
+
+ + + stopped vehicle on attention area +
+ (blocking vehicle) + + +
+
+
+ + stopped vehicle on attentio... + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + + The cells behind blocking vehicles +
+ are not marked as occluded +
+ + +
+
+
+ + The cells behind blocking vehicles... + + + + + + + + +
+
+
+ + + mark the cells which is unknown and +
+ belong to attention area are +
+ marked as occluded + +
+ +
+
+
+ + mark the cells which is unknown and... + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ 13 +
+
+
+ + 13 + + + + + + + +
+
+
+ 12 +
+
+
+ + 12 + + + + + + + +
+
+
+ 12 +
+
+
+ + 12 + + + + + + + +
+
+
+ 11 +
+
+
+ + 11 + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ 12 +
+
+
+ + 12 + + + + + + + +
+
+
+ 11 +
+
+
+ + 11 + + + + + + + +
+
+
+ 11 +
+
+
+ + 11 + + + + + + + +
+
+
+ 10 +
+
+
+ + 10 + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ 11 +
+
+
+ + 11 + + + + + + + +
+
+
+ 10 +
+
+
+ + 10 + + + + + + + +
+
+
+ 10 +
+
+
+ + 10 + + + + + + + +
+
+
+ 9 +
+
+
+ + 9 + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ 10 +
+
+
+ + 10 + + + + + + + +
+
+
+ 9 +
+
+
+ + 9 + + + + + + + +
+
+
+ 9 +
+
+
+ + 9 + + + + + + +
+
+
+ 8 +
+
+
+ + 8 + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ 9 +
+
+
+ + 9 + + + + + + + +
+
+
+ 8 +
+
+
+ + 8 + + + + + + +
+
+
+ 8 +
+
+
+ + 8 + + + + + + +
+
+
+ 7 +
+
+
+ + 7 + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ 8 +
+
+
+ + 8 + + + + + + + +
+
+
+ 7 +
+
+
+ + 7 + + + + + + +
+
+
+ 7 +
+
+
+ + 7 + + + + + + +
+
+
+ 6 +
+
+
+ + 6 + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ 7 +
+
+
+ + 7 + + + + + + +
+
+
+ 6 +
+
+
+ + 6 + + + + + + +
+
+
+ 6 +
+
+
+ + 6 + + + + + + +
+
+
+ 5 +
+
+
+ + 5 + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ 6 +
+
+
+ + 6 + + + + + + +
+
+
+ 5 +
+
+
+ + 5 + + + + + + +
+
+
+ 5 +
+
+
+ + 5 + + + + + + +
+
+
+ 4 +
+
+
+ + 4 + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ 5 +
+
+
+ + 5 + + + + + + +
+
+
+ 4 +
+
+
+ + 4 + + + + + + +
+
+
+ 4 +
+
+
+ + 4 + + + + + + +
+
+
+ 3 +
+
+
+ + 3 + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + 4 +
+
+ +
+
+
+ + 4... + + + + + + +
+
+
+ 3 +
+
+
+ + 3 + + + + + + +
+
+
+ 3 +
+
+
+ + 3 + + + + + + +
+
+
+ 2 +
+
+
+ + 2 + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ 3 +
+
+
+ + 3 + + + + + + +
+
+
+ 2 +
+
+
+ + 2 + + + + + + +
+
+
+ 2 +
+
+
+ + 2 + + + + + + +
+
+
+ 1 +
+
+
+ + 1 + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ 2 +
+
+
+ + 2 + + + + + + +
+
+
+ 1 +
+
+
+ + 1 + + + + + + +
+
+
+ 1 +
+
+
+ + 1 + + + + + + +
+
+
+ 0 +
+
+
+ + 0 + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ 1 +
+
+
+ + 1 + + + + + + +
+
+
+ 0 +
+
+
+ + 0 + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ 0 +
+
+
+ + 0 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + nearest occlusion cell + +
+
+
+ + nearest occlusion... + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_velocity_intersection_module/docs/stuck-vehicle.drawio.svg b/planning/behavior_velocity_intersection_module/docs/stuck-vehicle.drawio.svg index d2a8286122e54..c29cb7ade21cd 100644 --- a/planning/behavior_velocity_intersection_module/docs/stuck-vehicle.drawio.svg +++ b/planning/behavior_velocity_intersection_module/docs/stuck-vehicle.drawio.svg @@ -5,28 +5,28 @@ xmlns="http://www.w3.org/2000/svg" xmlns:xlink="http://www.w3.org/1999/xlink" version="1.1" - width="2831px" - height="1073px" - viewBox="-0.5 -0.5 2831 1073" - content="<mxfile host="Electron" modified="2023-06-07T05:26:53.508Z" agent="5.0 (X11; Linux x86_64) AppleWebKit/537.36 (KHTML, like Gecko) draw.io/20.3.0 Chrome/104.0.5112.114 Electron/20.1.3 Safari/537.36" etag="B7vxgoBz0X2z8hjEAQoV" version="20.3.0" type="device"><diagram name="intersection" id="0L5whF3ImEvTl2DSWTjR">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</diagram></mxfile>" + width="2842px" + height="1201px" + viewBox="-0.5 -0.5 2842 1201" + content="<mxfile host="Electron" modified="2023-10-03T03:33:38.127Z" agent="5.0 (X11; Linux x86_64) AppleWebKit/537.36 (KHTML, like Gecko) draw.io/20.3.0 Chrome/104.0.5112.114 Electron/20.1.3 Safari/537.36" etag="TqIhwSEqRvc68Imy-9XT" version="20.3.0" type="device"><diagram name="intersection" id="0L5whF3ImEvTl2DSWTjR">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</diagram></mxfile>" > - - - - - - - - - - - - - - + + + + + + + + + + + + + - + - + - + - - - - - + + + + + - + - - - + + + - - - + + + - - - - + + + + + + + - - - - + + + + - - - + + + - - + + - + - - - - - + + + + + - +
@@ -259,22 +262,22 @@
- stuck vehicle detection area is generated on the path around the exit o... + stuck vehicle detection area is generated on the path around the exit o... - - - - - - - - - - - - + + + + + + + + + + + - + - + - + - - - - - + + + + + - + - +
@@ -366,14 +369,14 @@
- %3CmxGraphModel%3E%3Croot%3E%3... + %3CmxGraphModel%3E%3Croot%3E%3... - - - + + + - - - + + + - - - - + + + + - - - - + + + + - - - + + + - - + + - + - - + + - - + + @@ -510,7 +513,7 @@
@@ -521,18 +524,18 @@
- stuck vehicle detection area is generated from the path, so in this... + stuck vehicle detection area is generated from the path, so in this... - - - - - + + + + + -
+
@@ -542,76 +545,158 @@
- stop_line_margin + stop_line_margin - + - - + +
`
 - ` + ` + + + + +
+
+
+ + + stuck_vehicle_detect_dist + + +
+
+
+ + stuck_vehicle_detect_dist + + + + + +
+
+
+ + + %3CmxGraphModel%3E%3Croot%3E%3CmxCell%20id%3D%220%22%2F%3E%3CmxCell%20id%3D%221%22%20parent%3D%220%22%2F%3E%3CmxCell%20id%3D%222%22%20value%3D%22%26lt%3Bfont%20style%3D%26quot%3Bfont-size%3A%2035px%3B%26quot%3B%26gt%3B%26lt%3Bb%26gt%3Bw%2F%20traffic%20light%2C%20right%26lt%3B%2Fb%26gt%3B%26lt%3Bbr%26gt%3B%26lt%3Bbr%26gt%3B%26lt%3Bb%26gt%3B(Left-hand%20traffic)%26lt%3B%2Fb%26gt%3B%26lt%3B%2Ffont%26gt%3B%26lt%3Bspan%20style%3D%26quot%3Bfont-size%3A%2030px%3B%26quot%3B%26gt%3B%26lt%3Bb%26gt%3B%26lt%3Bbr%26gt%3B%26lt%3B%2Fb%26gt%3B%26lt%3B%2Fspan%26gt%3B%22%20style%3D%22text%3Bhtml%3D1%3Balign%3Dleft%3BverticalAlign%3Dmiddle%3Bresizable%3D0%3Bpoints%3D%5B%5D%3Bautosize%3D1%3BstrokeColor%3Dnone%3BfillColor%3Dnone%3B%22%20vertex%3D%221%22%20parent%3D%221%22%3E%3CmxGeometry%20x%3D%22-17325%22%20y%3D%22-7209%22%20width%3D%22350%22%20height%3D%22140%22%20as%3D%22geometry%22%2F%3E%3C%2FmxCell%3E%3C%2Froot%3E%3C%2FmxGraphModel%3E + + straight +
+
+ (Right-hand traffic) + + + +
+ + +
+
+
+ + %3CmxGraphModel%3E%3Croot%3E%3CmxCell%20id%3D%220%22%2F%... + + + + + +
+
+
+ + + %3CmxGraphModel%3E%3Croot%3E%3CmxCell%20id%3D%220%22%2F%3E%3CmxCell%20id%3D%221%22%20parent%3D%220%22%2F%3E%3CmxCell%20id%3D%222%22%20value%3D%22%26lt%3Bfont%20style%3D%26quot%3Bfont-size%3A%2035px%3B%26quot%3B%26gt%3B%26lt%3Bb%26gt%3Bw%2F%20traffic%20light%2C%20right%26lt%3B%2Fb%26gt%3B%26lt%3Bbr%26gt%3B%26lt%3Bbr%26gt%3B%26lt%3Bb%26gt%3B(Left-hand%20traffic)%26lt%3B%2Fb%26gt%3B%26lt%3B%2Ffont%26gt%3B%26lt%3Bspan%20style%3D%26quot%3Bfont-size%3A%2030px%3B%26quot%3B%26gt%3B%26lt%3Bb%26gt%3B%26lt%3Bbr%26gt%3B%26lt%3B%2Fb%26gt%3B%26lt%3B%2Fspan%26gt%3B%22%20style%3D%22text%3Bhtml%3D1%3Balign%3Dleft%3BverticalAlign%3Dmiddle%3Bresizable%3D0%3Bpoints%3D%5B%5D%3Bautosize%3D1%3BstrokeColor%3Dnone%3BfillColor%3Dnone%3B%22%20vertex%3D%221%22%20parent%3D%221%22%3E%3CmxGeometry%20x%3D%22-17325%22%20y%3D%22-7209%22%20width%3D%22350%22%20height%3D%22140%22%20as%3D%22geometry%22%2F%3E%3C%2FmxCell%3E%3C%2Froot%3E%3C%2FmxGraphModel%3E + + left turn +
+
+ (Right-hand traffic) + + + +
+ + +
+
+
+ + %3CmxGraphModel%3E%3Croot%3E%3CmxCell%20id%3D%220%22%2F%... + + diff --git a/planning/behavior_velocity_intersection_module/package.xml b/planning/behavior_velocity_intersection_module/package.xml index 1d6067d4be322..0c9b3407d0f38 100644 --- a/planning/behavior_velocity_intersection_module/package.xml +++ b/planning/behavior_velocity_intersection_module/package.xml @@ -9,6 +9,7 @@ Takayuki Murooka Tomoya Kimura Shumpei Wakabayashi + Kyoichi Sugahara Apache License 2.0 @@ -19,11 +20,12 @@ autoware_auto_perception_msgs autoware_auto_planning_msgs + autoware_perception_msgs behavior_velocity_planner_common geometry_msgs interpolation lanelet2_extension - magic_enum + libopencv-dev motion_utils nav_msgs pluginlib @@ -37,8 +39,6 @@ vehicle_info_util visualization_msgs - grid_map_rviz_plugin - ament_lint_auto autoware_lint_common diff --git a/planning/behavior_velocity_intersection_module/src/debug.cpp b/planning/behavior_velocity_intersection_module/src/debug.cpp index ae8eee39d6556..8d9beb34d05ee 100644 --- a/planning/behavior_velocity_intersection_module/src/debug.cpp +++ b/planning/behavior_velocity_intersection_module/src/debug.cpp @@ -17,7 +17,16 @@ #include #include -#include +#include +#include + +#include + +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif #include #include @@ -104,7 +113,33 @@ visualization_msgs::msg::MarkerArray createPoseMarkerArray( return msg; } -visualization_msgs::msg::Marker createPointMarkerArray( +visualization_msgs::msg::MarkerArray createLineMarkerArray( + const geometry_msgs::msg::Point & point_start, const geometry_msgs::msg::Point & point_end, + const std::string & ns, const int64_t id, const double r, const double g, const double b) +{ + visualization_msgs::msg::MarkerArray msg; + + visualization_msgs::msg::Marker marker; + marker.header.frame_id = "map"; + marker.ns = ns + "_line"; + marker.id = id; + marker.lifetime = rclcpp::Duration::from_seconds(0.3); + marker.type = visualization_msgs::msg::Marker::ARROW; + marker.action = visualization_msgs::msg::Marker::ADD; + geometry_msgs::msg::Vector3 arrow; + arrow.x = 1.0; + arrow.y = 1.0; + arrow.z = 1.0; + marker.scale = arrow; + marker.color = createMarkerColor(r, g, b, 0.999); + marker.points.push_back(point_start); + marker.points.push_back(point_end); + + msg.markers.push_back(marker); + return msg; +} + +[[maybe_unused]] visualization_msgs::msg::Marker createPointMarkerArray( const geometry_msgs::msg::Point & point, const std::string & ns, const int64_t id, const double r, const double g, const double b) { @@ -138,18 +173,18 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( &debug_marker_array); } - if (debug_data_.adjacent_area) { + if (debug_data_.occlusion_attention_area) { appendMarkerArray( createLaneletPolygonsMarkerArray( - debug_data_.adjacent_area.value(), "adjacent_area", lane_id_, 0.913, 0.639, 0.149), + debug_data_.occlusion_attention_area.value(), "occlusion_attention_area", lane_id_, 0.917, + 0.568, 0.596), &debug_marker_array); } - if (debug_data_.intersection_area) { + if (debug_data_.adjacent_area) { appendMarkerArray( - debug::createPolygonMarkerArray( - debug_data_.intersection_area.value(), "intersection_area", lane_id_, now, 0.3, 0.0, 0.0, - 0.0, 1.0, 0.0), + createLaneletPolygonsMarkerArray( + debug_data_.adjacent_area.value(), "adjacent_area", lane_id_, 0.913, 0.639, 0.149), &debug_marker_array); } @@ -176,17 +211,6 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( &debug_marker_array, now); } - if (debug_data_.nearest_occlusion_point) { - debug_marker_array.markers.push_back(createPointMarkerArray( - debug_data_.nearest_occlusion_point.value(), "nearest_occlusion", module_id_, 0.5, 0.5, 0.0)); - } - - if (debug_data_.nearest_occlusion_projection_point) { - debug_marker_array.markers.push_back(createPointMarkerArray( - debug_data_.nearest_occlusion_projection_point.value(), "nearest_occlusion_projection", - module_id_, 0.5, 0.5, 0.0)); - } - size_t i{0}; for (const auto & p : debug_data_.candidate_collision_object_polygons) { appendMarkerArray( @@ -201,11 +225,33 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( debug_data_.conflicting_targets, "conflicting_targets", module_id_, now, 0.99, 0.4, 0.0), &debug_marker_array, now); + appendMarkerArray( + debug::createObjectsMarkerArray( + debug_data_.amber_ignore_targets, "amber_ignore_targets", module_id_, now, 0.0, 1.0, 0.0), + &debug_marker_array, now); + + appendMarkerArray( + debug::createObjectsMarkerArray( + debug_data_.red_overshoot_ignore_targets, "red_overshoot_ignore_targets", module_id_, now, + 0.0, 1.0, 0.0), + &debug_marker_array, now); + appendMarkerArray( debug::createObjectsMarkerArray( debug_data_.stuck_targets, "stuck_targets", module_id_, now, 0.99, 0.99, 0.2), &debug_marker_array, now); + appendMarkerArray( + debug::createObjectsMarkerArray( + debug_data_.yield_stuck_targets, "stuck_targets", module_id_, now, 0.4, 0.99, 0.2), + &debug_marker_array, now); + + appendMarkerArray( + debug::createObjectsMarkerArray( + debug_data_.blocking_attention_objects, "blocking_attention_objects", module_id_, now, 0.99, + 0.99, 0.6), + &debug_marker_array, now); + /* appendMarkerArray( createPoseMarkerArray( @@ -229,34 +275,55 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( &debug_marker_array, now); } + if (debug_data_.nearest_occlusion_projection) { + const auto [point_start, point_end] = debug_data_.nearest_occlusion_projection.value(); + appendMarkerArray( + createLineMarkerArray( + point_start, point_end, "nearest_occlusion_projection", lane_id_, 0.5, 0.5, 0.0), + &debug_marker_array, now); + } return debug_marker_array; } motion_utils::VirtualWalls IntersectionModule::createVirtualWalls() { - // TODO(Mamoru Sobue): collision stop pose depends on before/after occlusion clearance motion_utils::VirtualWalls virtual_walls; motion_utils::VirtualWall wall; - wall.style = motion_utils::VirtualWallType::stop; if (debug_data_.collision_stop_wall_pose) { + wall.style = motion_utils::VirtualWallType::stop; wall.text = "intersection"; wall.ns = "intersection" + std::to_string(module_id_) + "_"; wall.pose = debug_data_.collision_stop_wall_pose.value(); virtual_walls.push_back(wall); } if (debug_data_.occlusion_first_stop_wall_pose) { + wall.style = motion_utils::VirtualWallType::stop; wall.text = "intersection"; wall.ns = "intersection_occlusion_first_stop" + std::to_string(module_id_) + "_"; wall.pose = debug_data_.occlusion_first_stop_wall_pose.value(); virtual_walls.push_back(wall); } if (debug_data_.occlusion_stop_wall_pose) { + wall.style = motion_utils::VirtualWallType::stop; wall.text = "intersection_occlusion"; + if (debug_data_.static_occlusion_with_traffic_light_timeout) { + std::stringstream timeout; + timeout << std::setprecision(2) + << debug_data_.static_occlusion_with_traffic_light_timeout.value(); + wall.text += "(" + timeout.str() + ")"; + } wall.ns = "intersection_occlusion" + std::to_string(module_id_) + "_"; wall.pose = debug_data_.occlusion_stop_wall_pose.value(); virtual_walls.push_back(wall); } + if (debug_data_.absence_traffic_light_creep_wall) { + wall.style = motion_utils::VirtualWallType::slowdown; + wall.text = "intersection_occlusion"; + wall.ns = "intersection_occlusion" + std::to_string(module_id_) + "_"; + wall.pose = debug_data_.absence_traffic_light_creep_wall.value(); + virtual_walls.push_back(wall); + } return virtual_walls; } diff --git a/planning/behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_intersection_module/src/manager.cpp index 6a2b7200abb2e..3c0d7fa0b4f1c 100644 --- a/planning/behavior_velocity_intersection_module/src/manager.cpp +++ b/planning/behavior_velocity_intersection_module/src/manager.cpp @@ -17,6 +17,7 @@ #include #include #include +#include #include @@ -29,94 +30,157 @@ namespace behavior_velocity_planner { +using tier4_autoware_utils::getOrDeclareParameter; + IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) : SceneModuleManagerInterfaceWithRTC( node, getModuleName(), - node.declare_parameter(std::string(getModuleName()) + ".enable_rtc.intersection")), + getOrDeclareParameter(node, std::string(getModuleName()) + ".enable_rtc.intersection")), occlusion_rtc_interface_( &node, "intersection_occlusion", - node.declare_parameter( - std::string(getModuleName()) + ".enable_rtc.intersection_to_occlusion")) + getOrDeclareParameter( + node, std::string(getModuleName()) + ".enable_rtc.intersection_to_occlusion")) { const std::string ns(getModuleName()); auto & ip = intersection_param_; - const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo(); ip.common.attention_area_margin = - node.declare_parameter(ns + ".common.attention_area_margin"); + getOrDeclareParameter(node, ns + ".common.attention_area_margin"); ip.common.attention_area_length = - node.declare_parameter(ns + ".common.attention_area_length"); + getOrDeclareParameter(node, ns + ".common.attention_area_length"); ip.common.attention_area_angle_thr = - node.declare_parameter(ns + ".common.attention_area_angle_threshold"); - ip.common.stop_line_margin = node.declare_parameter(ns + ".common.stop_line_margin"); + getOrDeclareParameter(node, ns + ".common.attention_area_angle_threshold"); + ip.common.stop_line_margin = getOrDeclareParameter(node, ns + ".common.stop_line_margin"); ip.common.intersection_velocity = - node.declare_parameter(ns + ".common.intersection_velocity"); + getOrDeclareParameter(node, ns + ".common.intersection_velocity"); ip.common.intersection_max_acc = - node.declare_parameter(ns + ".common.intersection_max_accel"); + getOrDeclareParameter(node, ns + ".common.intersection_max_accel"); ip.common.stop_overshoot_margin = - node.declare_parameter(ns + ".common.stop_overshoot_margin"); + getOrDeclareParameter(node, ns + ".common.stop_overshoot_margin"); ip.common.use_intersection_area = - node.declare_parameter(ns + ".common.use_intersection_area"); + getOrDeclareParameter(node, ns + ".common.use_intersection_area"); ip.common.path_interpolation_ds = - node.declare_parameter(ns + ".common.path_interpolation_ds"); + getOrDeclareParameter(node, ns + ".common.path_interpolation_ds"); ip.common.consider_wrong_direction_vehicle = - node.declare_parameter(ns + ".common.consider_wrong_direction_vehicle"); - + getOrDeclareParameter(node, ns + ".common.consider_wrong_direction_vehicle"); + ip.common.max_accel = getOrDeclareParameter(node, ns + ".common.max_accel"); + ip.common.max_jerk = getOrDeclareParameter(node, ns + ".common.max_jerk"); + ip.common.delay_response_time = + getOrDeclareParameter(node, ns + ".common.delay_response_time"); + + ip.stuck_vehicle.turn_direction.left = + getOrDeclareParameter(node, ns + ".stuck_vehicle.turn_direction.left"); + ip.stuck_vehicle.turn_direction.right = + getOrDeclareParameter(node, ns + ".stuck_vehicle.turn_direction.right"); + ip.stuck_vehicle.turn_direction.straight = + getOrDeclareParameter(node, ns + ".stuck_vehicle.turn_direction.straight"); ip.stuck_vehicle.use_stuck_stopline = - node.declare_parameter(ns + ".stuck_vehicle.use_stuck_stopline"); + getOrDeclareParameter(node, ns + ".stuck_vehicle.use_stuck_stopline"); ip.stuck_vehicle.stuck_vehicle_detect_dist = - node.declare_parameter(ns + ".stuck_vehicle.stuck_vehicle_detect_dist"); - ip.stuck_vehicle.stuck_vehicle_ignore_dist = - node.declare_parameter(ns + ".stuck_vehicle.stuck_vehicle_ignore_dist") + - vehicle_info.max_longitudinal_offset_m; + getOrDeclareParameter(node, ns + ".stuck_vehicle.stuck_vehicle_detect_dist"); ip.stuck_vehicle.stuck_vehicle_vel_thr = - node.declare_parameter(ns + ".stuck_vehicle.stuck_vehicle_vel_thr"); + getOrDeclareParameter(node, ns + ".stuck_vehicle.stuck_vehicle_vel_thr"); /* ip.stuck_vehicle.assumed_front_car_decel = - node.declare_parameter(ns + ".stuck_vehicle.assumed_front_car_decel"); + getOrDeclareParameter(node, ns + ".stuck_vehicle.assumed_front_car_decel"); ip.stuck_vehicle.enable_front_car_decel_prediction = - node.declare_parameter(ns + ".stuck_vehicle.enable_front_car_decel_prediction"); + getOrDeclareParameter(node, ns + ".stuck_vehicle.enable_front_car_decel_prediction"); */ ip.stuck_vehicle.timeout_private_area = - node.declare_parameter(ns + ".stuck_vehicle.timeout_private_area"); + getOrDeclareParameter(node, ns + ".stuck_vehicle.timeout_private_area"); + ip.stuck_vehicle.enable_private_area_stuck_disregard = + getOrDeclareParameter(node, ns + ".stuck_vehicle.enable_private_area_stuck_disregard"); + ip.stuck_vehicle.yield_stuck_turn_direction.left = + getOrDeclareParameter(node, ns + ".stuck_vehicle.yield_stuck.turn_direction.left"); + ip.stuck_vehicle.yield_stuck_turn_direction.right = + getOrDeclareParameter(node, ns + ".stuck_vehicle.yield_stuck.turn_direction.right"); + ip.stuck_vehicle.yield_stuck_turn_direction.straight = + getOrDeclareParameter(node, ns + ".stuck_vehicle.yield_stuck.turn_direction.straight"); + ip.stuck_vehicle.yield_stuck_distance_thr = + getOrDeclareParameter(node, ns + ".stuck_vehicle.yield_stuck.distance_thr"); ip.collision_detection.min_predicted_path_confidence = - node.declare_parameter(ns + ".collision_detection.min_predicted_path_confidence"); + getOrDeclareParameter(node, ns + ".collision_detection.min_predicted_path_confidence"); ip.collision_detection.minimum_ego_predicted_velocity = - node.declare_parameter(ns + ".collision_detection.minimum_ego_predicted_velocity"); + getOrDeclareParameter(node, ns + ".collision_detection.minimum_ego_predicted_velocity"); ip.collision_detection.state_transit_margin_time = - node.declare_parameter(ns + ".collision_detection.state_transit_margin_time"); - ip.collision_detection.normal.collision_start_margin_time = - node.declare_parameter(ns + ".collision_detection.normal.collision_start_margin_time"); - ip.collision_detection.normal.collision_end_margin_time = - node.declare_parameter(ns + ".collision_detection.normal.collision_end_margin_time"); - ip.collision_detection.relaxed.collision_start_margin_time = - node.declare_parameter(ns + ".collision_detection.relaxed.collision_start_margin_time"); - ip.collision_detection.relaxed.collision_end_margin_time = - node.declare_parameter(ns + ".collision_detection.relaxed.collision_end_margin_time"); + getOrDeclareParameter(node, ns + ".collision_detection.state_transit_margin_time"); + ip.collision_detection.fully_prioritized.collision_start_margin_time = + getOrDeclareParameter( + node, ns + ".collision_detection.fully_prioritized.collision_start_margin_time"); + ip.collision_detection.fully_prioritized.collision_end_margin_time = + getOrDeclareParameter( + node, ns + ".collision_detection.fully_prioritized.collision_end_margin_time"); + ip.collision_detection.partially_prioritized.collision_start_margin_time = + getOrDeclareParameter( + node, ns + ".collision_detection.partially_prioritized.collision_start_margin_time"); + ip.collision_detection.partially_prioritized.collision_end_margin_time = + getOrDeclareParameter( + node, ns + ".collision_detection.partially_prioritized.collision_end_margin_time"); + ip.collision_detection.not_prioritized.collision_start_margin_time = + getOrDeclareParameter( + node, ns + ".collision_detection.not_prioritized.collision_start_margin_time"); + ip.collision_detection.not_prioritized.collision_end_margin_time = getOrDeclareParameter( + node, ns + ".collision_detection.not_prioritized.collision_end_margin_time"); ip.collision_detection.keep_detection_vel_thr = - node.declare_parameter(ns + ".collision_detection.keep_detection_vel_thr"); - - ip.occlusion.enable = node.declare_parameter(ns + ".occlusion.enable"); + getOrDeclareParameter(node, ns + ".collision_detection.keep_detection_vel_thr"); + ip.collision_detection.use_upstream_velocity = + getOrDeclareParameter(node, ns + ".collision_detection.use_upstream_velocity"); + ip.collision_detection.minimum_upstream_velocity = + getOrDeclareParameter(node, ns + ".collision_detection.minimum_upstream_velocity"); + ip.collision_detection.yield_on_green_traffic_light.distance_to_assigned_lanelet_start = + getOrDeclareParameter( + node, + ns + ".collision_detection.yield_on_green_traffic_light.distance_to_assigned_lanelet_start"); + ip.collision_detection.yield_on_green_traffic_light.duration = getOrDeclareParameter( + node, ns + ".collision_detection.yield_on_green_traffic_light.duration"); + ip.collision_detection.yield_on_green_traffic_light.object_dist_to_stopline = + getOrDeclareParameter( + node, ns + ".collision_detection.yield_on_green_traffic_light.object_dist_to_stopline"); + ip.collision_detection.ignore_on_amber_traffic_light.object_expected_deceleration = + getOrDeclareParameter( + node, ns + ".collision_detection.ignore_on_amber_traffic_light.object_expected_deceleration"); + ip.collision_detection.ignore_on_red_traffic_light.object_margin_to_path = + getOrDeclareParameter( + node, ns + ".collision_detection.ignore_on_red_traffic_light.object_margin_to_path"); + + ip.occlusion.enable = getOrDeclareParameter(node, ns + ".occlusion.enable"); ip.occlusion.occlusion_attention_area_length = - node.declare_parameter(ns + ".occlusion.occlusion_attention_area_length"); - ip.occlusion.enable_creeping = node.declare_parameter(ns + ".occlusion.enable_creeping"); + getOrDeclareParameter(node, ns + ".occlusion.occlusion_attention_area_length"); + ip.occlusion.enable_creeping = + getOrDeclareParameter(node, ns + ".occlusion.enable_creeping"); ip.occlusion.occlusion_creep_velocity = - node.declare_parameter(ns + ".occlusion.occlusion_creep_velocity"); - ip.occlusion.peeking_offset = node.declare_parameter(ns + ".occlusion.peeking_offset"); - ip.occlusion.free_space_max = node.declare_parameter(ns + ".occlusion.free_space_max"); - ip.occlusion.occupied_min = node.declare_parameter(ns + ".occlusion.occupied_min"); - ip.occlusion.do_dp = node.declare_parameter(ns + ".occlusion.do_dp"); + getOrDeclareParameter(node, ns + ".occlusion.occlusion_creep_velocity"); + ip.occlusion.peeking_offset = + getOrDeclareParameter(node, ns + ".occlusion.peeking_offset"); + ip.occlusion.free_space_max = getOrDeclareParameter(node, ns + ".occlusion.free_space_max"); + ip.occlusion.occupied_min = getOrDeclareParameter(node, ns + ".occlusion.occupied_min"); + ip.occlusion.do_dp = getOrDeclareParameter(node, ns + ".occlusion.do_dp"); ip.occlusion.before_creep_stop_time = - node.declare_parameter(ns + ".occlusion.before_creep_stop_time"); + getOrDeclareParameter(node, ns + ".occlusion.before_creep_stop_time"); ip.occlusion.min_vehicle_brake_for_rss = - node.declare_parameter(ns + ".occlusion.min_vehicle_brake_for_rss"); + getOrDeclareParameter(node, ns + ".occlusion.min_vehicle_brake_for_rss"); ip.occlusion.max_vehicle_velocity_for_rss = - node.declare_parameter(ns + ".occlusion.max_vehicle_velocity_for_rss"); - ip.occlusion.denoise_kernel = node.declare_parameter(ns + ".occlusion.denoise_kernel"); + getOrDeclareParameter(node, ns + ".occlusion.max_vehicle_velocity_for_rss"); + ip.occlusion.denoise_kernel = + getOrDeclareParameter(node, ns + ".occlusion.denoise_kernel"); ip.occlusion.possible_object_bbox = - node.declare_parameter>(ns + ".occlusion.possible_object_bbox"); + getOrDeclareParameter>(node, ns + ".occlusion.possible_object_bbox"); ip.occlusion.ignore_parked_vehicle_speed_threshold = - node.declare_parameter(ns + ".occlusion.ignore_parked_vehicle_speed_threshold"); + getOrDeclareParameter(node, ns + ".occlusion.ignore_parked_vehicle_speed_threshold"); + ip.occlusion.stop_release_margin_time = + getOrDeclareParameter(node, ns + ".occlusion.stop_release_margin_time"); + ip.occlusion.temporal_stop_before_attention_area = + getOrDeclareParameter(node, ns + ".occlusion.temporal_stop_before_attention_area"); + ip.occlusion.absence_traffic_light.creep_velocity = + getOrDeclareParameter(node, ns + ".occlusion.absence_traffic_light.creep_velocity"); + ip.occlusion.absence_traffic_light.maximum_peeking_distance = getOrDeclareParameter( + node, ns + ".occlusion.absence_traffic_light.maximum_peeking_distance"); + ip.occlusion.attention_lane_crop_curvature_threshold = + getOrDeclareParameter(node, ns + ".occlusion.attention_lane_crop_curvature_threshold"); + ip.occlusion.attention_lane_curvature_calculation_ds = + getOrDeclareParameter(node, ns + ".occlusion.attention_lane_curvature_calculation_ds"); + ip.occlusion.static_occlusion_with_traffic_light_timeout = getOrDeclareParameter( + node, ns + ".occlusion.static_occlusion_with_traffic_light_timeout"); } void IntersectionModuleManager::launchNewModules( @@ -146,13 +210,21 @@ void IntersectionModuleManager::launchNewModules( continue; } - const auto associative_ids = - planning_utils::getAssociativeIntersectionLanelets(ll, lanelet_map, routing_graph); const std::string location = ll.attributeOr("location", "else"); const bool is_private_area = (location.compare("private") == 0); + const auto associative_ids = + planning_utils::getAssociativeIntersectionLanelets(ll, lanelet_map, routing_graph); + bool has_traffic_light = false; + if (const auto tl_reg_elems = ll.regulatoryElementsAs(); + tl_reg_elems.size() != 0) { + const auto tl_reg_elem = tl_reg_elems.front(); + const auto stop_line_opt = tl_reg_elem->stopLine(); + if (!!stop_line_opt) has_traffic_light = true; + } const auto new_module = std::make_shared( - module_id, lane_id, planner_data_, intersection_param_, associative_ids, is_private_area, - enable_occlusion_detection, node_, logger_.get_child("intersection_module"), clock_); + module_id, lane_id, planner_data_, intersection_param_, associative_ids, turn_direction, + has_traffic_light, enable_occlusion_detection, is_private_area, node_, + logger_.get_child("intersection_module"), clock_); generateUUID(module_id); /* set RTC status as non_occluded status initially */ const UUID uuid = getUUID(new_module->getModuleId()); @@ -263,12 +335,13 @@ MergeFromPrivateModuleManager::MergeFromPrivateModuleManager(rclcpp::Node & node { const std::string ns(getModuleName()); auto & mp = merge_from_private_area_param_; - mp.stop_duration_sec = node.declare_parameter(ns + ".stop_duration_sec"); + mp.stop_duration_sec = getOrDeclareParameter(node, ns + ".stop_duration_sec"); mp.attention_area_length = node.get_parameter("intersection.common.attention_area_length").as_double(); - mp.stop_line_margin = node.declare_parameter(ns + ".stop_line_margin"); + mp.stop_line_margin = getOrDeclareParameter(node, ns + ".stop_line_margin"); mp.path_interpolation_ds = node.get_parameter("intersection.common.path_interpolation_ds").as_double(); + mp.stop_distance_threshold = getOrDeclareParameter(node, ns + ".stop_distance_threshold"); } void MergeFromPrivateModuleManager::launchNewModules( diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index 7ae549ce8b274..53a99a1ee6b4d 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -17,28 +17,60 @@ #include "util.hpp" #include -#include #include -#include -#include -#include #include -#include +#include #include +#include +#include +#include +#include +#include +#include + +#include +#include #include #include +#include #include #include +#include #include #include #include - namespace behavior_velocity_planner { namespace bg = boost::geometry; +namespace +{ +Polygon2d createOneStepPolygon( + const geometry_msgs::msg::Pose & prev_pose, const geometry_msgs::msg::Pose & next_pose, + const autoware_auto_perception_msgs::msg::Shape & shape) +{ + const auto prev_poly = tier4_autoware_utils::toPolygon2d(prev_pose, shape); + const auto next_poly = tier4_autoware_utils::toPolygon2d(next_pose, shape); + + Polygon2d one_step_poly; + for (const auto & point : prev_poly.outer()) { + one_step_poly.outer().push_back(point); + } + for (const auto & point : next_poly.outer()) { + one_step_poly.outer().push_back(point); + } + + bg::correct(one_step_poly); + + Polygon2d convex_one_step_poly; + bg::convex_hull(one_step_poly, convex_one_step_poly); + + return convex_one_step_poly; +} +} // namespace + static bool isTargetCollisionVehicleType( const autoware_auto_perception_msgs::msg::PredictedObject & object) { @@ -61,14 +93,18 @@ static bool isTargetCollisionVehicleType( } IntersectionModule::IntersectionModule( - const int64_t module_id, const int64_t lane_id, std::shared_ptr planner_data, + const int64_t module_id, const int64_t lane_id, + [[maybe_unused]] std::shared_ptr planner_data, const PlannerParam & planner_param, const std::set & associative_ids, - const bool is_private_area, const bool enable_occlusion_detection, rclcpp::Node & node, + const std::string & turn_direction, const bool has_traffic_light, + const bool enable_occlusion_detection, const bool is_private_area, rclcpp::Node & node, const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock) : SceneModuleInterface(module_id, logger, clock), node_(node), lane_id_(lane_id), associative_ids_(associative_ids), + turn_direction_(turn_direction), + has_traffic_light_(has_traffic_light), enable_occlusion_detection_(enable_occlusion_detection), occlusion_attention_divisions_(std::nullopt), is_private_area_(is_private_area), @@ -77,15 +113,29 @@ IntersectionModule::IntersectionModule( velocity_factor_.init(VelocityFactor::INTERSECTION); planner_param_ = planner_param; - const auto & assigned_lanelet = - planner_data->route_handler_->getLaneletMapPtr()->laneletLayer.get(lane_id); - turn_direction_ = assigned_lanelet.attributeOr("turn_direction", "else"); - collision_state_machine_.setMarginTime( - planner_param_.collision_detection.state_transit_margin_time); - before_creep_state_machine_.setMarginTime(planner_param_.occlusion.before_creep_stop_time); - before_creep_state_machine_.setState(StateMachine::State::STOP); - stuck_private_area_timeout_.setMarginTime(planner_param_.stuck_vehicle.timeout_private_area); - stuck_private_area_timeout_.setState(StateMachine::State::STOP); + { + collision_state_machine_.setMarginTime( + planner_param_.collision_detection.state_transit_margin_time); + } + { + before_creep_state_machine_.setMarginTime(planner_param_.occlusion.before_creep_stop_time); + before_creep_state_machine_.setState(StateMachine::State::STOP); + } + { + occlusion_stop_state_machine_.setMarginTime(planner_param_.occlusion.stop_release_margin_time); + occlusion_stop_state_machine_.setState(StateMachine::State::GO); + } + { + temporal_stop_before_attention_state_machine_.setMarginTime( + planner_param_.occlusion.before_creep_stop_time); + temporal_stop_before_attention_state_machine_.setState(StateMachine::State::STOP); + } + { + static_occlusion_timeout_state_machine_.setMarginTime( + planner_param_.occlusion.static_occlusion_with_traffic_light_timeout); + static_occlusion_timeout_state_machine_.setState(StateMachine::State::STOP); + } + decision_state_pub_ = node_.create_publisher("~/debug/intersection/decision_state", 1); } @@ -115,7 +165,7 @@ template void prepareRTCByDecisionResult( const T & result, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, double * default_distance, bool * occlusion_safety, - double * occlusion_distance, bool * occlusion_first_stop_required) + double * occlusion_distance) { static_assert("Unsupported type passed to prepareRTCByDecisionResult"); return; @@ -126,8 +176,7 @@ void prepareRTCByDecisionResult( [[maybe_unused]] const IntersectionModule::Indecisive & result, [[maybe_unused]] const autoware_auto_planning_msgs::msg::PathWithLaneId & path, [[maybe_unused]] bool * default_safety, [[maybe_unused]] double * default_distance, - [[maybe_unused]] bool * occlusion_safety, [[maybe_unused]] double * occlusion_distance, - [[maybe_unused]] bool * occlusion_first_stop_required) + [[maybe_unused]] bool * occlusion_safety, [[maybe_unused]] double * occlusion_distance) { return; } @@ -136,36 +185,50 @@ template <> void prepareRTCByDecisionResult( const IntersectionModule::StuckStop & result, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, - double * default_distance, bool * occlusion_safety, double * occlusion_distance, - [[maybe_unused]] bool * occlusion_first_stop_required) + double * default_distance, bool * occlusion_safety, double * occlusion_distance) { RCLCPP_DEBUG(rclcpp::get_logger("prepareRTCByDecisionResult"), "StuckStop"); - const auto closest_idx = result.stop_lines.closest_idx; - const auto stop_line_idx = result.stop_line_idx; + const auto closest_idx = result.closest_idx; + const auto stop_line_idx = result.stuck_stop_line_idx; *default_safety = false; *default_distance = motion_utils::calcSignedArcLength(path.points, closest_idx, stop_line_idx); *occlusion_safety = true; - if (!result.is_detection_area_empty) { - const auto occlusion_stop_line_idx = result.stop_lines.occlusion_peeking_stop_line; + if (result.occlusion_stop_line_idx) { + const auto occlusion_stop_line_idx = result.occlusion_stop_line_idx.value(); *occlusion_distance = motion_utils::calcSignedArcLength(path.points, closest_idx, occlusion_stop_line_idx); } return; } +template <> +void prepareRTCByDecisionResult( + const IntersectionModule::YieldStuckStop & result, + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, + double * default_distance, bool * occlusion_safety, [[maybe_unused]] double * occlusion_distance) +{ + RCLCPP_DEBUG(rclcpp::get_logger("prepareRTCByDecisionResult"), "YieldStuckStop"); + const auto closest_idx = result.closest_idx; + const auto stop_line_idx = result.stuck_stop_line_idx; + *default_safety = false; + *default_distance = motion_utils::calcSignedArcLength(path.points, closest_idx, stop_line_idx); + *occlusion_safety = true; + return; +} + template <> void prepareRTCByDecisionResult( const IntersectionModule::NonOccludedCollisionStop & result, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, - double * default_distance, bool * occlusion_safety, double * occlusion_distance, - [[maybe_unused]] bool * occlusion_first_stop_required) + double * default_distance, bool * occlusion_safety, double * occlusion_distance) { RCLCPP_DEBUG(rclcpp::get_logger("prepareRTCByDecisionResult"), "NonOccludedCollisionStop"); - const auto closest_idx = result.stop_lines.closest_idx; - const auto stop_line_idx = result.stop_line_idx; + const auto closest_idx = result.closest_idx; + const auto collision_stop_line_idx = result.collision_stop_line_idx; *default_safety = false; - *default_distance = motion_utils::calcSignedArcLength(path.points, closest_idx, stop_line_idx); - const auto occlusion_stop_line = result.stop_lines.occlusion_peeking_stop_line; + *default_distance = + motion_utils::calcSignedArcLength(path.points, closest_idx, collision_stop_line_idx); + const auto occlusion_stop_line = result.occlusion_stop_line_idx; *occlusion_safety = true; *occlusion_distance = motion_utils::calcSignedArcLength(path.points, closest_idx, occlusion_stop_line); @@ -176,11 +239,10 @@ template <> void prepareRTCByDecisionResult( const IntersectionModule::FirstWaitBeforeOcclusion & result, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, - double * default_distance, bool * occlusion_safety, double * occlusion_distance, - bool * occlusion_first_stop_required) + double * default_distance, bool * occlusion_safety, double * occlusion_distance) { RCLCPP_DEBUG(rclcpp::get_logger("prepareRTCByDecisionResult"), "FirstWaitBeforeOcclusion"); - const auto closest_idx = result.stop_lines.closest_idx; + const auto closest_idx = result.closest_idx; const auto first_stop_line_idx = result.first_stop_line_idx; const auto occlusion_stop_line_idx = result.occlusion_stop_line_idx; *default_safety = false; @@ -189,7 +251,6 @@ void prepareRTCByDecisionResult( *occlusion_safety = result.is_actually_occlusion_cleared; *occlusion_distance = motion_utils::calcSignedArcLength(path.points, closest_idx, occlusion_stop_line_idx); - *occlusion_first_stop_required = true; return; } @@ -197,18 +258,35 @@ template <> void prepareRTCByDecisionResult( const IntersectionModule::PeekingTowardOcclusion & result, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, - double * default_distance, bool * occlusion_safety, double * occlusion_distance, - [[maybe_unused]] bool * occlusion_first_stop_required) + double * default_distance, bool * occlusion_safety, double * occlusion_distance) { RCLCPP_DEBUG(rclcpp::get_logger("prepareRTCByDecisionResult"), "PeekingTowardOcclusion"); - const auto closest_idx = result.stop_lines.closest_idx; - const auto stop_line_idx = result.stop_line_idx; - *occlusion_safety = result.is_actually_occlusion_cleared; - *occlusion_distance = motion_utils::calcSignedArcLength(path.points, closest_idx, stop_line_idx); - const auto default_stop_line_idx = result.stop_lines.default_stop_line; + const auto closest_idx = result.closest_idx; + const auto collision_stop_line_idx = result.collision_stop_line_idx; + const auto occlusion_stop_line_idx = result.occlusion_stop_line_idx; *default_safety = true; *default_distance = - motion_utils::calcSignedArcLength(path.points, closest_idx, default_stop_line_idx); + motion_utils::calcSignedArcLength(path.points, closest_idx, collision_stop_line_idx); + *occlusion_safety = result.is_actually_occlusion_cleared; + *occlusion_distance = + motion_utils::calcSignedArcLength(path.points, closest_idx, occlusion_stop_line_idx); + return; +} + +template <> +void prepareRTCByDecisionResult( + const IntersectionModule::OccludedAbsenceTrafficLight & result, + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, + double * default_distance, bool * occlusion_safety, double * occlusion_distance) +{ + RCLCPP_DEBUG(rclcpp::get_logger("prepareRTCByDecisionResult"), "OccludedAbsenceTrafficLight"); + const auto closest_idx = result.closest_idx; + const auto collision_stop_line_idx = result.closest_idx; + *default_safety = !result.collision_detected; + *default_distance = + motion_utils::calcSignedArcLength(path.points, closest_idx, collision_stop_line_idx); + *occlusion_safety = result.is_actually_occlusion_cleared; + *occlusion_distance = 0; return; } @@ -216,15 +294,15 @@ template <> void prepareRTCByDecisionResult( const IntersectionModule::OccludedCollisionStop & result, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, - double * default_distance, bool * occlusion_safety, double * occlusion_distance, - [[maybe_unused]] bool * occlusion_first_stop_required) + double * default_distance, bool * occlusion_safety, double * occlusion_distance) { RCLCPP_DEBUG(rclcpp::get_logger("prepareRTCByDecisionResult"), "OccludedCollisionStop"); - const auto closest_idx = result.stop_lines.closest_idx; - const auto stop_line_idx = result.stop_line_idx; + const auto closest_idx = result.closest_idx; + const auto collision_stop_line_idx = result.collision_stop_line_idx; const auto occlusion_stop_line_idx = result.occlusion_stop_line_idx; *default_safety = false; - *default_distance = motion_utils::calcSignedArcLength(path.points, closest_idx, stop_line_idx); + *default_distance = + motion_utils::calcSignedArcLength(path.points, closest_idx, collision_stop_line_idx); *occlusion_safety = result.is_actually_occlusion_cleared; *occlusion_distance = motion_utils::calcSignedArcLength(path.points, closest_idx, occlusion_stop_line_idx); @@ -235,16 +313,15 @@ template <> void prepareRTCByDecisionResult( const IntersectionModule::Safe & result, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, - double * default_distance, bool * occlusion_safety, double * occlusion_distance, - [[maybe_unused]] bool * occlusion_first_stop_required) + double * default_distance, bool * occlusion_safety, double * occlusion_distance) { RCLCPP_DEBUG(rclcpp::get_logger("prepareRTCByDecisionResult"), "Safe"); - const auto closest_idx = result.stop_lines.closest_idx; - const auto default_stop_line_idx = result.stop_lines.default_stop_line; - const auto occlusion_stop_line_idx = result.stop_lines.occlusion_peeking_stop_line; + const auto closest_idx = result.closest_idx; + const auto collision_stop_line_idx = result.collision_stop_line_idx; + const auto occlusion_stop_line_idx = result.occlusion_stop_line_idx; *default_safety = true; *default_distance = - motion_utils::calcSignedArcLength(path.points, closest_idx, default_stop_line_idx); + motion_utils::calcSignedArcLength(path.points, closest_idx, collision_stop_line_idx); *occlusion_safety = true; *occlusion_distance = motion_utils::calcSignedArcLength(path.points, closest_idx, occlusion_stop_line_idx); @@ -255,16 +332,15 @@ template <> void prepareRTCByDecisionResult( const IntersectionModule::TrafficLightArrowSolidOn & result, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, - double * default_distance, bool * occlusion_safety, double * occlusion_distance, - [[maybe_unused]] bool * occlusion_first_stop_required) + double * default_distance, bool * occlusion_safety, double * occlusion_distance) { RCLCPP_DEBUG(rclcpp::get_logger("prepareRTCByDecisionResult"), "TrafficLightArrowSolidOn"); - const auto closest_idx = result.stop_lines.closest_idx; - const auto default_stop_line_idx = result.stop_lines.default_stop_line; - const auto occlusion_stop_line_idx = result.stop_lines.occlusion_peeking_stop_line; + const auto closest_idx = result.closest_idx; + const auto collision_stop_line_idx = result.collision_stop_line_idx; + const auto occlusion_stop_line_idx = result.occlusion_stop_line_idx; *default_safety = !result.collision_detected; *default_distance = - motion_utils::calcSignedArcLength(path.points, closest_idx, default_stop_line_idx); + motion_utils::calcSignedArcLength(path.points, closest_idx, collision_stop_line_idx); *occlusion_safety = true; *occlusion_distance = motion_utils::calcSignedArcLength(path.points, closest_idx, occlusion_stop_line_idx); @@ -281,11 +357,13 @@ void IntersectionModule::prepareRTCStatus( VisitorSwitch{[&](const auto & decision) { prepareRTCByDecisionResult( decision, path, &default_safety, &default_distance, &occlusion_safety_, - &occlusion_stop_distance_, &occlusion_first_stop_required_); + &occlusion_stop_distance_); }}, decision_result); setSafe(default_safety); setDistance(default_distance); + occlusion_first_stop_required_ = + std::holds_alternative(decision_result); } template @@ -326,9 +404,10 @@ void reactRTCApprovalByDecisionResult( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), "StuckStop, approval = (default: %d, occlusion: %d)", rtc_default_approved, rtc_occlusion_approved); + const auto closest_idx = decision_result.closest_idx; if (!rtc_default_approved) { // use default_rtc uuid for stuck vehicle detection - const auto stop_line_idx = decision_result.stop_line_idx; + const auto stop_line_idx = decision_result.stuck_stop_line_idx; planning_utils::setVelocityFromIndex(stop_line_idx, 0.0, path); debug_data->collision_stop_wall_pose = planning_utils::getAheadPose(stop_line_idx, baselink2front, *path); @@ -338,14 +417,14 @@ void reactRTCApprovalByDecisionResult( stop_factor.stop_factor_points = planning_utils::toRosPoints(debug_data->conflicting_targets); planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( - path->points, path->points.at(decision_result.stop_lines.closest_idx).point.pose, + path->points, path->points.at(closest_idx).point.pose, path->points.at(stop_line_idx).point.pose, VelocityFactor::INTERSECTION); } } if ( - !rtc_occlusion_approved && !decision_result.is_detection_area_empty && + !rtc_occlusion_approved && decision_result.occlusion_stop_line_idx && planner_param.occlusion.enable) { - const auto occlusion_stop_line_idx = decision_result.stop_lines.occlusion_peeking_stop_line; + const auto occlusion_stop_line_idx = decision_result.occlusion_stop_line_idx.value(); planning_utils::setVelocityFromIndex(occlusion_stop_line_idx, 0.0, path); debug_data->occlusion_stop_wall_pose = planning_utils::getAheadPose(occlusion_stop_line_idx, baselink2front, *path); @@ -354,13 +433,45 @@ void reactRTCApprovalByDecisionResult( stop_factor.stop_pose = path->points.at(occlusion_stop_line_idx).point.pose; planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( - path->points, path->points.at(decision_result.stop_lines.closest_idx).point.pose, + path->points, path->points.at(closest_idx).point.pose, path->points.at(occlusion_stop_line_idx).point.pose, VelocityFactor::INTERSECTION); } } return; } +template <> +void reactRTCApprovalByDecisionResult( + const bool rtc_default_approved, const bool rtc_occlusion_approved, + const IntersectionModule::YieldStuckStop & decision_result, + [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, + const double baselink2front, autoware_auto_planning_msgs::msg::PathWithLaneId * path, + StopReason * stop_reason, VelocityFactorInterface * velocity_factor, util::DebugData * debug_data) +{ + RCLCPP_DEBUG( + rclcpp::get_logger("reactRTCApprovalByDecisionResult"), + "YieldStuckStop, approval = (default: %d, occlusion: %d)", rtc_default_approved, + rtc_occlusion_approved); + const auto closest_idx = decision_result.closest_idx; + if (!rtc_default_approved) { + // use default_rtc uuid for stuck vehicle detection + const auto stop_line_idx = decision_result.stuck_stop_line_idx; + planning_utils::setVelocityFromIndex(stop_line_idx, 0.0, path); + debug_data->collision_stop_wall_pose = + planning_utils::getAheadPose(stop_line_idx, baselink2front, *path); + { + tier4_planning_msgs::msg::StopFactor stop_factor; + stop_factor.stop_pose = path->points.at(stop_line_idx).point.pose; + stop_factor.stop_factor_points = planning_utils::toRosPoints(debug_data->conflicting_targets); + planning_utils::appendStopReason(stop_factor, stop_reason); + velocity_factor->set( + path->points, path->points.at(closest_idx).point.pose, + path->points.at(stop_line_idx).point.pose, VelocityFactor::INTERSECTION); + } + } + return; +} + template <> void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, @@ -374,7 +485,7 @@ void reactRTCApprovalByDecisionResult( "NonOccludedCollisionStop, approval = (default: %d, occlusion: %d)", rtc_default_approved, rtc_occlusion_approved); if (!rtc_default_approved) { - const auto stop_line_idx = decision_result.stop_line_idx; + const auto stop_line_idx = decision_result.collision_stop_line_idx; planning_utils::setVelocityFromIndex(stop_line_idx, 0.0, path); debug_data->collision_stop_wall_pose = planning_utils::getAheadPose(stop_line_idx, baselink2front, *path); @@ -383,12 +494,12 @@ void reactRTCApprovalByDecisionResult( stop_factor.stop_pose = path->points.at(stop_line_idx).point.pose; planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( - path->points, path->points.at(decision_result.stop_lines.closest_idx).point.pose, + path->points, path->points.at(decision_result.closest_idx).point.pose, path->points.at(stop_line_idx).point.pose, VelocityFactor::INTERSECTION); } } if (!rtc_occlusion_approved && planner_param.occlusion.enable) { - const auto stop_line_idx = decision_result.stop_lines.occlusion_peeking_stop_line; + const auto stop_line_idx = decision_result.occlusion_stop_line_idx; planning_utils::setVelocityFromIndex(stop_line_idx, 0.0, path); debug_data->occlusion_stop_wall_pose = planning_utils::getAheadPose(stop_line_idx, baselink2front, *path); @@ -397,7 +508,7 @@ void reactRTCApprovalByDecisionResult( stop_factor.stop_pose = path->points.at(stop_line_idx).point.pose; planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( - path->points, path->points.at(decision_result.stop_lines.closest_idx).point.pose, + path->points, path->points.at(decision_result.closest_idx).point.pose, path->points.at(stop_line_idx).point.pose, VelocityFactor::INTERSECTION); } } @@ -426,14 +537,14 @@ void reactRTCApprovalByDecisionResult( stop_factor.stop_pose = path->points.at(stop_line_idx).point.pose; planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( - path->points, path->points.at(decision_result.stop_lines.closest_idx).point.pose, + path->points, path->points.at(decision_result.closest_idx).point.pose, path->points.at(stop_line_idx).point.pose, VelocityFactor::INTERSECTION); } } if (!rtc_occlusion_approved && planner_param.occlusion.enable) { if (planner_param.occlusion.enable_creeping) { const size_t occlusion_peeking_stop_line = decision_result.occlusion_stop_line_idx; - const size_t closest_idx = decision_result.stop_lines.closest_idx; + const size_t closest_idx = decision_result.closest_idx; for (size_t i = closest_idx; i < occlusion_peeking_stop_line; i++) { planning_utils::setVelocityFromIndex( i, planner_param.occlusion.occlusion_creep_velocity, path); @@ -448,7 +559,7 @@ void reactRTCApprovalByDecisionResult( stop_factor.stop_pose = path->points.at(stop_line_idx).point.pose; planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( - path->points, path->points.at(decision_result.stop_lines.closest_idx).point.pose, + path->points, path->points.at(decision_result.closest_idx).point.pose, path->points.at(stop_line_idx).point.pose, VelocityFactor::INTERSECTION); } } @@ -471,9 +582,11 @@ void reactRTCApprovalByDecisionResult( if (!rtc_occlusion_approved && planner_param.occlusion.enable) { const size_t occlusion_peeking_stop_line = - decision_result.stop_lines.occlusion_peeking_stop_line; + decision_result.temporal_stop_before_attention_required + ? decision_result.first_attention_stop_line_idx + : decision_result.occlusion_stop_line_idx; if (planner_param.occlusion.enable_creeping) { - const size_t closest_idx = decision_result.stop_lines.closest_idx; + const size_t closest_idx = decision_result.closest_idx; for (size_t i = closest_idx; i < occlusion_peeking_stop_line; i++) { planning_utils::setVelocityFromIndex( i, planner_param.occlusion.occlusion_creep_velocity, path); @@ -482,17 +595,19 @@ void reactRTCApprovalByDecisionResult( planning_utils::setVelocityFromIndex(occlusion_peeking_stop_line, 0.0, path); debug_data->occlusion_stop_wall_pose = planning_utils::getAheadPose(occlusion_peeking_stop_line, baselink2front, *path); + debug_data->static_occlusion_with_traffic_light_timeout = + decision_result.static_occlusion_timeout; { tier4_planning_msgs::msg::StopFactor stop_factor; stop_factor.stop_pose = path->points.at(occlusion_peeking_stop_line).point.pose; planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( - path->points, path->points.at(decision_result.stop_lines.closest_idx).point.pose, + path->points, path->points.at(decision_result.closest_idx).point.pose, path->points.at(occlusion_peeking_stop_line).point.pose, VelocityFactor::INTERSECTION); } } - if (!rtc_default_approved && planner_param.occlusion.enable) { - const auto stop_line_idx = decision_result.stop_lines.default_stop_line; + if (!rtc_default_approved) { + const auto stop_line_idx = decision_result.collision_stop_line_idx; planning_utils::setVelocityFromIndex(stop_line_idx, 0.0, path); debug_data->collision_stop_wall_pose = planning_utils::getAheadPose(stop_line_idx, baselink2front, *path); @@ -501,7 +616,7 @@ void reactRTCApprovalByDecisionResult( stop_factor.stop_pose = path->points.at(stop_line_idx).point.pose; planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( - path->points, path->points.at(decision_result.stop_lines.closest_idx).point.pose, + path->points, path->points.at(decision_result.closest_idx).point.pose, path->points.at(stop_line_idx).point.pose, VelocityFactor::INTERSECTION); } } @@ -521,7 +636,7 @@ void reactRTCApprovalByDecisionResult( "OccludedCollisionStop, approval = (default: %d, occlusion: %d)", rtc_default_approved, rtc_occlusion_approved); if (!rtc_default_approved) { - const auto stop_line_idx = decision_result.stop_line_idx; + const auto stop_line_idx = decision_result.collision_stop_line_idx; planning_utils::setVelocityFromIndex(stop_line_idx, 0.0, path); debug_data->collision_stop_wall_pose = planning_utils::getAheadPose(stop_line_idx, baselink2front, *path); @@ -530,12 +645,59 @@ void reactRTCApprovalByDecisionResult( stop_factor.stop_pose = path->points.at(stop_line_idx).point.pose; planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( - path->points, path->points.at(decision_result.stop_lines.closest_idx).point.pose, + path->points, path->points.at(decision_result.closest_idx).point.pose, path->points.at(stop_line_idx).point.pose, VelocityFactor::INTERSECTION); } } if (!rtc_occlusion_approved && planner_param.occlusion.enable) { - const auto stop_line_idx = decision_result.occlusion_stop_line_idx; + const auto stop_line_idx = decision_result.temporal_stop_before_attention_required + ? decision_result.first_attention_stop_line_idx + : decision_result.occlusion_stop_line_idx; + planning_utils::setVelocityFromIndex(stop_line_idx, 0.0, path); + debug_data->occlusion_stop_wall_pose = + planning_utils::getAheadPose(stop_line_idx, baselink2front, *path); + debug_data->static_occlusion_with_traffic_light_timeout = + decision_result.static_occlusion_timeout; + { + tier4_planning_msgs::msg::StopFactor stop_factor; + stop_factor.stop_pose = path->points.at(stop_line_idx).point.pose; + planning_utils::appendStopReason(stop_factor, stop_reason); + velocity_factor->set( + path->points, path->points.at(decision_result.closest_idx).point.pose, + path->points.at(stop_line_idx).point.pose, VelocityFactor::INTERSECTION); + } + } + return; +} + +template <> +void reactRTCApprovalByDecisionResult( + const bool rtc_default_approved, const bool rtc_occlusion_approved, + const IntersectionModule::OccludedAbsenceTrafficLight & decision_result, + [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, + const double baselink2front, autoware_auto_planning_msgs::msg::PathWithLaneId * path, + StopReason * stop_reason, VelocityFactorInterface * velocity_factor, util::DebugData * debug_data) +{ + RCLCPP_DEBUG( + rclcpp::get_logger("reactRTCApprovalByDecisionResult"), + "OccludedAbsenceTrafficLight, approval = (default: %d, occlusion: %d)", rtc_default_approved, + rtc_occlusion_approved); + if (!rtc_default_approved) { + const auto stop_line_idx = decision_result.closest_idx; + planning_utils::setVelocityFromIndex(stop_line_idx, 0.0, path); + debug_data->collision_stop_wall_pose = + planning_utils::getAheadPose(stop_line_idx, baselink2front, *path); + { + tier4_planning_msgs::msg::StopFactor stop_factor; + stop_factor.stop_pose = path->points.at(stop_line_idx).point.pose; + planning_utils::appendStopReason(stop_factor, stop_reason); + velocity_factor->set( + path->points, path->points.at(decision_result.closest_idx).point.pose, + path->points.at(stop_line_idx).point.pose, VelocityFactor::INTERSECTION); + } + } + if (!rtc_occlusion_approved && decision_result.temporal_stop_before_attention_required) { + const auto stop_line_idx = decision_result.first_attention_area_stop_line_idx; planning_utils::setVelocityFromIndex(stop_line_idx, 0.0, path); debug_data->occlusion_stop_wall_pose = planning_utils::getAheadPose(stop_line_idx, baselink2front, *path); @@ -544,10 +706,20 @@ void reactRTCApprovalByDecisionResult( stop_factor.stop_pose = path->points.at(stop_line_idx).point.pose; planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( - path->points, path->points.at(decision_result.stop_lines.closest_idx).point.pose, + path->points, path->points.at(decision_result.closest_idx).point.pose, path->points.at(stop_line_idx).point.pose, VelocityFactor::INTERSECTION); } } + if (!rtc_occlusion_approved && !decision_result.temporal_stop_before_attention_required) { + const auto closest_idx = decision_result.closest_idx; + const auto peeking_limit_line = decision_result.peeking_limit_line_idx; + for (auto i = closest_idx; i <= peeking_limit_line; ++i) { + planning_utils::setVelocityFromIndex( + i, planner_param.occlusion.absence_traffic_light.creep_velocity, path); + } + debug_data->absence_traffic_light_creep_wall = + planning_utils::getAheadPose(closest_idx, baselink2front, *path); + } return; } @@ -563,7 +735,7 @@ void reactRTCApprovalByDecisionResult( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), "Safe, approval = (default: %d, occlusion: %d)", rtc_default_approved, rtc_occlusion_approved); if (!rtc_default_approved) { - const auto stop_line_idx = decision_result.stop_lines.default_stop_line; + const auto stop_line_idx = decision_result.collision_stop_line_idx; planning_utils::setVelocityFromIndex(stop_line_idx, 0.0, path); debug_data->collision_stop_wall_pose = planning_utils::getAheadPose(stop_line_idx, baselink2front, *path); @@ -572,12 +744,12 @@ void reactRTCApprovalByDecisionResult( stop_factor.stop_pose = path->points.at(stop_line_idx).point.pose; planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( - path->points, path->points.at(decision_result.stop_lines.closest_idx).point.pose, + path->points, path->points.at(decision_result.closest_idx).point.pose, path->points.at(stop_line_idx).point.pose, VelocityFactor::INTERSECTION); } } if (!rtc_occlusion_approved && planner_param.occlusion.enable) { - const auto stop_line_idx = decision_result.stop_lines.occlusion_peeking_stop_line; + const auto stop_line_idx = decision_result.occlusion_stop_line_idx; planning_utils::setVelocityFromIndex(stop_line_idx, 0.0, path); debug_data->occlusion_stop_wall_pose = planning_utils::getAheadPose(stop_line_idx, baselink2front, *path); @@ -586,7 +758,7 @@ void reactRTCApprovalByDecisionResult( stop_factor.stop_pose = path->points.at(stop_line_idx).point.pose; planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( - path->points, path->points.at(decision_result.stop_lines.closest_idx).point.pose, + path->points, path->points.at(decision_result.closest_idx).point.pose, path->points.at(stop_line_idx).point.pose, VelocityFactor::INTERSECTION); } } @@ -606,7 +778,7 @@ void reactRTCApprovalByDecisionResult( "TrafficLightArrowSolidOn, approval = (default: %d, occlusion: %d)", rtc_default_approved, rtc_occlusion_approved); if (!rtc_default_approved) { - const auto stop_line_idx = decision_result.stop_lines.default_stop_line; + const auto stop_line_idx = decision_result.collision_stop_line_idx; planning_utils::setVelocityFromIndex(stop_line_idx, 0.0, path); debug_data->collision_stop_wall_pose = planning_utils::getAheadPose(stop_line_idx, baselink2front, *path); @@ -615,7 +787,21 @@ void reactRTCApprovalByDecisionResult( stop_factor.stop_pose = path->points.at(stop_line_idx).point.pose; planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( - path->points, path->points.at(decision_result.stop_lines.closest_idx).point.pose, + path->points, path->points.at(decision_result.closest_idx).point.pose, + path->points.at(stop_line_idx).point.pose, VelocityFactor::INTERSECTION); + } + } + if (!rtc_occlusion_approved && planner_param.occlusion.enable) { + const auto stop_line_idx = decision_result.occlusion_stop_line_idx; + planning_utils::setVelocityFromIndex(stop_line_idx, 0.0, path); + debug_data->occlusion_stop_wall_pose = + planning_utils::getAheadPose(stop_line_idx, baselink2front, *path); + { + tier4_planning_msgs::msg::StopFactor stop_factor; + stop_factor.stop_pose = path->points.at(stop_line_idx).point.pose; + planning_utils::appendStopReason(stop_factor, stop_reason); + velocity_factor->set( + path->points, path->points.at(decision_result.closest_idx).point.pose, path->points.at(stop_line_idx).point.pose, VelocityFactor::INTERSECTION); } } @@ -639,6 +825,42 @@ void reactRTCApproval( return; } +static std::string formatDecisionResult(const IntersectionModule::DecisionResult & decision_result) +{ + if (std::holds_alternative(decision_result)) { + const auto indecisive = std::get(decision_result); + return "Indecisive because " + indecisive.error; + } + if (std::holds_alternative(decision_result)) { + return "Safe"; + } + if (std::holds_alternative(decision_result)) { + return "StuckStop"; + } + if (std::holds_alternative(decision_result)) { + return "YieldStuckStop"; + } + if (std::holds_alternative(decision_result)) { + return "NonOccludedCollisionStop"; + } + if (std::holds_alternative(decision_result)) { + return "FirstWaitBeforeOcclusion"; + } + if (std::holds_alternative(decision_result)) { + return "PeekingTowardOcclusion"; + } + if (std::holds_alternative(decision_result)) { + return "OccludedCollisionStop"; + } + if (std::holds_alternative(decision_result)) { + return "OccludedAbsenceTrafficLight"; + } + if (std::holds_alternative(decision_result)) { + return "TrafficLightArrowSolidOn"; + } + return ""; +} + bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) { debug_data_ = util::DebugData(); @@ -649,32 +871,10 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * // calculate the const auto decision_result = modifyPathVelocityDetail(path, stop_reason); + prev_decision_result_ = decision_result; - std::string decision_type = "intersection" + std::to_string(module_id_) + " : "; - if (std::get_if(&decision_result)) { - decision_type += "Indecisive"; - } - if (std::get_if(&decision_result)) { - decision_type += "StuckStop"; - } - if (std::get_if(&decision_result)) { - decision_type += "NonOccludedCollisionStop"; - } - if (std::get_if(&decision_result)) { - decision_type += "FirstWaitBeforeOcclusion"; - } - if (std::get_if(&decision_result)) { - decision_type += "PeekingTowardOcclusion"; - } - if (std::get_if(&decision_result)) { - decision_type += "OccludedCollisionStop"; - } - if (std::get_if(&decision_result)) { - decision_type += "Safe"; - } - if (std::get_if(&decision_result)) { - decision_type += "TrafficLightArrowSolidOn"; - } + const std::string decision_type = + "intersection" + std::to_string(module_id_) + " : " + formatDecisionResult(decision_result); std_msgs::msg::String decision_result_msg; decision_result_msg.data = decision_type; decision_state_pub_->publish(decision_result_msg); @@ -695,6 +895,36 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * return true; } +static bool isGreenSolidOn( + lanelet::ConstLanelet lane, const std::map & tl_infos) +{ + using TrafficSignalElement = autoware_perception_msgs::msg::TrafficSignalElement; + + std::optional tl_id = std::nullopt; + for (auto && tl_reg_elem : lane.regulatoryElementsAs()) { + tl_id = tl_reg_elem->id(); + break; + } + if (!tl_id) { + // this lane has no traffic light + return false; + } + const auto tl_info_it = tl_infos.find(tl_id.value()); + if (tl_info_it == tl_infos.end()) { + // the info of this traffic light is not available + return false; + } + const auto & tl_info = tl_info_it->second; + for (auto && tl_light : tl_info.signal.elements) { + if ( + tl_light.color == TrafficSignalElement::GREEN && + tl_light.shape == TrafficSignalElement::CIRCLE) { + return true; + } + } + return false; +} + IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( PathWithLaneId * path, [[maybe_unused]] StopReason * stop_reason) { @@ -707,17 +937,15 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( const auto interpolated_path_info_opt = util::generateInterpolatedPath( lane_id_, associative_ids_, *path, planner_param_.common.path_interpolation_ds, logger_); if (!interpolated_path_info_opt) { - RCLCPP_DEBUG(logger_, "splineInterpolate failed"); - is_peeking_ = false; - return IntersectionModule::Indecisive{}; + return IntersectionModule::Indecisive{"splineInterpolate failed"}; } const auto & interpolated_path_info = interpolated_path_info_opt.value(); if (!interpolated_path_info.lane_id_interval) { - RCLCPP_WARN(logger_, "Path has no interval on intersection lane %ld", lane_id_); - is_peeking_ = false; - return IntersectionModule::Indecisive{}; + return IntersectionModule::Indecisive{ + "Path has no interval on intersection lane " + std::to_string(lane_id_)}; } + // cache intersection lane information because it is invariant const auto & current_pose = planner_data_->current_odometry->pose; const auto lanelets_on_path = planning_utils::getLaneletsOnPath(*path, lanelet_map_ptr, current_pose); @@ -728,83 +956,140 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( planner_param_.occlusion.occlusion_attention_area_length, planner_param_.common.consider_wrong_direction_vehicle); } - const bool tl_arrow_solid_on = - util::isTrafficLightArrowActivated(assigned_lanelet, planner_data_->traffic_light_id_map); - intersection_lanelets_.value().update(tl_arrow_solid_on, interpolated_path_info); - - const auto & conflicting_lanelets = intersection_lanelets_.value().conflicting(); - const auto & first_conflicting_area = intersection_lanelets_.value().first_conflicting_area(); - if (conflicting_lanelets.empty() || !first_conflicting_area) { - is_peeking_ = false; - return IntersectionModule::Indecisive{}; + auto & intersection_lanelets = intersection_lanelets_.value(); + + // at the very first time of registration of this module, the path may not be conflicting with the + // attention area, so update() is called to update the internal data as well as traffic light info + const auto traffic_prioritized_level = + util::getTrafficPrioritizedLevel(assigned_lanelet, planner_data_->traffic_light_id_map); + const bool is_prioritized = + traffic_prioritized_level == util::TrafficPrioritizedLevel::FULLY_PRIORITIZED; + const auto footprint = planner_data_->vehicle_info_.createFootprint(0.0, 0.0); + intersection_lanelets.update(is_prioritized, interpolated_path_info, footprint); + + // this is abnormal + const auto & conflicting_lanelets = intersection_lanelets.conflicting(); + const auto & first_conflicting_area_opt = intersection_lanelets.first_conflicting_area(); + if (conflicting_lanelets.empty() || !first_conflicting_area_opt) { + return IntersectionModule::Indecisive{"conflicting area is empty"}; } + const auto first_conflicting_area = first_conflicting_area_opt.value(); - const auto & first_attention_area = intersection_lanelets_.value().first_attention_area(); + // generate all stop line candidates + // see the doc for struct IntersectionStopLines + const auto & first_attention_area_opt = intersection_lanelets.first_attention_area(); + /// even if the attention area is null, stuck vehicle stop line needs to be generated from + /// conflicting lanes const auto & dummy_first_attention_area = - first_attention_area ? first_attention_area.value() : first_conflicting_area.value(); + first_attention_area_opt ? first_attention_area_opt.value() : first_conflicting_area; + const double peeking_offset = + has_traffic_light_ ? planner_param_.occlusion.peeking_offset + : planner_param_.occlusion.absence_traffic_light.maximum_peeking_distance; const auto intersection_stop_lines_opt = util::generateIntersectionStopLines( - first_conflicting_area.value(), dummy_first_attention_area, planner_data_, - interpolated_path_info, planner_param_.stuck_vehicle.use_stuck_stopline, - planner_param_.common.stop_line_margin, planner_param_.occlusion.peeking_offset, path); + first_conflicting_area, dummy_first_attention_area, planner_data_, interpolated_path_info, + planner_param_.stuck_vehicle.use_stuck_stopline, planner_param_.common.stop_line_margin, + planner_param_.common.max_accel, planner_param_.common.max_jerk, + planner_param_.common.delay_response_time, peeking_offset, path); if (!intersection_stop_lines_opt) { - RCLCPP_DEBUG(logger_, "failed to generate intersection_stop_lines"); - is_peeking_ = false; - return IntersectionModule::Indecisive{}; + return IntersectionModule::Indecisive{"failed to generate intersection_stop_lines"}; } const auto & intersection_stop_lines = intersection_stop_lines_opt.value(); const auto - [closest_idx, stuck_stop_line_idx, default_stop_line_idx, occlusion_peeking_stop_line_idx, - pass_judge_line_idx] = intersection_stop_lines; + [closest_idx, stuck_stop_line_idx_opt, default_stop_line_idx_opt, + first_attention_stop_line_idx_opt, occlusion_peeking_stop_line_idx_opt, pass_judge_line_idx] = + intersection_stop_lines; + // see the doc for struct PathLanelets + const auto & conflicting_area = intersection_lanelets.conflicting_area(); const auto path_lanelets_opt = util::generatePathLanelets( - lanelets_on_path, *path, associative_ids_, closest_idx, + lanelets_on_path, interpolated_path_info, associative_ids_, first_conflicting_area, + conflicting_area, first_attention_area_opt, intersection_lanelets.attention_area(), closest_idx, planner_data_->vehicle_info_.vehicle_width_m); if (!path_lanelets_opt.has_value()) { - RCLCPP_DEBUG(logger_, "failed to generate PathLanelets"); - return IntersectionModule::Indecisive{}; + return IntersectionModule::Indecisive{"failed to generate PathLanelets"}; } const auto path_lanelets = path_lanelets_opt.value(); - const auto ego_lane_with_next_lane = - path_lanelets.next.has_value() - ? std::vector< - lanelet::ConstLanelet>{path_lanelets.ego_or_entry2exit, path_lanelets.next.value()} - : std::vector{path_lanelets.ego_or_entry2exit}; - const bool stuck_detected = - checkStuckVehicle(planner_data_, ego_lane_with_next_lane, *path, intersection_stop_lines); - - if (stuck_detected) { - const double dist_stopline = motion_utils::calcSignedArcLength( - path->points, path->points.at(closest_idx).point.pose.position, - path->points.at(stuck_stop_line_idx).point.pose.position); - const bool approached_stop_line = + // utility functions + auto fromEgoDist = [&](const size_t index) { + return motion_utils::calcSignedArcLength(path->points, closest_idx, index); + }; + auto stoppedForDuration = + [&](const size_t pos, const double duration, StateMachine & state_machine) { + const double dist_stopline = fromEgoDist(pos); + const bool approached_dist_stopline = + (std::fabs(dist_stopline) < planner_param_.common.stop_overshoot_margin); + const bool over_stopline = (dist_stopline < 0.0); + const bool is_stopped_duration = planner_data_->isVehicleStopped(duration); + if (over_stopline) { + state_machine.setState(StateMachine::State::GO); + } else if (is_stopped_duration && approached_dist_stopline) { + state_machine.setState(StateMachine::State::GO); + } + return state_machine.getState() == StateMachine::State::GO; + }; + auto stoppedAtPosition = [&](const size_t pos, const double duration) { + const double dist_stopline = fromEgoDist(pos); + const bool approached_dist_stopline = (std::fabs(dist_stopline) < planner_param_.common.stop_overshoot_margin); - const bool is_stopped = planner_data_->isVehicleStopped(); - if (is_stopped && approached_stop_line) { - stuck_private_area_timeout_.setStateWithMarginTime( - StateMachine::State::GO, logger_.get_child("stuck_private_area_timeout"), *clock_); + const bool over_stopline = (dist_stopline < -planner_param_.common.stop_overshoot_margin); + const bool is_stopped = planner_data_->isVehicleStopped(duration); + if (over_stopline) { + return true; + } else if (is_stopped && approached_dist_stopline) { + return true; } - const bool timeout = - (is_private_area_ && stuck_private_area_timeout_.getState() == StateMachine::State::GO); - if (!timeout) { - is_peeking_ = false; + return false; + }; + + // stuck vehicle detection is viable even if attention area is empty + // so this needs to be checked before attention area validation + const bool stuck_detected = checkStuckVehicle(planner_data_, path_lanelets); + if (stuck_detected && stuck_stop_line_idx_opt) { + auto stuck_stop_line_idx = stuck_stop_line_idx_opt.value(); + if (is_private_area_ && planner_param_.stuck_vehicle.enable_private_area_stuck_disregard) { + if ( + default_stop_line_idx_opt && + fromEgoDist(stuck_stop_line_idx) < -planner_param_.common.stop_overshoot_margin) { + stuck_stop_line_idx = default_stop_line_idx_opt.value(); + } + } else { return IntersectionModule::StuckStop{ - stuck_stop_line_idx, !first_attention_area.has_value(), intersection_stop_lines}; + closest_idx, stuck_stop_line_idx, occlusion_peeking_stop_line_idx_opt}; } } - if (!first_attention_area) { - RCLCPP_DEBUG(logger_, "attention area is empty"); - is_peeking_ = false; - return IntersectionModule::Indecisive{}; + // yield stuck vehicle detection is viable even if attention area is empty + // so this needs to be checked before attention area validation + const bool yield_stuck_detected = checkYieldStuckVehicle( + planner_data_, path_lanelets, intersection_lanelets.first_attention_area()); + if (yield_stuck_detected && stuck_stop_line_idx_opt) { + auto stuck_stop_line_idx = stuck_stop_line_idx_opt.value(); + if (is_private_area_ && planner_param_.stuck_vehicle.enable_private_area_stuck_disregard) { + if ( + default_stop_line_idx_opt && + fromEgoDist(stuck_stop_line_idx) < -planner_param_.common.stop_overshoot_margin) { + stuck_stop_line_idx = default_stop_line_idx_opt.value(); + } + } else { + return IntersectionModule::YieldStuckStop{closest_idx, stuck_stop_line_idx}; + } } - if (default_stop_line_idx == 0) { - RCLCPP_DEBUG(logger_, "stop line index is 0"); - is_peeking_ = false; - return IntersectionModule::Indecisive{}; + // if attention area is empty, collision/occlusion detection is impossible + if (!first_attention_area_opt) { + return IntersectionModule::Indecisive{"attention area is empty"}; } + const auto first_attention_area = first_attention_area_opt.value(); + // if attention area is not null but default stop line is not available, ego/backward-path has + // already passed the stop line + if (!default_stop_line_idx_opt) { + return IntersectionModule::Indecisive{"default stop line is null"}; + } + const auto default_stop_line_idx = default_stop_line_idx_opt.value(); + + // TODO(Mamoru Sobue): this part needs more formal handling const double baselink2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; debug_data_.pass_judge_wall_pose = planning_utils::getAheadPose(pass_judge_line_idx, baselink2front, *path); @@ -817,184 +1102,296 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( planner_data_->current_velocity->twist.linear.y); const bool keep_detection = (vel_norm < planner_param_.collision_detection.keep_detection_vel_thr); + const bool was_safe = std::holds_alternative(prev_decision_result_); // if ego is over the pass judge line and not stopped - if (is_peeking_) { - // do nothing - RCLCPP_DEBUG(logger_, "peeking now"); - } else if (is_over_default_stop_line && !is_over_pass_judge_line && keep_detection) { + if (is_over_default_stop_line && !is_over_pass_judge_line && keep_detection) { RCLCPP_DEBUG( logger_, "is_over_default_stop_line && !is_over_pass_judge_line && keep_detection"); // do nothing } else if ( - (is_over_default_stop_line && is_over_pass_judge_line && is_go_out_) || is_permanent_go_) { + (was_safe && is_over_default_stop_line && is_over_pass_judge_line && is_go_out_) || + is_permanent_go_) { // is_go_out_: previous RTC approval // activated_: current RTC approval is_permanent_go_ = true; - RCLCPP_DEBUG(logger_, "over the pass judge line. no plan needed."); - is_peeking_ = false; - return IntersectionModule::Indecisive{}; + return IntersectionModule::Indecisive{"over the pass judge line. no plan needed"}; } - const auto & attention_lanelets = intersection_lanelets_.value().attention(); - const auto & adjacent_lanelets = intersection_lanelets_.value().adjacent(); - const auto & occlusion_attention_lanelets = intersection_lanelets_.value().occlusion_attention(); - const auto & occlusion_attention_area = intersection_lanelets_.value().occlusion_attention_area(); - debug_data_.attention_area = intersection_lanelets_.value().attention_area(); - debug_data_.adjacent_area = intersection_lanelets_.value().adjacent_area(); + // occlusion stop line is generated from the intersection of ego footprint along the path with the + // attention area, so if this is null, eog has already passed the intersection + if (!first_attention_stop_line_idx_opt || !occlusion_peeking_stop_line_idx_opt) { + return IntersectionModule::Indecisive{"occlusion stop line is null"}; + } + const auto collision_stop_line_idx = + is_over_default_stop_line ? closest_idx : default_stop_line_idx; + const auto first_attention_stop_line_idx = first_attention_stop_line_idx_opt.value(); + const auto occlusion_stop_line_idx = occlusion_peeking_stop_line_idx_opt.value(); + + const auto & adjacent_lanelets = intersection_lanelets.adjacent(); + const auto & occlusion_attention_lanelets = intersection_lanelets.occlusion_attention(); + const auto & occlusion_attention_area = intersection_lanelets.occlusion_attention_area(); + debug_data_.attention_area = intersection_lanelets.attention_area(); + debug_data_.occlusion_attention_area = occlusion_attention_area; + debug_data_.adjacent_area = intersection_lanelets.adjacent_area(); // get intersection area - const auto intersection_area = planner_param_.common.use_intersection_area - ? util::getIntersectionArea(assigned_lanelet, lanelet_map_ptr) - : std::nullopt; - if (intersection_area) { - const auto intersection_area_2d = intersection_area.value(); - debug_data_.intersection_area = toGeomPoly(intersection_area_2d); - } - - // calculate dynamic collision around detection area - const double time_delay = (is_go_out_ || tl_arrow_solid_on) - ? 0.0 - : (planner_param_.collision_detection.state_transit_margin_time - - collision_state_machine_.getDuration()); - const auto target_objects = - filterTargetObjects(attention_lanelets, adjacent_lanelets, intersection_area); + const auto intersection_area = util::getIntersectionArea(assigned_lanelet, lanelet_map_ptr); + + auto target_objects = generateTargetObjects(intersection_lanelets, intersection_area); + + // If there are any vehicles on the attention area when ego entered the intersection on green + // light, do pseudo collision detection because the vehicles are very slow and no collisions may + // be detected. check if ego vehicle entered assigned lanelet + const bool is_green_solid_on = + isGreenSolidOn(assigned_lanelet, planner_data_->traffic_light_id_map); + if (is_green_solid_on) { + if (!initial_green_light_observed_time_) { + const auto assigned_lane_begin_point = assigned_lanelet.centerline().front(); + const bool approached_assigned_lane = + motion_utils::calcSignedArcLength( + path->points, closest_idx, + tier4_autoware_utils::createPoint( + assigned_lane_begin_point.x(), assigned_lane_begin_point.y(), + assigned_lane_begin_point.z())) < + planner_param_.collision_detection.yield_on_green_traffic_light + .distance_to_assigned_lanelet_start; + if (approached_assigned_lane) { + initial_green_light_observed_time_ = clock_->now(); + } + } + if (initial_green_light_observed_time_) { + const auto now = clock_->now(); + const bool exist_close_vehicles = std::any_of( + target_objects.all_attention_objects.begin(), target_objects.all_attention_objects.end(), + [&](const auto & object) { + return object.dist_to_stop_line.has_value() && + object.dist_to_stop_line.value() < + planner_param_.collision_detection.yield_on_green_traffic_light + .object_dist_to_stopline; + }); + if ( + exist_close_vehicles && + rclcpp::Duration((now - initial_green_light_observed_time_.value())).seconds() < + planner_param_.collision_detection.yield_on_green_traffic_light.duration) { + return IntersectionModule::NonOccludedCollisionStop{ + closest_idx, collision_stop_line_idx, occlusion_stop_line_idx}; + } + } + } + + // calculate dynamic collision around attention area + const double time_to_restart = (is_go_out_ || is_prioritized) + ? 0.0 + : (planner_param_.collision_detection.state_transit_margin_time - + collision_state_machine_.getDuration()); const bool has_collision = checkCollision( - *path, target_objects, path_lanelets, closest_idx, time_delay, tl_arrow_solid_on); + *path, &target_objects, path_lanelets, closest_idx, + std::min(occlusion_stop_line_idx, path->points.size() - 1), time_to_restart, + traffic_prioritized_level); collision_state_machine_.setStateWithMarginTime( has_collision ? StateMachine::State::STOP : StateMachine::State::GO, logger_.get_child("collision state_machine"), *clock_); const bool has_collision_with_margin = collision_state_machine_.getState() == StateMachine::State::STOP; - if (tl_arrow_solid_on) { - is_peeking_ = false; - return TrafficLightArrowSolidOn{has_collision, intersection_stop_lines}; + if (is_prioritized) { + return TrafficLightArrowSolidOn{ + has_collision_with_margin, closest_idx, collision_stop_line_idx, occlusion_stop_line_idx}; } // check occlusion on detection lane if (!occlusion_attention_divisions_) { occlusion_attention_divisions_ = util::generateDetectionLaneDivisions( occlusion_attention_lanelets, routing_graph_ptr, - planner_data_->occupancy_grid->info.resolution / std::sqrt(2.0)); + planner_data_->occupancy_grid->info.resolution, + planner_param_.occlusion.attention_lane_crop_curvature_threshold, + planner_param_.occlusion.attention_lane_curvature_calculation_ds); } + const auto & occlusion_attention_divisions = occlusion_attention_divisions_.value(); const double occlusion_dist_thr = std::fabs( std::pow(planner_param_.occlusion.max_vehicle_velocity_for_rss, 2) / (2 * planner_param_.occlusion.min_vehicle_brake_for_rss)); - std::vector parked_attention_objects; - std::copy_if( - target_objects.objects.begin(), target_objects.objects.end(), - std::back_inserter(parked_attention_objects), - [thresh = planner_param_.occlusion.ignore_parked_vehicle_speed_threshold](const auto & object) { - return std::hypot( - object.kinematics.initial_twist_with_covariance.twist.linear.x, - object.kinematics.initial_twist_with_covariance.twist.linear.y) <= thresh; - }); - const bool is_occlusion_cleared = - (enable_occlusion_detection_ && !occlusion_attention_lanelets.empty() && !tl_arrow_solid_on) - ? isOcclusionCleared( + auto occlusion_status = + (enable_occlusion_detection_ && !occlusion_attention_lanelets.empty() && !is_prioritized) + ? getOcclusionStatus( *planner_data_->occupancy_grid, occlusion_attention_area, adjacent_lanelets, - first_attention_area.value(), interpolated_path_info, - occlusion_attention_divisions_.value(), parked_attention_objects, occlusion_dist_thr) - : true; - - // check safety - const bool ext_occlusion_requested = (is_occlusion_cleared && !occlusion_activated_); - if (!is_occlusion_cleared || ext_occlusion_requested) { - const double dist_stopline = motion_utils::calcSignedArcLength( - path->points, path->points.at(closest_idx).point.pose.position, - path->points.at(default_stop_line_idx).point.pose.position); - const bool approached_stop_line = - (std::fabs(dist_stopline) < planner_param_.common.stop_overshoot_margin); - const bool over_stop_line = (dist_stopline < 0.0); - const bool is_stopped = - planner_data_->isVehicleStopped(planner_param_.occlusion.before_creep_stop_time); - if (over_stop_line) { - before_creep_state_machine_.setState(StateMachine::State::GO); - } - if (before_creep_state_machine_.getState() == StateMachine::State::GO) { - if (has_collision) { - is_peeking_ = true; - return IntersectionModule::OccludedCollisionStop{ - default_stop_line_idx, occlusion_peeking_stop_line_idx, is_occlusion_cleared, - intersection_stop_lines}; - } else { - is_peeking_ = true; - return IntersectionModule::PeekingTowardOcclusion{ - occlusion_peeking_stop_line_idx, is_occlusion_cleared, intersection_stop_lines}; + first_attention_area, interpolated_path_info, occlusion_attention_divisions, + target_objects, current_pose, occlusion_dist_thr) + : OcclusionType::NOT_OCCLUDED; + occlusion_stop_state_machine_.setStateWithMarginTime( + occlusion_status == OcclusionType::NOT_OCCLUDED ? StateMachine::State::GO : StateMachine::STOP, + logger_.get_child("occlusion_stop"), *clock_); + const bool is_occlusion_cleared_with_margin = + (occlusion_stop_state_machine_.getState() == StateMachine::State::GO); + // distinguish if ego detected occlusion or RTC detects occlusion + const bool ext_occlusion_requested = (is_occlusion_cleared_with_margin && !occlusion_activated_); + if (ext_occlusion_requested) { + occlusion_status = OcclusionType::RTC_OCCLUDED; + } + const bool is_occlusion_state = (!is_occlusion_cleared_with_margin || ext_occlusion_requested); + if (is_occlusion_state && occlusion_status == OcclusionType::NOT_OCCLUDED) { + occlusion_status = prev_occlusion_status_; + } else { + prev_occlusion_status_ = occlusion_status; + } + + // Safe + if (!is_occlusion_state && !has_collision_with_margin) { + return IntersectionModule::Safe{closest_idx, collision_stop_line_idx, occlusion_stop_line_idx}; + } + // Only collision + if (!is_occlusion_state && has_collision_with_margin) { + return IntersectionModule::NonOccludedCollisionStop{ + closest_idx, collision_stop_line_idx, occlusion_stop_line_idx}; + } + // Occluded + // occlusion_status is assured to be not NOT_OCCLUDED + const bool stopped_at_default_line = stoppedForDuration( + default_stop_line_idx, planner_param_.occlusion.before_creep_stop_time, + before_creep_state_machine_); + if (stopped_at_default_line) { + // if specified the parameter occlusion.temporal_stop_before_attention_area OR + // has_no_traffic_light_, ego will temporarily stop before entering attention area + const bool temporal_stop_before_attention_required = + (planner_param_.occlusion.temporal_stop_before_attention_area || !has_traffic_light_) + ? !stoppedForDuration( + first_attention_stop_line_idx, planner_param_.occlusion.before_creep_stop_time, + temporal_stop_before_attention_state_machine_) + : false; + if (!has_traffic_light_) { + if (fromEgoDist(first_attention_stop_line_idx) <= -peeking_offset) { + return IntersectionModule::Indecisive{ + "already passed maximum peeking line in the absence of traffic light"}; } + return IntersectionModule::OccludedAbsenceTrafficLight{ + is_occlusion_cleared_with_margin, has_collision_with_margin, + temporal_stop_before_attention_required, closest_idx, + first_attention_stop_line_idx, occlusion_stop_line_idx}; + } + // following remaining block is "has_traffic_light_" + // if ego is stuck by static occlusion in the presence of traffic light, start timeout count + const bool is_static_occlusion = occlusion_status == OcclusionType::STATICALLY_OCCLUDED; + const bool is_stuck_by_static_occlusion = + stoppedAtPosition(occlusion_stop_line_idx, planner_param_.occlusion.before_creep_stop_time) && + is_static_occlusion; + if (has_collision_with_margin) { + // if collision is detected, timeout is reset + static_occlusion_timeout_state_machine_.setState(StateMachine::State::STOP); + } else if (is_stuck_by_static_occlusion) { + static_occlusion_timeout_state_machine_.setStateWithMarginTime( + StateMachine::State::GO, logger_.get_child("static_occlusion"), *clock_); + } + const bool release_static_occlusion_stuck = + (static_occlusion_timeout_state_machine_.getState() == StateMachine::State::GO); + if (!has_collision_with_margin && release_static_occlusion_stuck) { + return IntersectionModule::Safe{ + closest_idx, collision_stop_line_idx, occlusion_stop_line_idx}; + } + // occlusion_status is either STATICALLY_OCCLUDED or DYNAMICALLY_OCCLUDED + const double max_timeout = + planner_param_.occlusion.static_occlusion_with_traffic_light_timeout + + planner_param_.occlusion.stop_release_margin_time; + const std::optional static_occlusion_timeout = + is_stuck_by_static_occlusion + ? std::make_optional( + max_timeout - static_occlusion_timeout_state_machine_.getDuration() - + occlusion_stop_state_machine_.getDuration()) + : (is_static_occlusion ? std::make_optional(max_timeout) : std::nullopt); + if (has_collision_with_margin) { + return IntersectionModule::OccludedCollisionStop{is_occlusion_cleared_with_margin, + temporal_stop_before_attention_required, + closest_idx, + collision_stop_line_idx, + first_attention_stop_line_idx, + occlusion_stop_line_idx, + static_occlusion_timeout}; } else { - if (is_stopped && approached_stop_line) { - // start waiting at the first stop line - before_creep_state_machine_.setState(StateMachine::State::GO); - } - is_peeking_ = true; - return IntersectionModule::FirstWaitBeforeOcclusion{ - default_stop_line_idx, occlusion_peeking_stop_line_idx, is_occlusion_cleared, - intersection_stop_lines}; + return IntersectionModule::PeekingTowardOcclusion{is_occlusion_cleared_with_margin, + temporal_stop_before_attention_required, + closest_idx, + collision_stop_line_idx, + first_attention_stop_line_idx, + occlusion_stop_line_idx, + static_occlusion_timeout}; } - } else if (has_collision_with_margin) { - const bool is_over_default_stopLine = - util::isOverTargetIndex(*path, closest_idx, current_pose, default_stop_line_idx); - const auto stop_line_idx = is_over_default_stopLine ? closest_idx : default_stop_line_idx; - is_peeking_ = false; - return IntersectionModule::NonOccludedCollisionStop{stop_line_idx, intersection_stop_lines}; + } else { + const auto occlusion_stop_line = + (planner_param_.occlusion.temporal_stop_before_attention_area || !has_traffic_light_) + ? first_attention_stop_line_idx + : occlusion_stop_line_idx; + return IntersectionModule::FirstWaitBeforeOcclusion{ + is_occlusion_cleared_with_margin, closest_idx, default_stop_line_idx, occlusion_stop_line}; } - - is_peeking_ = false; - return IntersectionModule::Safe{intersection_stop_lines}; } bool IntersectionModule::checkStuckVehicle( - const std::shared_ptr & planner_data, - const lanelet::ConstLanelets & ego_lane_with_next_lane, - const autoware_auto_planning_msgs::msg::PathWithLaneId & input_path, - const util::IntersectionStopLines & intersection_stop_lines) + const std::shared_ptr & planner_data, const util::PathLanelets & path_lanelets) { + const bool stuck_detection_direction = [&]() { + return (turn_direction_ == "left" && planner_param_.stuck_vehicle.turn_direction.left) || + (turn_direction_ == "right" && planner_param_.stuck_vehicle.turn_direction.right) || + (turn_direction_ == "straight" && planner_param_.stuck_vehicle.turn_direction.straight); + }(); + if (!stuck_detection_direction) { + return false; + } + const auto & objects_ptr = planner_data->predicted_objects; - const geometry_msgs::msg::Pose & current_pose = planner_data->current_odometry->pose; - const auto closest_idx = intersection_stop_lines.closest_idx; - const auto stuck_line_idx = intersection_stop_lines.stuck_stop_line; // considering lane change in the intersection, these lanelets are generated from the path - const auto ego_lane = ego_lane_with_next_lane.front(); - debug_data_.ego_lane = ego_lane.polygon3d(); const auto stuck_vehicle_detect_area = util::generateStuckVehicleDetectAreaPolygon( - input_path, ego_lane_with_next_lane, closest_idx, - planner_param_.stuck_vehicle.stuck_vehicle_detect_dist, - planner_param_.stuck_vehicle.stuck_vehicle_ignore_dist, - planner_data->vehicle_info_.vehicle_length_m); + path_lanelets, planner_param_.stuck_vehicle.stuck_vehicle_detect_dist); debug_data_.stuck_vehicle_detect_area = toGeomPoly(stuck_vehicle_detect_area); - const double dist_stuck_stopline = motion_utils::calcSignedArcLength( - input_path.points, input_path.points.at(stuck_line_idx).point.pose.position, - input_path.points.at(closest_idx).point.pose.position); - const bool is_over_stuck_stopline = - util::isOverTargetIndex(input_path, closest_idx, current_pose, stuck_line_idx) && - (dist_stuck_stopline > planner_param_.common.stop_overshoot_margin); + return util::checkStuckVehicleInIntersection( + objects_ptr, stuck_vehicle_detect_area, planner_param_.stuck_vehicle.stuck_vehicle_vel_thr, + &debug_data_); +} - bool is_stuck = false; - if (!is_over_stuck_stopline) { - is_stuck = util::checkStuckVehicleInIntersection( - objects_ptr, stuck_vehicle_detect_area, planner_param_.stuck_vehicle.stuck_vehicle_vel_thr, - &debug_data_); +bool IntersectionModule::checkYieldStuckVehicle( + const std::shared_ptr & planner_data, const util::PathLanelets & path_lanelets, + const std::optional & first_attention_area) +{ + if (!first_attention_area) { + return false; + } + + const bool yield_stuck_detection_direction = [&]() { + return (turn_direction_ == "left" && + planner_param_.stuck_vehicle.yield_stuck_turn_direction.left) || + (turn_direction_ == "right" && + planner_param_.stuck_vehicle.yield_stuck_turn_direction.right) || + (turn_direction_ == "straight" && + planner_param_.stuck_vehicle.yield_stuck_turn_direction.straight); + }(); + if (!yield_stuck_detection_direction) { + return false; } - return is_stuck; + + const auto & objects_ptr = planner_data->predicted_objects; + + const auto & ego_lane = path_lanelets.ego_or_entry2exit; + const auto ego_poly = ego_lane.polygon2d().basicPolygon(); + + return util::checkYieldStuckVehicleInIntersection( + objects_ptr, ego_poly, first_attention_area.value(), + planner_param_.stuck_vehicle.stuck_vehicle_vel_thr, + planner_param_.stuck_vehicle.yield_stuck_distance_thr, &debug_data_); } -autoware_auto_perception_msgs::msg::PredictedObjects IntersectionModule::filterTargetObjects( - const lanelet::ConstLanelets & attention_area_lanelets, - const lanelet::ConstLanelets & adjacent_lanelets, +util::TargetObjects IntersectionModule::generateTargetObjects( + const util::IntersectionLanelets & intersection_lanelets, const std::optional & intersection_area) const { - using lanelet::utils::getArcCoordinates; - using lanelet::utils::getPolygonFromArcLength; - const auto & objects_ptr = planner_data_->predicted_objects; // extract target objects - autoware_auto_perception_msgs::msg::PredictedObjects target_objects; + util::TargetObjects target_objects; target_objects.header = objects_ptr->header; + const auto & attention_lanelets = intersection_lanelets.attention(); + const auto & attention_lanelet_stoplines = intersection_lanelets.attention_stop_lines(); + const auto & adjacent_lanelets = intersection_lanelets.adjacent(); for (const auto & object : objects_ptr->objects) { // ignore non-vehicle type objects, such as pedestrian. if (!isTargetCollisionVehicleType(object)) { @@ -1003,44 +1400,73 @@ autoware_auto_perception_msgs::msg::PredictedObjects IntersectionModule::filterT // check direction of objects const auto object_direction = util::getObjectPoseWithVelocityDirection(object.kinematics); - const auto is_in_adjacent_lanelets = util::checkAngleForTargetLanelets( + const auto belong_adjacent_lanelet_id = util::checkAngleForTargetLanelets( object_direction, adjacent_lanelets, planner_param_.common.attention_area_angle_thr, planner_param_.common.consider_wrong_direction_vehicle, - planner_param_.common.attention_area_margin); - if (is_in_adjacent_lanelets) { + planner_param_.common.attention_area_margin, false); + if (belong_adjacent_lanelet_id) { continue; } + const auto is_parked_vehicle = + std::fabs(object.kinematics.initial_twist_with_covariance.twist.linear.x) < + planner_param_.occlusion.ignore_parked_vehicle_speed_threshold; + auto & container = is_parked_vehicle ? target_objects.parked_attention_objects + : target_objects.attention_objects; if (intersection_area) { + const auto & obj_pos = object.kinematics.initial_pose_with_covariance.pose.position; 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); - if (is_in_intersection_area) { - target_objects.objects.push_back(object); - } else if (util::checkAngleForTargetLanelets( - object_direction, attention_area_lanelets, - planner_param_.common.attention_area_angle_thr, - planner_param_.common.consider_wrong_direction_vehicle, - planner_param_.common.attention_area_margin)) { - target_objects.objects.push_back(object); + const auto belong_attention_lanelet_id = util::checkAngleForTargetLanelets( + object_direction, attention_lanelets, planner_param_.common.attention_area_angle_thr, + planner_param_.common.consider_wrong_direction_vehicle, + planner_param_.common.attention_area_margin, is_parked_vehicle); + if (belong_attention_lanelet_id) { + const auto id = belong_attention_lanelet_id.value(); + util::TargetObject target_object; + target_object.object = object; + target_object.attention_lanelet = attention_lanelets.at(id); + target_object.stop_line = attention_lanelet_stoplines.at(id); + container.push_back(target_object); + } else if (bg::within(Point2d{obj_pos.x, obj_pos.y}, intersection_area_2d)) { + util::TargetObject target_object; + target_object.object = object; + target_object.attention_lanelet = std::nullopt; + target_object.stop_line = std::nullopt; + target_objects.intersection_area_objects.push_back(target_object); } - } else if (util::checkAngleForTargetLanelets( - object_direction, attention_area_lanelets, + } else if (const auto belong_attention_lanelet_id = util::checkAngleForTargetLanelets( + object_direction, attention_lanelets, planner_param_.common.attention_area_angle_thr, planner_param_.common.consider_wrong_direction_vehicle, - planner_param_.common.attention_area_margin)) { + planner_param_.common.attention_area_margin, is_parked_vehicle); + belong_attention_lanelet_id.has_value()) { // intersection_area is not available, use detection_area_with_margin as before - target_objects.objects.push_back(object); + const auto id = belong_attention_lanelet_id.value(); + util::TargetObject target_object; + target_object.object = object; + target_object.attention_lanelet = attention_lanelets.at(id); + target_object.stop_line = attention_lanelet_stoplines.at(id); + container.push_back(target_object); } } + for (const auto & object : target_objects.attention_objects) { + target_objects.all_attention_objects.push_back(object); + } + for (const auto & object : target_objects.parked_attention_objects) { + target_objects.all_attention_objects.push_back(object); + } + for (auto & object : target_objects.all_attention_objects) { + object.calc_dist_to_stop_line(); + } return target_objects; } bool IntersectionModule::checkCollision( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const autoware_auto_perception_msgs::msg::PredictedObjects & objects, - const util::PathLanelets & path_lanelets, const int closest_idx, const double time_delay, - const bool tl_arrow_solid_on) + util::TargetObjects * target_objects, const util::PathLanelets & path_lanelets, + const size_t closest_idx, const size_t last_intersection_stop_line_candidate_idx, + const double time_delay, const util::TrafficPrioritizedLevel & traffic_prioritized_level) { using lanelet::utils::getArcCoordinates; using lanelet::utils::getPolygonFromArcLength; @@ -1048,28 +1474,110 @@ bool IntersectionModule::checkCollision( // check collision between target_objects predicted path and ego lane // cut the predicted path at passing_time const auto time_distance_array = util::calcIntersectionPassingTime( - path, planner_data_, associative_ids_, closest_idx, time_delay, - planner_param_.common.intersection_velocity, - planner_param_.collision_detection.minimum_ego_predicted_velocity); + path, planner_data_, associative_ids_, closest_idx, last_intersection_stop_line_candidate_idx, + time_delay, planner_param_.common.intersection_velocity, + planner_param_.collision_detection.minimum_ego_predicted_velocity, + planner_param_.collision_detection.use_upstream_velocity, + planner_param_.collision_detection.minimum_upstream_velocity); const double passing_time = time_distance_array.back().first; - auto target_objects = objects; - util::cutPredictPathWithDuration(&target_objects, clock_, passing_time); + util::cutPredictPathWithDuration(target_objects, clock_, passing_time); const auto & concat_lanelets = path_lanelets.all; const auto closest_arc_coords = getArcCoordinates( concat_lanelets, tier4_autoware_utils::getPose(path.points.at(closest_idx).point)); const auto & ego_lane = path_lanelets.ego_or_entry2exit; - + debug_data_.ego_lane = ego_lane.polygon3d(); const auto ego_poly = ego_lane.polygon2d().basicPolygon(); + + // change TTC margin based on ego traffic light color + const auto [collision_start_margin_time, collision_end_margin_time] = [&]() { + if (traffic_prioritized_level == util::TrafficPrioritizedLevel::FULLY_PRIORITIZED) { + return std::make_pair( + planner_param_.collision_detection.fully_prioritized.collision_start_margin_time, + planner_param_.collision_detection.fully_prioritized.collision_end_margin_time); + } + if (traffic_prioritized_level == util::TrafficPrioritizedLevel::PARTIALLY_PRIORITIZED) { + return std::make_pair( + planner_param_.collision_detection.partially_prioritized.collision_start_margin_time, + planner_param_.collision_detection.partially_prioritized.collision_end_margin_time); + } + return std::make_pair( + planner_param_.collision_detection.not_prioritized.collision_start_margin_time, + planner_param_.collision_detection.not_prioritized.collision_end_margin_time); + }(); + const auto expectedToStopBeforeStopLine = [&](const util::TargetObject & target_object) { + if (!target_object.dist_to_stop_line) { + return false; + } + const double dist_to_stop_line = target_object.dist_to_stop_line.value(); + if (dist_to_stop_line < 0) { + return false; + } + const double v = target_object.object.kinematics.initial_twist_with_covariance.twist.linear.x; + const double braking_distance = + v * v / + (2.0 * std::fabs(planner_param_.collision_detection.ignore_on_amber_traffic_light + .object_expected_deceleration)); + return dist_to_stop_line > braking_distance; + }; + const auto isTolerableOvershoot = [&](const util::TargetObject & target_object) { + if ( + !target_object.attention_lanelet || !target_object.dist_to_stop_line || + !target_object.stop_line) { + return false; + } + const double dist_to_stop_line = target_object.dist_to_stop_line.value(); + const double v = target_object.object.kinematics.initial_twist_with_covariance.twist.linear.x; + const double braking_distance = + v * v / + (2.0 * std::fabs(planner_param_.collision_detection.ignore_on_amber_traffic_light + .object_expected_deceleration)); + if (dist_to_stop_line > braking_distance) { + return false; + } + const auto stopline_front = target_object.stop_line.value().front(); + const auto stopline_back = target_object.stop_line.value().back(); + tier4_autoware_utils::LineString2d object_line; + object_line.emplace_back( + (stopline_front.x() + stopline_back.x()) / 2.0, + (stopline_front.y() + stopline_back.y()) / 2.0); + const auto stopline_mid = object_line.front(); + const auto endpoint = target_object.attention_lanelet.value().centerline().back(); + object_line.emplace_back(endpoint.x(), endpoint.y()); + std::vector intersections; + bg::intersection(object_line, ego_lane.centerline2d().basicLineString(), intersections); + if (intersections.empty()) { + return false; + } + const auto collision_point = intersections.front(); + // distance from object expected stop position to collision point + const double stopline_to_object = -1.0 * dist_to_stop_line + braking_distance; + const double stopline_to_collision = + std::hypot(collision_point.x() - stopline_mid.x(), collision_point.y() - stopline_mid.y()); + const double object2collision = stopline_to_collision - stopline_to_object; + const double margin = + planner_param_.collision_detection.ignore_on_red_traffic_light.object_margin_to_path; + return (object2collision > margin) || (object2collision < 0); + }; // check collision between predicted_path and ego_area - const double collision_start_margin_time = - tl_arrow_solid_on ? planner_param_.collision_detection.relaxed.collision_start_margin_time - : planner_param_.collision_detection.normal.collision_start_margin_time; - const double collision_end_margin_time = - tl_arrow_solid_on ? planner_param_.collision_detection.relaxed.collision_end_margin_time - : planner_param_.collision_detection.normal.collision_end_margin_time; bool collision_detected = false; - for (const auto & object : target_objects.objects) { + for (const auto & target_object : target_objects->all_attention_objects) { + const auto & object = target_object.object; + // If the vehicle is expected to stop before their stopline, ignore + const bool expected_to_stop_before_stopline = expectedToStopBeforeStopLine(target_object); + if ( + traffic_prioritized_level == util::TrafficPrioritizedLevel::PARTIALLY_PRIORITIZED && + expected_to_stop_before_stopline) { + debug_data_.amber_ignore_targets.objects.push_back(object); + continue; + } + const bool is_tolerable_overshoot = isTolerableOvershoot(target_object); + if ( + traffic_prioritized_level == util::TrafficPrioritizedLevel::FULLY_PRIORITIZED && + !expected_to_stop_before_stopline && is_tolerable_overshoot) { + debug_data_.red_overshoot_ignore_targets.objects.push_back(object); + continue; + } for (const auto & predicted_path : object.kinematics.predicted_paths) { if ( predicted_path.confidence < @@ -1081,14 +1589,14 @@ bool IntersectionModule::checkCollision( // collision point const auto first_itr = std::adjacent_find( predicted_path.path.cbegin(), predicted_path.path.cend(), - [&ego_poly](const auto & a, const auto & b) { - return bg::intersects(ego_poly, LineString2d{to_bg2d(a), to_bg2d(b)}); + [&ego_poly, &object](const auto & a, const auto & b) { + return bg::intersects(ego_poly, createOneStepPolygon(a, b, object.shape)); }); if (first_itr == predicted_path.path.cend()) continue; const auto last_itr = std::adjacent_find( predicted_path.path.crbegin(), predicted_path.path.crend(), - [&ego_poly](const auto & a, const auto & b) { - return bg::intersects(ego_poly, LineString2d{to_bg2d(a), to_bg2d(b)}); + [&ego_poly, &object](const auto & a, const auto & b) { + return bg::intersects(ego_poly, createOneStepPolygon(a, b, object.shape)); }); if (last_itr == predicted_path.path.crend()) continue; @@ -1160,14 +1668,14 @@ bool IntersectionModule::checkCollision( return collision_detected; } -bool IntersectionModule::isOcclusionCleared( +IntersectionModule::OcclusionType IntersectionModule::getOcclusionStatus( const nav_msgs::msg::OccupancyGrid & occ_grid, const std::vector & attention_areas, const lanelet::ConstLanelets & adjacent_lanelets, const lanelet::CompoundPolygon3d & first_attention_area, const util::InterpolatedPathInfo & interpolated_path_info, - const std::vector & lane_divisions, - const std::vector & parked_attention_objects, + const std::vector & lane_divisions, + const util::TargetObjects & target_objects, const geometry_msgs::msg::Pose & current_pose, const double occlusion_dist_thr) { const auto & path_ip = interpolated_path_info.path; @@ -1176,7 +1684,7 @@ bool IntersectionModule::isOcclusionCleared( const auto first_attention_area_idx = util::getFirstPointInsidePolygon(path_ip, lane_interval_ip, first_attention_area); if (!first_attention_area_idx) { - return false; + return OcclusionType::NOT_OCCLUDED; } const auto first_inside_attention_idx_ip_opt = @@ -1185,19 +1693,20 @@ bool IntersectionModule::isOcclusionCleared( first_inside_attention_idx_ip_opt ? std::make_pair(first_inside_attention_idx_ip_opt.value(), std::get<1>(lane_interval_ip)) : lane_interval_ip; + const auto [lane_start_idx, lane_end_idx] = lane_attention_interval_ip; const int width = occ_grid.info.width; const int height = occ_grid.info.height; const double resolution = occ_grid.info.resolution; const auto & origin = occ_grid.info.origin.position; + auto coord2index = [&](const double x, const double y) { + const int idx_x = (x - origin.x) / resolution; + const int idx_y = (y - origin.y) / resolution; + if (idx_x < 0 || idx_x >= width) return std::make_tuple(false, -1, -1); + if (idx_y < 0 || idx_y >= height) return std::make_tuple(false, -1, -1); + return std::make_tuple(true, idx_x, idx_y); + }; - // NOTE: interesting area is set to 0 for later masking - cv::Mat attention_mask(width, height, CV_8UC1, cv::Scalar(0)); - cv::Mat unknown_mask(width, height, CV_8UC1, cv::Scalar(0)); - - // (1) prepare detection area mask - // attention: 255 - // non-attention: 0 Polygon2d grid_poly; grid_poly.outer().emplace_back(origin.x, origin.y); grid_poly.outer().emplace_back(origin.x + (width - 1) * resolution, origin.y); @@ -1207,10 +1716,9 @@ bool IntersectionModule::isOcclusionCleared( grid_poly.outer().emplace_back(origin.x, origin.y); bg::correct(grid_poly); - std::vector> attention_area_cv_polygons; - for (const auto & attention_area : attention_areas) { - const auto area2d = lanelet::utils::to2D(attention_area); - Polygon2d area2d_poly; + auto findCommonCvPolygons = + [&](const auto & area2d, std::vector> & cv_polygons) -> void { + tier4_autoware_utils::Polygon2d area2d_poly; for (const auto & p : area2d) { area2d_poly.outer().emplace_back(p.x(), p.y()); } @@ -1219,21 +1727,32 @@ bool IntersectionModule::isOcclusionCleared( std::vector common_areas; bg::intersection(area2d_poly, grid_poly, common_areas); if (common_areas.empty()) { - continue; + return; } for (size_t i = 0; i < common_areas.size(); ++i) { common_areas[i].outer().push_back(common_areas[i].outer().front()); bg::correct(common_areas[i]); } for (const auto & common_area : common_areas) { - std::vector attention_area_cv_polygon; + std::vector cv_polygon; for (const auto & p : common_area.outer()) { const int idx_x = static_cast((p.x() - origin.x) / resolution); const int idx_y = static_cast((p.y() - origin.y) / resolution); - attention_area_cv_polygon.emplace_back(idx_x, height - 1 - idx_y); + cv_polygon.emplace_back(idx_x, height - 1 - idx_y); } - attention_area_cv_polygons.push_back(attention_area_cv_polygon); + cv_polygons.push_back(cv_polygon); } + }; + + // (1) prepare detection area mask + // attention: 255 + // non-attention: 0 + // NOTE: interesting area is set to 255 for later masking + cv::Mat attention_mask(width, height, CV_8UC1, cv::Scalar(0)); + std::vector> attention_area_cv_polygons; + for (const auto & attention_area : attention_areas) { + const auto area2d = lanelet::utils::to2D(attention_area); + findCommonCvPolygons(area2d, attention_area_cv_polygons); } for (const auto & poly : attention_area_cv_polygons) { cv::fillPoly(attention_mask, poly, cv::Scalar(255), cv::LINE_AA); @@ -1243,30 +1762,7 @@ bool IntersectionModule::isOcclusionCleared( std::vector> adjacent_lane_cv_polygons; for (const auto & adjacent_lanelet : adjacent_lanelets) { const auto area2d = adjacent_lanelet.polygon2d().basicPolygon(); - Polygon2d area2d_poly; - for (const auto & p : area2d) { - area2d_poly.outer().emplace_back(p.x(), p.y()); - } - area2d_poly.outer().push_back(area2d_poly.outer().front()); - bg::correct(area2d_poly); - std::vector common_areas; - bg::intersection(area2d_poly, grid_poly, common_areas); - if (common_areas.empty()) { - continue; - } - for (size_t i = 0; i < common_areas.size(); ++i) { - common_areas[i].outer().push_back(common_areas[i].outer().front()); - bg::correct(common_areas[i]); - } - for (const auto & common_area : common_areas) { - std::vector adjacent_lane_cv_polygon; - for (const auto & p : common_area.outer()) { - const int idx_x = std::floor((p.x() - origin.x) / resolution); - const int idx_y = std::floor((p.y() - origin.y) / resolution); - adjacent_lane_cv_polygon.emplace_back(idx_x, height - 1 - idx_y); - } - adjacent_lane_cv_polygons.push_back(adjacent_lane_cv_polygon); - } + findCommonCvPolygons(area2d, adjacent_lane_cv_polygons); } for (const auto & poly : adjacent_lane_cv_polygons) { cv::fillPoly(attention_mask, poly, cv::Scalar(0), cv::LINE_AA); @@ -1274,6 +1770,10 @@ bool IntersectionModule::isOcclusionCleared( // (2) prepare unknown mask // In OpenCV the pixel at (X=x, Y=y) (with left-upper origin) is accessed by img[y, x] + // unknown: 255 + // not-unknown: 0 + cv::Mat unknown_mask_raw(width, height, CV_8UC1, cv::Scalar(0)); + cv::Mat unknown_mask(width, height, CV_8UC1, cv::Scalar(0)); for (int x = 0; x < width; x++) { for (int y = 0; y < height; y++) { const int idx = y * width + x; @@ -1281,159 +1781,53 @@ bool IntersectionModule::isOcclusionCleared( if ( planner_param_.occlusion.free_space_max <= intensity && intensity < planner_param_.occlusion.occupied_min) { - unknown_mask.at(height - 1 - y, x) = 255; + unknown_mask_raw.at(height - 1 - y, x) = 255; } } } - - // (3) occlusion mask - cv::Mat occlusion_mask_raw(width, height, CV_8UC1, cv::Scalar(0)); - cv::bitwise_and(attention_mask, unknown_mask, occlusion_mask_raw); - // (3.1) apply morphologyEx - cv::Mat occlusion_mask; + // (2.1) apply morphologyEx const int morph_size = static_cast(planner_param_.occlusion.denoise_kernel / resolution); cv::morphologyEx( - occlusion_mask_raw, occlusion_mask, cv::MORPH_OPEN, + unknown_mask_raw, unknown_mask, cv::MORPH_OPEN, cv::getStructuringElement(cv::MORPH_RECT, cv::Size(morph_size, morph_size))); - // (4) create distance grid - // value: 0 - 254: signed distance representing [distance_min, distance_max] - // 255: undefined value - const double distance_max = std::hypot(width * resolution / 2, height * resolution / 2); - const double distance_min = -distance_max; - const int undef_pixel = 255; - const int max_cost_pixel = 254; - auto dist2pixel = [=](const double dist) { - return std::min( - max_cost_pixel, - static_cast((dist - distance_min) / (distance_max - distance_min) * max_cost_pixel)); - }; - auto pixel2dist = [=](const int pixel) { - return pixel * 1.0 / max_cost_pixel * (distance_max - distance_min) + distance_min; - }; - const int zero_dist_pixel = dist2pixel(0.0); - const int parked_vehicle_pixel = zero_dist_pixel - 1; // magic - - auto coord2index = [&](const double x, const double y) { - const int idx_x = (x - origin.x) / resolution; - const int idx_y = (y - origin.y) / resolution; - if (idx_x < 0 || idx_x >= width) return std::make_tuple(false, -1, -1); - if (idx_y < 0 || idx_y >= height) return std::make_tuple(false, -1, -1); - return std::make_tuple(true, idx_x, idx_y); - }; - - cv::Mat distance_grid(width, height, CV_8UC1, cv::Scalar(undef_pixel)); - cv::Mat projection_ind_grid(width, height, CV_32S, cv::Scalar(-1)); - - // (4.1) fill zero_dist_pixel on the path - const auto [lane_start, lane_end] = lane_attention_interval_ip; - for (int i = static_cast(lane_end); i >= static_cast(lane_start); i--) { - const auto & path_pos = path_ip.points.at(i).point.pose.position; - const int idx_x = (path_pos.x - origin.x) / resolution; - const int idx_y = (path_pos.y - origin.y) / resolution; - if (idx_x < 0 || idx_x >= width) continue; - if (idx_y < 0 || idx_y >= height) continue; - distance_grid.at(height - 1 - idx_y, idx_x) = zero_dist_pixel; - projection_ind_grid.at(height - 1 - idx_y, idx_x) = i; + // (3) occlusion mask + static constexpr unsigned char OCCLUDED = 255; + static constexpr unsigned char BLOCKED = 127; + cv::Mat occlusion_mask(width, height, CV_8UC1, cv::Scalar(0)); + cv::bitwise_and(attention_mask, unknown_mask, occlusion_mask); + // re-use attention_mask + attention_mask = cv::Mat(width, height, CV_8UC1, cv::Scalar(0)); + // (3.1) draw all cells on attention_mask behind blocking vehicles as not occluded + const auto & blocking_attention_objects = target_objects.parked_attention_objects; + for (const auto & blocking_attention_object : blocking_attention_objects) { + debug_data_.blocking_attention_objects.objects.push_back(blocking_attention_object.object); } - - // (4.2) fill parked_vehicle_pixel to parked_vehicles (both positive and negative) - for (const auto & parked_attention_object : parked_attention_objects) { - const auto obj_poly = tier4_autoware_utils::toPolygon2d(parked_attention_object); - std::vector common_areas; - bg::intersection(obj_poly, grid_poly, common_areas); - if (common_areas.empty()) continue; - for (size_t i = 0; i < common_areas.size(); ++i) { - common_areas[i].outer().push_back(common_areas[i].outer().front()); - bg::correct(common_areas[i]); - } - std::vector> parked_attention_object_area_cv_polygons; - for (const auto & common_area : common_areas) { - std::vector parked_attention_object_area_cv_polygon; - for (const auto & p : common_area.outer()) { - const int idx_x = static_cast((p.x() - origin.x) / resolution); - const int idx_y = static_cast((p.y() - origin.y) / resolution); - parked_attention_object_area_cv_polygon.emplace_back(idx_x, height - 1 - idx_y); - } - parked_attention_object_area_cv_polygons.push_back(parked_attention_object_area_cv_polygon); - } - for (const auto & poly : parked_attention_object_area_cv_polygons) { - cv::fillPoly(distance_grid, poly, cv::Scalar(parked_vehicle_pixel), cv::LINE_AA); - } + std::vector> blocking_polygons; + for (const auto & blocking_attention_object : blocking_attention_objects) { + const Polygon2d obj_poly = tier4_autoware_utils::toPolygon2d(blocking_attention_object.object); + findCommonCvPolygons(obj_poly.outer(), blocking_polygons); } - - for (const auto & lane_division : lane_divisions) { - const auto & divisions = lane_division.divisions; - for (const auto & division : divisions) { - bool is_in_grid = false; - bool zero_dist_cell_found = false; - int projection_ind = -1; - std::optional> cost_prev_checkpoint = - std::nullopt; // cost, x, y, projection_ind - for (auto point = division.begin(); point != division.end(); point++) { - const double x = point->x(), y = point->y(); - const auto [valid, idx_x, idx_y] = coord2index(x, y); - // exited grid just now - if (is_in_grid && !valid) break; - - // still not entering grid - if (!is_in_grid && !valid) continue; - - // From here, "valid" - const int pixel = distance_grid.at(height - 1 - idx_y, idx_x); - - // entered grid for 1st time - if (!is_in_grid) { - assert(pixel == undef_pixel || pixel == zero_dist_pixel); - is_in_grid = true; - if (pixel == undef_pixel) { - continue; - } - } - - if (pixel == zero_dist_pixel) { - zero_dist_cell_found = true; - projection_ind = projection_ind_grid.at(height - 1 - idx_y, idx_x); - assert(projection_ind >= 0); - cost_prev_checkpoint = - std::make_optional>(0.0, x, y, projection_ind); - continue; - } - - // hit positive parked vehicle - if (zero_dist_cell_found && pixel == parked_vehicle_pixel) { - while (point != division.end()) { - const double x = point->x(), y = point->y(); - const auto [valid, idx_x, idx_y] = coord2index(x, y); - if (valid) occlusion_mask.at(height - 1 - idx_y, idx_x) = 0; - point++; - } - break; - } - - if (zero_dist_cell_found) { - // finally traversed to defined cell (first half) - const auto [prev_cost, prev_checkpoint_x, prev_checkpoint_y, prev_projection_ind] = - cost_prev_checkpoint.value(); - const double dy = y - prev_checkpoint_y, dx = x - prev_checkpoint_x; - double new_dist = prev_cost + std::hypot(dy, dx); - const int new_projection_ind = projection_ind_grid.at(height - 1 - idx_y, idx_x); - const double cur_dist = pixel2dist(pixel); - if (planner_param_.occlusion.do_dp && cur_dist < new_dist) { - new_dist = cur_dist; - if (new_projection_ind > 0) { - projection_ind = std::min(prev_projection_ind, new_projection_ind); - } - } - projection_ind_grid.at(height - 1 - idx_y, idx_x) = projection_ind; - distance_grid.at(height - 1 - idx_y, idx_x) = dist2pixel(new_dist); - cost_prev_checkpoint = std::make_optional>( - new_dist, x, y, projection_ind); - } + for (const auto & blocking_polygon : blocking_polygons) { + cv::fillPoly(attention_mask, blocking_polygon, cv::Scalar(BLOCKED), cv::LINE_AA); + } + for (const auto & division : lane_divisions) { + bool blocking_vehicle_found = false; + for (const auto & point_it : division) { + const auto [valid, idx_x, idx_y] = coord2index(point_it.x(), point_it.y()); + if (!valid) continue; + if (blocking_vehicle_found) { + occlusion_mask.at(height - 1 - idx_y, idx_x) = 0; + continue; + } + if (attention_mask.at(height - 1 - idx_y, idx_x) == BLOCKED) { + blocking_vehicle_found = true; + occlusion_mask.at(height - 1 - idx_y, idx_x) = 0; } } } + // (4) extract occlusion polygons const auto & possible_object_bbox = planner_param_.occlusion.possible_object_bbox; const double possible_object_bbox_x = possible_object_bbox.at(0) / resolution; const double possible_object_bbox_y = possible_object_bbox.at(1) / resolution; @@ -1442,19 +1836,12 @@ bool IntersectionModule::isOcclusionCleared( cv::findContours(occlusion_mask, contours, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_NONE); std::vector> valid_contours; for (const auto & contour : contours) { - std::vector valid_contour; - for (const auto & p : contour) { - if (distance_grid.at(p.y, p.x) == undef_pixel) { - continue; - } - valid_contour.push_back(p); - } - if (valid_contour.size() <= 2) { + if (contour.size() <= 2) { continue; } std::vector approx_contour; cv::approxPolyDP( - valid_contour, approx_contour, + contour, approx_contour, std::round(std::min(possible_object_bbox_x, possible_object_bbox_y) / std::sqrt(2.0)), true); if (approx_contour.size() <= 2) continue; // check area @@ -1481,83 +1868,183 @@ bool IntersectionModule::isOcclusionCleared( } debug_data_.occlusion_polygons.push_back(polygon_msg); } + // (4.1) re-draw occluded cells using valid_contours + occlusion_mask = cv::Mat(width, height, CV_8UC1, cv::Scalar(0)); + for (const auto & valid_contour : valid_contours) { + // NOTE: drawContour does not work well + cv::fillPoly(occlusion_mask, valid_contour, cv::Scalar(OCCLUDED), cv::LINE_AA); + } - const int min_cost_thr = dist2pixel(occlusion_dist_thr); - int min_cost = undef_pixel - 1; - geometry_msgs::msg::Point nearest_occlusion_point; - for (const auto & occlusion_contour : valid_contours) { - for (const auto & p : occlusion_contour) { - const int pixel = static_cast(distance_grid.at(p.y, p.x)); - const bool occluded = (occlusion_mask.at(p.y, p.x) == 255); - if (pixel == undef_pixel || !occluded) { + // (5) find distance + // (5.1) discretize path_ip with resolution for computational cost + LineString2d path_linestring; + path_linestring.emplace_back( + path_ip.points.at(lane_start_idx).point.pose.position.x, + path_ip.points.at(lane_start_idx).point.pose.position.y); + { + auto prev_path_linestring_point = path_ip.points.at(lane_start_idx).point.pose.position; + for (auto i = lane_start_idx + 1; i <= lane_end_idx; i++) { + const auto path_linestring_point = path_ip.points.at(i).point.pose.position; + if ( + tier4_autoware_utils::calcDistance2d(prev_path_linestring_point, path_linestring_point) < + 1.0 /* rough tick for computational cost */) { continue; } - if (pixel < min_cost) { - min_cost = pixel; - nearest_occlusion_point.x = origin.x + p.x * resolution; - nearest_occlusion_point.y = origin.y + (height - 1 - p.y) * resolution; - nearest_occlusion_point.z = origin.z; + path_linestring.emplace_back(path_linestring_point.x, path_linestring_point.y); + prev_path_linestring_point = path_linestring_point; + } + } + + auto findNearestPointToProjection = []( + const lanelet::ConstLineString3d & division, + const Point2d & projection, const double dist_thresh) { + double min_dist = std::numeric_limits::infinity(); + auto nearest = division.end(); + for (auto it = division.begin(); it != division.end(); it++) { + const double dist = std::hypot(it->x() - projection.x(), it->y() - projection.y()); + if (dist < min_dist) { + min_dist = dist; + nearest = it; + } + if (dist < dist_thresh) { + break; + } + } + return nearest; + }; + struct NearestOcclusionPoint + { + int64 division_index{0}; + int64 point_index{0}; + double dist{0.0}; + geometry_msgs::msg::Point point; + geometry_msgs::msg::Point projection; + } nearest_occlusion_point; + double min_dist = std::numeric_limits::infinity(); + for (unsigned division_index = 0; division_index < lane_divisions.size(); ++division_index) { + const auto & division = lane_divisions.at(division_index); + LineString2d division_linestring; + auto division_point_it = division.begin(); + division_linestring.emplace_back(division_point_it->x(), division_point_it->y()); + for (auto point_it = division.begin(); point_it != division.end(); point_it++) { + if ( + std::hypot(point_it->x() - division_point_it->x(), point_it->y() - division_point_it->y()) < + 3.0 /* rough tick for computational cost */) { + continue; + } + division_linestring.emplace_back(point_it->x(), point_it->y()); + division_point_it = point_it; + } + + // find the intersection point of lane_line and path + std::vector intersection_points; + boost::geometry::intersection(division_linestring, path_linestring, intersection_points); + if (intersection_points.empty()) { + continue; + } + const auto & projection_point = intersection_points.at(0); + const auto projection_it = findNearestPointToProjection(division, projection_point, resolution); + if (projection_it == division.end()) { + continue; + } + double acc_dist = 0.0; + auto acc_dist_it = projection_it; + for (auto point_it = projection_it; point_it != division.end(); point_it++) { + const double dist = + std::hypot(point_it->x() - acc_dist_it->x(), point_it->y() - acc_dist_it->y()); + acc_dist += dist; + acc_dist_it = point_it; + const auto [valid, idx_x, idx_y] = coord2index(point_it->x(), point_it->y()); + if (!valid) continue; + const auto pixel = occlusion_mask.at(height - 1 - idx_y, idx_x); + if (pixel == BLOCKED) { + break; + } + if (pixel == OCCLUDED) { + if (acc_dist < min_dist) { + min_dist = acc_dist; + nearest_occlusion_point = { + division_index, std::distance(division.begin(), point_it), acc_dist, + tier4_autoware_utils::createPoint(point_it->x(), point_it->y(), origin.z), + tier4_autoware_utils::createPoint(projection_it->x(), projection_it->y(), origin.z)}; + } } } } - if (min_cost > min_cost_thr) { - return true; - } else { - debug_data_.nearest_occlusion_point = nearest_occlusion_point; - return false; + if (min_dist == std::numeric_limits::infinity() || min_dist > occlusion_dist_thr) { + return OcclusionType::NOT_OCCLUDED; } + + debug_data_.nearest_occlusion_projection = + std::make_pair(nearest_occlusion_point.point, nearest_occlusion_point.projection); + LineString2d ego_occlusion_line; + ego_occlusion_line.emplace_back(current_pose.position.x, current_pose.position.y); + ego_occlusion_line.emplace_back(nearest_occlusion_point.point.x, nearest_occlusion_point.point.y); + for (const auto & attention_object : target_objects.all_attention_objects) { + const auto obj_poly = tier4_autoware_utils::toPolygon2d(attention_object.object); + if (bg::intersects(obj_poly, ego_occlusion_line)) { + return OcclusionType::DYNAMICALLY_OCCLUDED; + } + } + for (const auto & attention_object : target_objects.intersection_area_objects) { + const auto obj_poly = tier4_autoware_utils::toPolygon2d(attention_object.object); + if (bg::intersects(obj_poly, ego_occlusion_line)) { + return OcclusionType::DYNAMICALLY_OCCLUDED; + } + } + return OcclusionType::STATICALLY_OCCLUDED; } /* -bool IntersectionModule::checkFrontVehicleDeceleration( + bool IntersectionModule::checkFrontVehicleDeceleration( lanelet::ConstLanelets & ego_lane_with_next_lane, lanelet::ConstLanelet & closest_lanelet, const Polygon2d & stuck_vehicle_detect_area, const autoware_auto_perception_msgs::msg::PredictedObject & object, const double assumed_front_car_decel) -{ + { const auto & object_pose = object.kinematics.initial_pose_with_covariance.pose; // consider vehicle in ego-lane && in front of ego const auto lon_vel = object.kinematics.initial_twist_with_covariance.twist.linear.x; const double object_decel = - planner_param_.stuck_vehicle.assumed_front_car_decel; // NOTE: this is positive + planner_param_.stuck_vehicle.assumed_front_car_decel; // NOTE: this is positive const double stopping_distance = lon_vel * lon_vel / (2 * object_decel); std::vector center_points; for (auto && p : ego_lane_with_next_lane[0].centerline()) - center_points.push_back(std::move(lanelet::utils::conversion::toGeomMsgPt(p))); + center_points.push_back(std::move(lanelet::utils::conversion::toGeomMsgPt(p))); for (auto && p : ego_lane_with_next_lane[1].centerline()) - center_points.push_back(std::move(lanelet::utils::conversion::toGeomMsgPt(p))); + center_points.push_back(std::move(lanelet::utils::conversion::toGeomMsgPt(p))); const double lat_offset = - std::fabs(motion_utils::calcLateralOffset(center_points, object_pose.position)); + std::fabs(motion_utils::calcLateralOffset(center_points, object_pose.position)); // get the nearest centerpoint to object std::vector dist_obj_center_points; for (const auto & p : center_points) - dist_obj_center_points.push_back(tier4_autoware_utils::calcDistance2d(object_pose.position, -p)); const int obj_closest_centerpoint_idx = std::distance( dist_obj_center_points.begin(), - std::min_element(dist_obj_center_points.begin(), dist_obj_center_points.end())); + dist_obj_center_points.push_back(tier4_autoware_utils::calcDistance2d(object_pose.position, + p)); const int obj_closest_centerpoint_idx = std::distance( dist_obj_center_points.begin(), + std::min_element(dist_obj_center_points.begin(), dist_obj_center_points.end())); // find two center_points whose distances from `closest_centerpoint` cross stopping_distance double acc_dist_prev = 0.0, acc_dist = 0.0; auto p1 = center_points[obj_closest_centerpoint_idx]; auto p2 = center_points[obj_closest_centerpoint_idx]; for (unsigned i = obj_closest_centerpoint_idx; i < center_points.size() - 1; ++i) { - p1 = center_points[i]; - p2 = center_points[i + 1]; - acc_dist_prev = acc_dist; - const auto arc_position_p1 = - lanelet::utils::getArcCoordinates(ego_lane_with_next_lane, toPose(p1)); - const auto arc_position_p2 = - lanelet::utils::getArcCoordinates(ego_lane_with_next_lane, toPose(p2)); - const double delta = arc_position_p2.length - arc_position_p1.length; - acc_dist += delta; - if (acc_dist > stopping_distance) { - break; - } + p1 = center_points[i]; + p2 = center_points[i + 1]; + acc_dist_prev = acc_dist; + const auto arc_position_p1 = + lanelet::utils::getArcCoordinates(ego_lane_with_next_lane, toPose(p1)); + const auto arc_position_p2 = + lanelet::utils::getArcCoordinates(ego_lane_with_next_lane, toPose(p2)); + const double delta = arc_position_p2.length - arc_position_p1.length; + acc_dist += delta; + if (acc_dist > stopping_distance) { + break; + } } // if stopping_distance >= center_points, stopping_point is center_points[end] const double ratio = (acc_dist <= stopping_distance) - ? 0.0 - : (acc_dist - stopping_distance) / (stopping_distance - acc_dist_prev); + ? 0.0 + : (acc_dist - stopping_distance) / (stopping_distance - acc_dist_prev); // linear interpolation geometry_msgs::msg::Point stopping_point; stopping_point.x = (p1.x * ratio + p2.x) / (1 + ratio); @@ -1571,18 +2058,18 @@ p)); const int obj_closest_centerpoint_idx = std::distance( dist_obj_center_poin autoware_auto_perception_msgs::msg::PredictedObject predicted_object = object; predicted_object.kinematics.initial_pose_with_covariance.pose.position = stopping_point; predicted_object.kinematics.initial_pose_with_covariance.pose.orientation = - tier4_autoware_utils::createQuaternionFromRPY(0, 0, lane_yaw); + tier4_autoware_utils::createQuaternionFromRPY(0, 0, lane_yaw); auto predicted_obj_footprint = tier4_autoware_utils::toPolygon2d(predicted_object); const bool is_in_stuck_area = !bg::disjoint(predicted_obj_footprint, stuck_vehicle_detect_area); debug_data_.predicted_obj_pose.position = stopping_point; debug_data_.predicted_obj_pose.orientation = - tier4_autoware_utils::createQuaternionFromRPY(0, 0, lane_yaw); + tier4_autoware_utils::createQuaternionFromRPY(0, 0, lane_yaw); if (is_in_stuck_area) { - return true; + return true; } return false; -} + } */ } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp index 774b68710be7f..93fec171ec0d2 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -18,11 +18,9 @@ #include "util_type.hpp" #include -#include #include -#include +#include #include -#include #include #include @@ -60,13 +58,21 @@ class IntersectionModule : public SceneModuleInterface bool use_intersection_area; bool consider_wrong_direction_vehicle; double path_interpolation_ds; + double max_accel; + double max_jerk; + double delay_response_time; } common; struct StuckVehicle { + struct TurnDirection + { + bool left; + bool right; + bool straight; + }; + TurnDirection turn_direction; bool use_stuck_stopline; //! stopline generate before the intersection lanelet when is_stuck. double stuck_vehicle_detect_dist; //! distance from end point to finish stuck vehicle check - double stuck_vehicle_ignore_dist; //! distance from intersection start to start stuck vehicle - //! check double stuck_vehicle_vel_thr; //! Threshold of the speed to be recognized as stopped /* double @@ -74,6 +80,9 @@ class IntersectionModule : public SceneModuleInterface bool enable_front_car_decel_prediction; //! flag for using above feature */ double timeout_private_area; + bool enable_private_area_stuck_disregard; + double yield_stuck_distance_thr; + TurnDirection yield_stuck_turn_direction; } stuck_vehicle; struct CollisionDetection { @@ -81,17 +90,38 @@ class IntersectionModule : public SceneModuleInterface //! minimum confidence value of predicted path to use for collision detection double minimum_ego_predicted_velocity; //! used to calculate ego's future velocity profile double state_transit_margin_time; - struct Normal + struct FullyPrioritized + { + double collision_start_margin_time; //! start margin time to check collision + double collision_end_margin_time; //! end margin time to check collision + } fully_prioritized; + struct PartiallyPrioritized { double collision_start_margin_time; //! start margin time to check collision double collision_end_margin_time; //! end margin time to check collision - } normal; - struct Relaxed + } partially_prioritized; + struct NotPrioritized { - double collision_start_margin_time; - double collision_end_margin_time; - } relaxed; + double collision_start_margin_time; //! start margin time to check collision + double collision_end_margin_time; //! end margin time to check collision + } not_prioritized; double keep_detection_vel_thr; //! keep detection if ego is ego.vel < keep_detection_vel_thr + bool use_upstream_velocity; + double minimum_upstream_velocity; + struct YieldOnGreeTrafficLight + { + double distance_to_assigned_lanelet_start; + double duration; + double object_dist_to_stopline; + } yield_on_green_traffic_light; + struct IgnoreOnAmberTrafficLight + { + double object_expected_deceleration; + } ignore_on_amber_traffic_light; + struct IgnoreOnRedTrafficLight + { + double object_margin_to_path; + } ignore_on_red_traffic_light; } collision_detection; struct Occlusion { @@ -109,81 +139,124 @@ class IntersectionModule : public SceneModuleInterface double denoise_kernel; std::vector possible_object_bbox; double ignore_parked_vehicle_speed_threshold; + double stop_release_margin_time; + bool temporal_stop_before_attention_area; + struct AbsenceTrafficLight + { + double creep_velocity; + double maximum_peeking_distance; + } absence_traffic_light; + double attention_lane_crop_curvature_threshold; + double attention_lane_curvature_calculation_ds; + double static_occlusion_with_traffic_light_timeout; } occlusion; }; - /* - enum OcclusionState { - NONE, - BEFORE_FIRST_STOP_LINE, - WAIT_FIRST_STOP_LINE, - CREEP_SECOND_STOP_LINE, - COLLISION_DETECTED, + enum OcclusionType { + NOT_OCCLUDED, + STATICALLY_OCCLUDED, + DYNAMICALLY_OCCLUDED, + RTC_OCCLUDED, // actual occlusion does not exist, only disapproved by RTC }; - */ - using Indecisive = std::monostate; + struct Indecisive + { + std::string error; + }; struct StuckStop { - size_t stop_line_idx; - // NOTE: this is optional because stuck vehicle detection is possible - // even if the detection area is empty. - // Still this may be required for RTC's default stop line - bool is_detection_area_empty; - util::IntersectionStopLines stop_lines; + size_t closest_idx{0}; + size_t stuck_stop_line_idx{0}; + std::optional occlusion_stop_line_idx{std::nullopt}; + }; + struct YieldStuckStop + { + size_t closest_idx{0}; + size_t stuck_stop_line_idx{0}; }; struct NonOccludedCollisionStop { - size_t stop_line_idx; - util::IntersectionStopLines stop_lines; + size_t closest_idx{0}; + size_t collision_stop_line_idx{0}; + size_t occlusion_stop_line_idx{0}; }; struct FirstWaitBeforeOcclusion { - size_t first_stop_line_idx; - size_t occlusion_stop_line_idx; - bool is_actually_occlusion_cleared; - util::IntersectionStopLines stop_lines; + bool is_actually_occlusion_cleared{false}; + size_t closest_idx{0}; + size_t first_stop_line_idx{0}; + size_t occlusion_stop_line_idx{0}; }; + // A state peeking to occlusion limit line in the presence of traffic light struct PeekingTowardOcclusion { - size_t stop_line_idx; // NOTE: if intersection_occlusion is disapproved externally through RTC, // it indicates "is_forcefully_occluded" - bool is_actually_occlusion_cleared; - util::IntersectionStopLines stop_lines; + bool is_actually_occlusion_cleared{false}; + bool temporal_stop_before_attention_required{false}; + size_t closest_idx{0}; + size_t collision_stop_line_idx{0}; + size_t first_attention_stop_line_idx{0}; + size_t occlusion_stop_line_idx{0}; + // if null, it is dynamic occlusion and shows up intersection_occlusion(dyn) + // if valid, it contains the remaining time to release the static occlusion stuck and shows up + // intersection_occlusion(x.y) + std::optional static_occlusion_timeout{std::nullopt}; }; + // A state detecting both collision and occlusion in the presence of traffic light struct OccludedCollisionStop { - size_t stop_line_idx; - size_t occlusion_stop_line_idx; - bool is_actually_occlusion_cleared; - util::IntersectionStopLines stop_lines; + bool is_actually_occlusion_cleared{false}; + bool temporal_stop_before_attention_required{false}; + size_t closest_idx{0}; + size_t collision_stop_line_idx{0}; + size_t first_attention_stop_line_idx{0}; + size_t occlusion_stop_line_idx{0}; + // if null, it is dynamic occlusion and shows up intersection_occlusion(dyn) + // if valid, it contains the remaining time to release the static occlusion stuck + std::optional static_occlusion_timeout{std::nullopt}; + }; + struct OccludedAbsenceTrafficLight + { + bool is_actually_occlusion_cleared{false}; + bool collision_detected{false}; + bool temporal_stop_before_attention_required{false}; + size_t closest_idx{0}; + size_t first_attention_area_stop_line_idx{0}; + size_t peeking_limit_line_idx{0}; }; struct Safe { // NOTE: if RTC is disapproved status, default stop lines are still needed. - util::IntersectionStopLines stop_lines; + size_t closest_idx{0}; + size_t collision_stop_line_idx{0}; + size_t occlusion_stop_line_idx{0}; }; struct TrafficLightArrowSolidOn { - bool collision_detected; - util::IntersectionStopLines stop_lines; + bool collision_detected{false}; + size_t closest_idx{0}; + size_t collision_stop_line_idx{0}; + size_t occlusion_stop_line_idx{0}; }; using DecisionResult = std::variant< - Indecisive, // internal process error, or over the pass judge line - StuckStop, // detected stuck vehicle - NonOccludedCollisionStop, // detected collision while FOV is clear - FirstWaitBeforeOcclusion, // stop for a while before peeking to occlusion - PeekingTowardOcclusion, // peeking into occlusion while collision is not detected - OccludedCollisionStop, // occlusion and collision are both detected - Safe, // judge as safe - TrafficLightArrowSolidOn // only detect vehicles violating traffic rules + Indecisive, // internal process error, or over the pass judge line + StuckStop, // detected stuck vehicle + YieldStuckStop, // detected yield stuck vehicle + NonOccludedCollisionStop, // detected collision while FOV is clear + FirstWaitBeforeOcclusion, // stop for a while before peeking to occlusion + PeekingTowardOcclusion, // peeking into occlusion while collision is not detected + OccludedCollisionStop, // occlusion and collision are both detected + OccludedAbsenceTrafficLight, // occlusion is detected in the absence of traffic light + Safe, // judge as safe + TrafficLightArrowSolidOn // only detect vehicles violating traffic rules >; IntersectionModule( const int64_t module_id, const int64_t lane_id, std::shared_ptr planner_data, const PlannerParam & planner_param, const std::set & associative_ids, - const bool is_private_area, const bool enable_occlusion_detection, rclcpp::Node & node, + const std::string & turn_direction, const bool has_traffic_light, + const bool enable_occlusion_detection, const bool is_private_area, rclcpp::Node & node, const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock); /** @@ -207,36 +280,43 @@ class IntersectionModule : public SceneModuleInterface rclcpp::Node & node_; const int64_t lane_id_; const std::set associative_ids_; - std::string turn_direction_; - bool is_go_out_ = false; - bool is_permanent_go_ = false; - bool is_peeking_ = false; + const std::string turn_direction_; + const bool has_traffic_light_; + + bool is_go_out_{false}; + bool is_permanent_go_{false}; + DecisionResult prev_decision_result_{Indecisive{""}}; + OcclusionType prev_occlusion_status_; + // Parameter PlannerParam planner_param_; - std::optional intersection_lanelets_; - // for an intersection lane, its associative lanes are those that share same parent lanelet and - // have same turn_direction + + std::optional intersection_lanelets_{std::nullopt}; // for occlusion detection const bool enable_occlusion_detection_; - std::optional> occlusion_attention_divisions_; - // OcclusionState prev_occlusion_state_ = OcclusionState::NONE; + std::optional> occlusion_attention_divisions_{ + std::nullopt}; StateMachine collision_state_machine_; //! for stable collision checking StateMachine before_creep_state_machine_; //! for two phase stop - // NOTE: uuid_ is base member + StateMachine occlusion_stop_state_machine_; + StateMachine temporal_stop_before_attention_state_machine_; + StateMachine static_occlusion_timeout_state_machine_; + + // for pseudo-collision detection when ego just entered intersection on green light and upcoming + // vehicles are very slow + std::optional initial_green_light_observed_time_{std::nullopt}; // for stuck vehicle detection const bool is_private_area_; - StateMachine stuck_private_area_timeout_; // for RTC const UUID occlusion_uuid_; - bool occlusion_safety_ = true; - double occlusion_stop_distance_; - bool occlusion_activated_ = true; + bool occlusion_safety_{true}; + double occlusion_stop_distance_{0.0}; + bool occlusion_activated_{true}; // for first stop in two-phase stop - const UUID occlusion_first_stop_uuid_; - bool occlusion_first_stop_required_ = false; + bool occlusion_first_stop_required_{false}; void initializeRTCStatus(); void prepareRTCStatus( @@ -246,30 +326,31 @@ class IntersectionModule : public SceneModuleInterface bool checkStuckVehicle( const std::shared_ptr & planner_data, - const lanelet::ConstLanelets & ego_lane_with_next_lane, - const autoware_auto_planning_msgs::msg::PathWithLaneId & input_path, - const util::IntersectionStopLines & intersection_stop_lines); + const util::PathLanelets & path_lanelets); - autoware_auto_perception_msgs::msg::PredictedObjects filterTargetObjects( - const lanelet::ConstLanelets & attention_area_lanelets, - const lanelet::ConstLanelets & adjacent_lanelets, + bool checkYieldStuckVehicle( + const std::shared_ptr & planner_data, + const util::PathLanelets & path_lanelets, + const std::optional & first_attention_area); + + util::TargetObjects generateTargetObjects( + const util::IntersectionLanelets & intersection_lanelets, const std::optional & intersection_area) const; bool checkCollision( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const autoware_auto_perception_msgs::msg::PredictedObjects & target_objects, - const util::PathLanelets & path_lanelets, const int closest_idx, const double time_delay, - const bool tl_arrow_solid_on); + util::TargetObjects * target_objects, const util::PathLanelets & path_lanelets, + const size_t closest_idx, const size_t last_intersection_stop_line_candidate_idx, + const double time_delay, const util::TrafficPrioritizedLevel & traffic_prioritized_level); - bool isOcclusionCleared( + OcclusionType getOcclusionStatus( const nav_msgs::msg::OccupancyGrid & occ_grid, const std::vector & attention_areas, const lanelet::ConstLanelets & adjacent_lanelets, const lanelet::CompoundPolygon3d & first_attention_area, const util::InterpolatedPathInfo & interpolated_path_info, - const std::vector & lane_divisions, - const std::vector & - parked_attention_objects, + const std::vector & lane_divisions, + const util::TargetObjects & target_objects, const geometry_msgs::msg::Pose & current_pose, const double occlusion_dist_thr); /* diff --git a/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp b/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp index 10ad395953476..698242da67528 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp @@ -22,6 +22,7 @@ #include #include #include +#include #include #include @@ -93,8 +94,10 @@ bool MergeFromPrivateRoadModule::modifyPathVelocity(PathWithLaneId * path, StopR planner_param_.attention_area_length, planner_param_.occlusion_attention_area_length, planner_param_.consider_wrong_direction_vehicle); } - intersection_lanelets_.value().update(false, interpolated_path_info); - const auto & first_conflicting_area = intersection_lanelets_.value().first_conflicting_area(); + auto & intersection_lanelets = intersection_lanelets_.value(); + const auto local_footprint = planner_data_->vehicle_info_.createFootprint(0.0, 0.0); + intersection_lanelets.update(false, interpolated_path_info, local_footprint); + const auto & first_conflicting_area = intersection_lanelets.first_conflicting_area(); if (!first_conflicting_area) { return false; } @@ -134,12 +137,11 @@ bool MergeFromPrivateRoadModule::modifyPathVelocity(PathWithLaneId * path, StopR const double signed_arc_dist_to_stop_point = motion_utils::calcSignedArcLength( path->points, current_pose.position, path->points.at(stop_line_idx).point.pose.position); - constexpr double distance_threshold = 2.0; if ( - signed_arc_dist_to_stop_point < distance_threshold && + signed_arc_dist_to_stop_point < planner_param_.stop_distance_threshold && planner_data_->isVehicleStopped(planner_param_.stop_duration_sec)) { state_machine_.setState(StateMachine::State::GO); - if (signed_arc_dist_to_stop_point < -distance_threshold) { + if (signed_arc_dist_to_stop_point < -planner_param_.stop_distance_threshold) { RCLCPP_ERROR(logger_, "Failed to stop near stop line but ego stopped. Change state to GO"); } } diff --git a/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp b/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp index aa334f11be84d..19e7b9201093f 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp @@ -21,7 +21,6 @@ #include #include #include -#include #include #include @@ -59,6 +58,7 @@ class MergeFromPrivateRoadModule : public SceneModuleInterface double attention_area_length; double stop_line_margin; double stop_duration_sec; + double stop_distance_threshold; double path_interpolation_ds; double occlusion_attention_area_length; bool consider_wrong_direction_vehicle; diff --git a/planning/behavior_velocity_intersection_module/src/util.cpp b/planning/behavior_velocity_intersection_module/src/util.cpp index a987ce9af9dfe..25eaaf4d8cb9d 100644 --- a/planning/behavior_velocity_intersection_module/src/util.cpp +++ b/planning/behavior_velocity_intersection_module/src/util.cpp @@ -16,15 +16,19 @@ #include "util_type.hpp" +#include #include #include #include -#include +#include #include #include #include +#include #include +#include +#include #include #include @@ -40,6 +44,21 @@ #include #include +namespace tier4_autoware_utils +{ + +template <> +inline geometry_msgs::msg::Point getPoint(const lanelet::ConstPoint3d & p) +{ + geometry_msgs::msg::Point point; + point.x = p.x(); + point.y = p.y(); + point.z = p.z(); + return point; +} + +} // namespace tier4_autoware_utils + namespace behavior_velocity_planner { namespace bg = boost::geometry; @@ -201,12 +220,57 @@ static std::optional getStopLineIndexFromMap( return stop_idx_ip_opt.get(); } +static std::optional getFirstPointInsidePolygonByFootprint( + const lanelet::CompoundPolygon3d & polygon, const InterpolatedPathInfo & interpolated_path_info, + const tier4_autoware_utils::LinearRing2d & footprint) +{ + const auto & path_ip = interpolated_path_info.path; + const auto [lane_start, lane_end] = interpolated_path_info.lane_id_interval.value(); + + const auto area_2d = lanelet::utils::to2D(polygon).basicPolygon(); + for (auto i = lane_start; i <= lane_end; ++i) { + const auto & base_pose = path_ip.points.at(i).point.pose; + const auto path_footprint = tier4_autoware_utils::transformVector( + footprint, tier4_autoware_utils::pose2transform(base_pose)); + if (bg::intersects(path_footprint, area_2d)) { + return std::make_optional(i); + } + } + return std::nullopt; +} + +static std::optional> +getFirstPointInsidePolygonsByFootprint( + const std::vector & polygons, + const InterpolatedPathInfo & interpolated_path_info, + const tier4_autoware_utils::LinearRing2d & footprint) +{ + const auto & path_ip = interpolated_path_info.path; + const auto [lane_start, lane_end] = interpolated_path_info.lane_id_interval.value(); + + for (size_t i = lane_start; i <= lane_end; ++i) { + const auto & pose = path_ip.points.at(i).point.pose; + const auto path_footprint = + tier4_autoware_utils::transformVector(footprint, tier4_autoware_utils::pose2transform(pose)); + for (size_t j = 0; j < polygons.size(); ++j) { + const auto area_2d = lanelet::utils::to2D(polygons.at(j)).basicPolygon(); + const bool is_in_polygon = bg::intersects(area_2d, path_footprint); + if (is_in_polygon) { + return std::make_optional>(i, j); + } + } + } + return std::nullopt; +} + std::optional generateIntersectionStopLines( const lanelet::CompoundPolygon3d & first_conflicting_area, const lanelet::CompoundPolygon3d & first_detection_area, const std::shared_ptr & planner_data, const InterpolatedPathInfo & interpolated_path_info, const bool use_stuck_stopline, - const double stop_line_margin, const double peeking_offset, + const double stop_line_margin, const double max_accel, const double max_jerk, + const double delay_response_time, const double peeking_offset, autoware_auto_planning_msgs::msg::PathWithLaneId * original_path) { const auto & path_ip = interpolated_path_info.path; @@ -217,6 +281,7 @@ std::optional generateIntersectionStopLines( const int base2front_idx_dist = std::ceil(planner_data->vehicle_info_.max_longitudinal_offset_m / ds); + /* // find the index of the first point that intersects with detection_areas const auto first_inside_detection_idx_ip_opt = getFirstPointInsidePolygon(path_ip, lane_interval_ip, first_detection_area); @@ -225,16 +290,30 @@ std::optional generateIntersectionStopLines( return std::nullopt; } const auto first_inside_detection_ip = first_inside_detection_idx_ip_opt.value(); + */ + // find the index of the first point whose vehicle footprint on it intersects with detection_area + const auto local_footprint = planner_data->vehicle_info_.createFootprint(0.0, 0.0); + std::optional first_footprint_inside_detection_ip_opt = + getFirstPointInsidePolygonByFootprint( + first_detection_area, interpolated_path_info, local_footprint); + if (!first_footprint_inside_detection_ip_opt) { + return std::nullopt; + } + const auto first_footprint_inside_detection_ip = first_footprint_inside_detection_ip_opt.value(); // (1) default stop line position on interpolated path - int stop_idx_ip_int = 0; + bool default_stop_line_valid = true; + int stop_idx_ip_int = -1; if (const auto map_stop_idx_ip = getStopLineIndexFromMap(interpolated_path_info, planner_data, 10.0); map_stop_idx_ip) { stop_idx_ip_int = static_cast(map_stop_idx_ip.value()) - base2front_idx_dist; - } else { - stop_idx_ip_int = static_cast(first_inside_detection_ip) - stop_line_margin_idx_dist - - base2front_idx_dist; + } + if (stop_idx_ip_int < 0) { + stop_idx_ip_int = first_footprint_inside_detection_ip - stop_line_margin_idx_dist; + } + if (stop_idx_ip_int < 0) { + default_stop_line_valid = false; } const auto default_stop_line_ip = stop_idx_ip_int >= 0 ? static_cast(stop_idx_ip_int) : 0; @@ -248,18 +327,23 @@ std::optional generateIntersectionStopLines( const auto closest_idx_ip = closest_idx_ip_opt.value(); // (3) occlusion peeking stop line position on interpolated path - const auto local_footprint = planner_data->vehicle_info_.createFootprint(0.0, 0.0); - const auto area_2d = lanelet::utils::to2D(first_detection_area).basicPolygon(); int occlusion_peeking_line_ip_int = static_cast(default_stop_line_ip); - for (size_t i = default_stop_line_ip; i <= std::get<1>(lane_interval_ip); ++i) { - const auto & base_pose = path_ip.points.at(i).point.pose; - const auto path_footprint = tier4_autoware_utils::transformVector( - local_footprint, tier4_autoware_utils::pose2transform(base_pose)); - if (bg::intersects(path_footprint, area_2d)) { - occlusion_peeking_line_ip_int = i; - break; + bool occlusion_peeking_line_valid = true; + { + // NOTE: if footprints[0] is already inside the detection area, invalid + const auto & base_pose0 = path_ip.points.at(default_stop_line_ip).point.pose; + const auto path_footprint0 = tier4_autoware_utils::transformVector( + local_footprint, tier4_autoware_utils::pose2transform(base_pose0)); + if (bg::intersects( + path_footprint0, lanelet::utils::to2D(first_detection_area).basicPolygon())) { + occlusion_peeking_line_valid = false; } } + if (occlusion_peeking_line_valid) { + occlusion_peeking_line_ip_int = first_footprint_inside_detection_ip; + } + const auto first_attention_stop_line_ip = static_cast(occlusion_peeking_line_ip_int); + const bool first_attention_stop_line_valid = occlusion_peeking_line_valid; occlusion_peeking_line_ip_int += std::ceil(peeking_offset / ds); const auto occlusion_peeking_line_ip = static_cast( std::clamp(occlusion_peeking_line_ip_int, 0, static_cast(path_ip.points.size()) - 1)); @@ -267,38 +351,54 @@ std::optional generateIntersectionStopLines( // (4) pass judge line position on interpolated path const double velocity = planner_data->current_velocity->twist.linear.x; const double acceleration = planner_data->current_acceleration->accel.accel.linear.x; - const double max_stop_acceleration = planner_data->max_stop_acceleration_threshold; - const double max_stop_jerk = planner_data->max_stop_jerk_threshold; - const double delay_response_time = planner_data->delay_response_time; const double braking_dist = planning_utils::calcJudgeLineDistWithJerkLimit( - velocity, acceleration, max_stop_acceleration, max_stop_jerk, delay_response_time); - int pass_judge_ip_int = static_cast(first_inside_detection_ip) - base2front_idx_dist - - std::ceil(braking_dist / ds); + velocity, acceleration, max_accel, max_jerk, delay_response_time); + int pass_judge_ip_int = + static_cast(first_footprint_inside_detection_ip) - std::ceil(braking_dist / ds); const auto pass_judge_line_ip = static_cast( std::clamp(pass_judge_ip_int, 0, static_cast(path_ip.points.size()) - 1)); // (5) stuck vehicle stop line int stuck_stop_line_ip_int = 0; + bool stuck_stop_line_valid = true; if (use_stuck_stopline) { - const auto stuck_stop_line_idx_ip_opt = - getFirstPointInsidePolygon(path_ip, lane_interval_ip, first_conflicting_area); + // NOTE: when ego vehicle is approaching detection area and already passed + // first_conflicting_area, this could be null. + const auto stuck_stop_line_idx_ip_opt = getFirstPointInsidePolygonByFootprint( + first_conflicting_area, interpolated_path_info, local_footprint); if (!stuck_stop_line_idx_ip_opt) { - return std::nullopt; + stuck_stop_line_valid = false; + stuck_stop_line_ip_int = 0; + } else { + stuck_stop_line_ip_int = stuck_stop_line_idx_ip_opt.value() - stop_line_margin_idx_dist; } - stuck_stop_line_ip_int = stuck_stop_line_idx_ip_opt.value(); } else { - stuck_stop_line_ip_int = std::get<0>(lane_interval_ip); + stuck_stop_line_ip_int = + std::get<0>(lane_interval_ip) - (stop_line_margin_idx_dist + base2front_idx_dist); + } + if (stuck_stop_line_ip_int < 0) { + stuck_stop_line_valid = false; } - const auto stuck_stop_line_ip = static_cast( - std::max(0, stuck_stop_line_ip_int - stop_line_margin_idx_dist - base2front_idx_dist)); + const auto stuck_stop_line_ip = static_cast(std::max(0, stuck_stop_line_ip_int)); - IntersectionStopLines intersection_stop_lines; + struct IntersectionStopLinesTemp + { + size_t closest_idx{0}; + size_t stuck_stop_line{0}; + size_t default_stop_line{0}; + size_t first_attention_stop_line{0}; + size_t occlusion_peeking_stop_line{0}; + size_t pass_judge_line{0}; + }; + + IntersectionStopLinesTemp intersection_stop_lines_temp; std::list> stop_lines = { - {&closest_idx_ip, &intersection_stop_lines.closest_idx}, - {&stuck_stop_line_ip, &intersection_stop_lines.stuck_stop_line}, - {&default_stop_line_ip, &intersection_stop_lines.default_stop_line}, - {&occlusion_peeking_line_ip, &intersection_stop_lines.occlusion_peeking_stop_line}, - {&pass_judge_line_ip, &intersection_stop_lines.pass_judge_line}, + {&closest_idx_ip, &intersection_stop_lines_temp.closest_idx}, + {&stuck_stop_line_ip, &intersection_stop_lines_temp.stuck_stop_line}, + {&default_stop_line_ip, &intersection_stop_lines_temp.default_stop_line}, + {&first_attention_stop_line_ip, &intersection_stop_lines_temp.first_attention_stop_line}, + {&occlusion_peeking_line_ip, &intersection_stop_lines_temp.occlusion_peeking_stop_line}, + {&pass_judge_line_ip, &intersection_stop_lines_temp.pass_judge_line}, }; stop_lines.sort( [](const auto & it1, const auto & it2) { return *(std::get<0>(it1)) < *(std::get<0>(it2)); }); @@ -311,27 +411,71 @@ std::optional generateIntersectionStopLines( *stop_idx = insert_idx.value(); } if ( - intersection_stop_lines.occlusion_peeking_stop_line < - intersection_stop_lines.default_stop_line) { - intersection_stop_lines.occlusion_peeking_stop_line = intersection_stop_lines.default_stop_line; + intersection_stop_lines_temp.occlusion_peeking_stop_line < + intersection_stop_lines_temp.default_stop_line) { + intersection_stop_lines_temp.occlusion_peeking_stop_line = + intersection_stop_lines_temp.default_stop_line; } if ( - intersection_stop_lines.occlusion_peeking_stop_line > intersection_stop_lines.pass_judge_line) { - intersection_stop_lines.pass_judge_line = intersection_stop_lines.occlusion_peeking_stop_line; + intersection_stop_lines_temp.occlusion_peeking_stop_line > + intersection_stop_lines_temp.pass_judge_line) { + intersection_stop_lines_temp.pass_judge_line = + intersection_stop_lines_temp.occlusion_peeking_stop_line; + } + + IntersectionStopLines intersection_stop_lines; + intersection_stop_lines.closest_idx = intersection_stop_lines_temp.closest_idx; + if (stuck_stop_line_valid) { + intersection_stop_lines.stuck_stop_line = intersection_stop_lines_temp.stuck_stop_line; + } + if (default_stop_line_valid) { + intersection_stop_lines.default_stop_line = intersection_stop_lines_temp.default_stop_line; } + if (first_attention_stop_line_valid) { + intersection_stop_lines.first_attention_stop_line = + intersection_stop_lines_temp.first_attention_stop_line; + } + if (occlusion_peeking_line_valid) { + intersection_stop_lines.occlusion_peeking_stop_line = + intersection_stop_lines_temp.occlusion_peeking_stop_line; + } + intersection_stop_lines.pass_judge_line = intersection_stop_lines_temp.pass_judge_line; return intersection_stop_lines; } std::optional getFirstPointInsidePolygon( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const std::pair lane_interval, const lanelet::CompoundPolygon3d & polygon) + const std::pair lane_interval, const lanelet::CompoundPolygon3d & polygon, + const bool search_forward) { + // NOTE: if first point is already inside the polygon, returns nullopt const auto polygon_2d = lanelet::utils::to2D(polygon); - for (size_t i = lane_interval.first; i <= lane_interval.second; ++i) { - auto p = path.points.at(i).point.pose.position; - const auto is_in_lanelet = bg::within(to_bg2d(p), polygon_2d); - if (is_in_lanelet) { - return std::make_optional(i); + if (search_forward) { + const auto & p0 = path.points.at(lane_interval.first).point.pose.position; + if (bg::within(to_bg2d(p0), polygon_2d)) { + return std::nullopt; + } + for (size_t i = lane_interval.first; i <= lane_interval.second; ++i) { + const auto & p = path.points.at(i).point.pose.position; + const auto is_in_lanelet = bg::within(to_bg2d(p), polygon_2d); + if (is_in_lanelet) { + return std::make_optional(i); + } + } + } else { + const auto & p0 = path.points.at(lane_interval.second).point.pose.position; + if (bg::within(to_bg2d(p0), polygon_2d)) { + return std::nullopt; + } + for (size_t i = lane_interval.second; i >= lane_interval.first; --i) { + const auto & p = path.points.at(i).point.pose.position; + const auto is_in_lanelet = bg::within(to_bg2d(p), polygon_2d); + if (is_in_lanelet) { + return std::make_optional(i); + } + if (i == 0) { + break; + } } } return std::nullopt; @@ -341,21 +485,42 @@ static std::optional> getFirstPointInsidePolygons( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const std::pair lane_interval, - const std::vector & polygons) + const std::vector & polygons, const bool search_forward = true) { - for (size_t i = lane_interval.first; i <= lane_interval.second; ++i) { - bool is_in_lanelet = false; - auto p = path.points.at(i).point.pose.position; - for (const auto & polygon : polygons) { - const auto polygon_2d = lanelet::utils::to2D(polygon); - is_in_lanelet = bg::within(to_bg2d(p), polygon_2d); + if (search_forward) { + for (size_t i = lane_interval.first; i <= lane_interval.second; ++i) { + bool is_in_lanelet = false; + const auto & p = path.points.at(i).point.pose.position; + for (const auto & polygon : polygons) { + const auto polygon_2d = lanelet::utils::to2D(polygon); + is_in_lanelet = bg::within(to_bg2d(p), polygon_2d); + if (is_in_lanelet) { + return std::make_optional>( + i, polygon); + } + } if (is_in_lanelet) { - return std::make_optional>( - i, polygon); + break; } } - if (is_in_lanelet) { - break; + } else { + for (size_t i = lane_interval.second; i >= lane_interval.first; --i) { + bool is_in_lanelet = false; + const auto & p = path.points.at(i).point.pose.position; + for (const auto & polygon : polygons) { + const auto polygon_2d = lanelet::utils::to2D(polygon); + is_in_lanelet = bg::within(to_bg2d(p), polygon_2d); + if (is_in_lanelet) { + return std::make_optional>( + i, polygon); + } + } + if (is_in_lanelet) { + break; + } + if (i == 0) { + break; + } } } return std::nullopt; @@ -403,6 +568,110 @@ static std::vector getPolygon3dFromLanelets( return polys; } +static std::string getTurnDirection(lanelet::ConstLanelet lane) +{ + return lane.attributeOr("turn_direction", "else"); +} + +/** + * @param pair lanelets and the vector of original lanelets in topological order (not reversed as + *in generateDetectionLaneDivisions()) + **/ +static std::pair> +mergeLaneletsByTopologicalSort( + const lanelet::ConstLanelets & lanelets, + const lanelet::routing::RoutingGraphPtr routing_graph_ptr) +{ + const int n_node = lanelets.size(); + std::vector> adjacency(n_node); + for (int dst = 0; dst < n_node; ++dst) { + adjacency[dst].resize(n_node); + for (int src = 0; src < n_node; ++src) { + adjacency[dst][src] = false; + } + } + std::set lanelet_ids; + std::unordered_map id2ind; + std::unordered_map ind2id; + std::unordered_map id2lanelet; + int ind = 0; + for (const auto & lanelet : lanelets) { + lanelet_ids.insert(lanelet.id()); + const auto id = lanelet.id(); + id2ind[id] = ind; + ind2id[ind] = id; + id2lanelet[id] = lanelet; + ind++; + } + // NOTE: this function aims to traverse the detection lanelet backward from ego side to farthest + // side, so if lane B follows lane A on the routing_graph, adj[B][A] = true + for (const auto & lanelet : lanelets) { + const auto & followings = routing_graph_ptr->following(lanelet); + const int dst = lanelet.id(); + for (const auto & following : followings) { + if (const int src = following.id(); lanelet_ids.find(src) != lanelet_ids.end()) { + adjacency[(id2ind[src])][(id2ind[dst])] = true; + } + } + } + // terminal node + std::map> branches; + auto has_no_previous = [&](const int node) { + for (int dst = 0; dst < n_node; dst++) { + if (adjacency[dst][node]) { + return false; + } + } + return true; + }; + for (int src = 0; src < n_node; src++) { + if (!has_no_previous(src)) { + continue; + } + // So `src` has no previous lanelets + branches[(ind2id[src])] = std::vector{}; + auto & branch = branches[(ind2id[src])]; + lanelet::Id node_iter = ind2id[src]; + std::set visited_ids; + while (true) { + const auto & destinations = adjacency[(id2ind[node_iter])]; + // NOTE: assuming detection lanelets have only one "previous"(on the routing_graph) lanelet + const auto next = std::find(destinations.begin(), destinations.end(), true); + if (next == destinations.end()) { + branch.push_back(node_iter); + break; + } + if (visited_ids.find(node_iter) != visited_ids.end()) { + // loop detected + break; + } + branch.push_back(node_iter); + visited_ids.insert(node_iter); + node_iter = ind2id[std::distance(destinations.begin(), next)]; + } + } + for (decltype(branches)::iterator it = branches.begin(); it != branches.end(); it++) { + auto & branch = it->second; + std::reverse(branch.begin(), branch.end()); + } + lanelet::ConstLanelets merged; + std::vector originals; + for (const auto & [id, sub_ids] : branches) { + if (sub_ids.size() == 0) { + continue; + } + lanelet::ConstLanelets merge; + originals.push_back(lanelet::ConstLanelets({})); + auto & original = originals.back(); + for (const auto sub_id : sub_ids) { + merge.push_back(id2lanelet[sub_id]); + original.push_back(id2lanelet[sub_id]); + } + merged.push_back(lanelet::utils::combineLaneletsShape(merge)); + } + return {merged, originals}; +} + IntersectionLanelets getObjectiveLanelets( lanelet::LaneletMapConstPtr lanelet_map_ptr, lanelet::routing::RoutingGraphPtr routing_graph_ptr, const lanelet::ConstLanelet assigned_lanelet, const lanelet::ConstLanelets & lanelets_on_path, @@ -540,14 +809,59 @@ IntersectionLanelets getObjectiveLanelets( } } } + lanelet::ConstLanelets occlusion_detection_and_preceding_lanelets_wo_turn_direction; + for (const auto & ll : occlusion_detection_and_preceding_lanelets) { + const auto turn_direction = getTurnDirection(ll); + if (turn_direction == "left" || turn_direction == "right") { + continue; + } + occlusion_detection_and_preceding_lanelets_wo_turn_direction.push_back(ll); + } + + auto [attention_lanelets, original_attention_lanelet_sequences] = + mergeLaneletsByTopologicalSort(detection_and_preceding_lanelets, routing_graph_ptr); IntersectionLanelets result; - result.attention_ = std::move(detection_and_preceding_lanelets); + result.attention_ = std::move(attention_lanelets); + for (const auto & original_attention_lanelet_seq : original_attention_lanelet_sequences) { + // NOTE: in mergeLaneletsByTopologicalSort(), sub_ids are empty checked, so it is ensured that + // back() exists. + std::optional stop_line{std::nullopt}; + for (auto it = original_attention_lanelet_seq.rbegin(); + it != original_attention_lanelet_seq.rend(); ++it) { + const auto traffic_lights = it->regulatoryElementsAs(); + for (const auto & traffic_light : traffic_lights) { + const auto stop_line_opt = traffic_light->stopLine(); + if (!stop_line_opt) continue; + stop_line = stop_line_opt.get(); + break; + } + if (stop_line) break; + } + result.attention_stop_lines_.push_back(stop_line); + } result.attention_non_preceding_ = std::move(detection_lanelets); + for (unsigned i = 0; i < result.attention_non_preceding_.size(); ++i) { + std::optional stop_line = std::nullopt; + const auto & ll = result.attention_non_preceding_.at(i); + const auto traffic_lights = ll.regulatoryElementsAs(); + for (const auto & traffic_light : traffic_lights) { + const auto stop_line_opt = traffic_light->stopLine(); + if (!stop_line_opt) continue; + stop_line = stop_line_opt.get(); + } + result.attention_non_preceding_stop_lines_.push_back(stop_line); + } result.conflicting_ = std::move(conflicting_ex_ego_lanelets); result.adjacent_ = planning_utils::getConstLaneletsFromIds(lanelet_map_ptr, associative_ids); - result.occlusion_attention_ = std::move(occlusion_detection_and_preceding_lanelets); - // compoundPolygon3d + // NOTE: occlusion_attention is not inverted here + // TODO(Mamoru Sobue): apply mergeLaneletsByTopologicalSort for occlusion lanelets as well and + // then trim part of them based on curvature threshold + result.occlusion_attention_ = + std::move(occlusion_detection_and_preceding_lanelets_wo_turn_direction); + + // NOTE: to properly update(), each element in conflicting_/conflicting_area_, + // attention_non_preceding_/attention_non_preceding_area_ need to be matched result.attention_area_ = getPolygon3dFromLanelets(result.attention_); result.attention_non_preceding_area_ = getPolygon3dFromLanelets(result.attention_non_preceding_); result.conflicting_area_ = getPolygon3dFromLanelets(result.conflicting_); @@ -556,11 +870,6 @@ IntersectionLanelets getObjectiveLanelets( return result; } -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) @@ -674,11 +983,11 @@ bool hasAssociatedTrafficLight(lanelet::ConstLanelet lane) return tl_id.has_value(); } -bool isTrafficLightArrowActivated( - lanelet::ConstLanelet lane, - const std::map & tl_infos) +TrafficPrioritizedLevel getTrafficPrioritizedLevel( + lanelet::ConstLanelet lane, const std::map & tl_infos) { - const auto & turn_direction = lane.attributeOr("turn_direction", "else"); + using TrafficSignalElement = autoware_perception_msgs::msg::TrafficSignalElement; + std::optional tl_id = std::nullopt; for (auto && tl_reg_elem : lane.regulatoryElementsAs()) { tl_id = tl_reg_elem->id(); @@ -686,153 +995,95 @@ bool isTrafficLightArrowActivated( } if (!tl_id) { // this lane has no traffic light - return false; + return TrafficPrioritizedLevel::NOT_PRIORITIZED; } const auto tl_info_it = tl_infos.find(tl_id.value()); if (tl_info_it == tl_infos.end()) { // the info of this traffic light is not available - return false; + return TrafficPrioritizedLevel::NOT_PRIORITIZED; } const auto & tl_info = tl_info_it->second; - for (auto && tl_light : tl_info.signal.lights) { - if (tl_light.color != autoware_auto_perception_msgs::msg::TrafficLight::GREEN) continue; - if (tl_light.status != autoware_auto_perception_msgs::msg::TrafficLight::SOLID_ON) continue; - if ( - turn_direction == std::string("left") && - tl_light.shape == autoware_auto_perception_msgs::msg::TrafficLight::LEFT_ARROW) - return true; - if ( - turn_direction == std::string("right") && - tl_light.shape == autoware_auto_perception_msgs::msg::TrafficLight::RIGHT_ARROW) - return true; + bool has_amber_signal{false}; + for (auto && tl_light : tl_info.signal.elements) { + if (tl_light.color == TrafficSignalElement::AMBER) { + has_amber_signal = true; + } + if (tl_light.color == TrafficSignalElement::RED) { + // NOTE: Return here since the red signal has the highest priority. + return TrafficPrioritizedLevel::FULLY_PRIORITIZED; + } } - return false; + if (has_amber_signal) { + return TrafficPrioritizedLevel::PARTIALLY_PRIORITIZED; + } + return TrafficPrioritizedLevel::NOT_PRIORITIZED; } -std::vector generateDetectionLaneDivisions( +double getHighestCurvature(const lanelet::ConstLineString3d & centerline) +{ + std::vector points; + for (auto point = centerline.begin(); point != centerline.end(); point++) { + points.push_back(*point); + } + + SplineInterpolationPoints2d interpolation(points); + const std::vector curvatures = interpolation.getSplineInterpolatedCurvatures(); + std::vector curvatures_positive; + for (const auto & curvature : curvatures) { + curvatures_positive.push_back(std::fabs(curvature)); + } + return *std::max_element(curvatures_positive.begin(), curvatures_positive.end()); +} + +std::vector generateDetectionLaneDivisions( lanelet::ConstLanelets detection_lanelets_all, - [[maybe_unused]] const lanelet::routing::RoutingGraphPtr routing_graph_ptr, - const double resolution) + const lanelet::routing::RoutingGraphPtr routing_graph_ptr, const double resolution, + const double curvature_threshold, const double curvature_calculation_ds) { using lanelet::utils::getCenterlineWithOffset; - using lanelet::utils::to2D; // (0) remove left/right lanelet lanelet::ConstLanelets detection_lanelets; for (const auto & detection_lanelet : detection_lanelets_all) { - const auto turn_direction = getTurnDirection(detection_lanelet); - if (turn_direction.compare("left") == 0 || turn_direction.compare("right") == 0) { + // TODO(Mamoru Sobue): instead of ignoring, only trim straight part of lanelet + const auto fine_centerline = + lanelet::utils::generateFineCenterline(detection_lanelet, curvature_calculation_ds); + const double highest_curvature = getHighestCurvature(fine_centerline); + if (highest_curvature > curvature_threshold) { continue; } detection_lanelets.push_back(detection_lanelet); } // (1) tsort detection_lanelets - // generate adjacency matrix - // if lanelet1 => lanelet2; then adjacency[lanelet2][lanelet1] = true - const int n_node = detection_lanelets.size(); - std::vector> adjacency(n_node); - for (int dst = 0; dst < n_node; ++dst) { - adjacency[dst].resize(n_node); - for (int src = 0; src < n_node; ++src) { - adjacency[dst][src] = false; - } - } - std::set detection_lanelet_ids; - std::unordered_map id2ind, ind2id; - std::unordered_map id2lanelet; - int ind = 0; - for (const auto & detection_lanelet : detection_lanelets) { - detection_lanelet_ids.insert(detection_lanelet.id()); - const int id = detection_lanelet.id(); - id2ind[id] = ind; - ind2id[ind] = id; - id2lanelet[id] = detection_lanelet; - ind++; - } - for (const auto & detection_lanelet : detection_lanelets) { - const auto & followings = routing_graph_ptr->following(detection_lanelet); - const int dst = detection_lanelet.id(); - for (const auto & following : followings) { - if (const int src = following.id(); - detection_lanelet_ids.find(src) != detection_lanelet_ids.end()) { - adjacency[(id2ind[src])][(id2ind[dst])] = true; - } - } - } - // terminal node - std::map> branches; - auto has_no_previous = [&](const int node) { - for (int dst = 0; dst < n_node; dst++) { - if (adjacency[dst][node]) { - return false; - } - } - return true; - }; - for (int src = 0; src < n_node; src++) { - if (!has_no_previous(src)) { - continue; - } - branches[(ind2id[src])] = std::vector{}; - auto & branch = branches[(ind2id[src])]; - int node_iter = ind2id[src]; - while (true) { - const auto & destinations = adjacency[(id2ind[node_iter])]; - // NOTE: assuming detection lanelets have only one previous lanelet - const auto next = std::find(destinations.begin(), destinations.end(), true); - if (next == destinations.end()) { - branch.push_back(node_iter); - break; - } - branch.push_back(node_iter); - node_iter = ind2id[std::distance(destinations.begin(), next)]; - } - } - for (decltype(branches)::iterator it = branches.begin(); it != branches.end(); it++) { - auto & branch = it->second; - std::reverse(branch.begin(), branch.end()); - } + const auto [merged_detection_lanelets, originals] = + mergeLaneletsByTopologicalSort(detection_lanelets, routing_graph_ptr); // (2) merge each branch to one lanelet // NOTE: somehow bg::area() for merged lanelet does not work, so calculate it here - std::unordered_map> merged_branches; - for (const auto & [src, branch] : branches) { - lanelet::Points3d lefts; - lanelet::Points3d rights; + std::vector> merged_lanelet_with_area; + for (unsigned i = 0; i < merged_detection_lanelets.size(); ++i) { + const auto & merged_detection_lanelet = merged_detection_lanelets.at(i); + const auto & original = originals.at(i); double area = 0; - for (const auto & lane_id : branch) { - const auto lane = id2lanelet[lane_id]; - for (const auto & left_point : lane.leftBound()) { - lefts.push_back(lanelet::Point3d(left_point)); - } - for (const auto & right_point : lane.rightBound()) { - rights.push_back(lanelet::Point3d(right_point)); - } - area += bg::area(lane.polygon2d().basicPolygon()); + for (const auto & partition : original) { + area += bg::area(partition.polygon2d().basicPolygon()); } - lanelet::LineString3d right = lanelet::LineString3d(lanelet::InvalId, lefts).invert(); - lanelet::LineString3d left = lanelet::LineString3d(lanelet::InvalId, rights).invert(); - lanelet::Lanelet merged = lanelet::Lanelet(lanelet::InvalId, left, right); - merged_branches[src] = std::make_pair(merged, area); + merged_lanelet_with_area.emplace_back(merged_detection_lanelet, area); } // (3) discretize each merged lanelet - std::vector detection_divisions; - for (const auto & [last_lane_id, branch] : merged_branches) { - DiscretizedLane detection_division; - detection_division.lane_id = last_lane_id; - const auto detection_lanelet = branch.first; - const double area = branch.second; - const double length = bg::length(detection_lanelet.centerline()); + std::vector detection_divisions; + for (const auto & [merged_lanelet, area] : merged_lanelet_with_area) { + const double length = bg::length(merged_lanelet.centerline()); const double width = area / length; - auto & divisions = detection_division.divisions; for (int i = 0; i < static_cast(width / resolution); ++i) { const double offset = resolution * i - width / 2; - divisions.push_back(to2D(getCenterlineWithOffset(detection_lanelet, offset, resolution))); + detection_divisions.push_back( + getCenterlineWithOffset(merged_lanelet, offset, resolution).invert()); } - divisions.push_back(to2D(getCenterlineWithOffset(detection_lanelet, width / 2, resolution))); - detection_divisions.push_back(detection_division); + detection_divisions.push_back( + getCenterlineWithOffset(merged_lanelet, width / 2, resolution).invert()); } return detection_divisions; } @@ -918,36 +1169,69 @@ bool checkStuckVehicleInIntersection( return false; } +bool checkYieldStuckVehicleInIntersection( + const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr objects_ptr, + const lanelet::BasicPolygon2d & ego_poly, const lanelet::CompoundPolygon3d & first_attention_area, + const double stuck_vehicle_vel_thr, const double yield_stuck_distance_thr, DebugData * debug_data) +{ + const auto first_attention_area_2d = lanelet::utils::to2D(first_attention_area); + Polygon2d first_attention_area_poly; + for (const auto & p : first_attention_area_2d) { + first_attention_area_poly.outer().emplace_back(p.x(), p.y()); + } + + for (const auto & object : objects_ptr->objects) { + if (!isTargetStuckVehicleType(object)) { + continue; // not target vehicle type + } + const auto obj_v_norm = std::hypot( + object.kinematics.initial_twist_with_covariance.twist.linear.x, + object.kinematics.initial_twist_with_covariance.twist.linear.y); + if (obj_v_norm > stuck_vehicle_vel_thr) { + continue; // not stop vehicle + } + + const auto obj_footprint = tier4_autoware_utils::toPolygon2d(object); + + // check if the object is too close to the ego path + if (yield_stuck_distance_thr < bg::distance(ego_poly, obj_footprint)) { + continue; + } + + // check if the footprint is in the stuck detect area + const bool is_in_stuck_area = bg::within(obj_footprint, first_attention_area_poly); + if (is_in_stuck_area && debug_data) { + debug_data->yield_stuck_targets.objects.push_back(object); + return true; + } + } + return false; +} + Polygon2d generateStuckVehicleDetectAreaPolygon( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const lanelet::ConstLanelets & ego_lane_with_next_lane, const int closest_idx, - const double stuck_vehicle_detect_dist, const double stuck_vehicle_ignore_dist, - const double vehicle_length_m) + const util::PathLanelets & path_lanelets, const double stuck_vehicle_detect_dist) { using lanelet::utils::getArcCoordinates; using lanelet::utils::getLaneletLength3d; using lanelet::utils::getPolygonFromArcLength; using lanelet::utils::to2D; - const double extra_dist = stuck_vehicle_detect_dist + vehicle_length_m; - const double ignore_dist = stuck_vehicle_ignore_dist + vehicle_length_m; - - const double intersection_exit_length = getLaneletLength3d(ego_lane_with_next_lane.front()); - - const auto closest_arc_coords = getArcCoordinates( - ego_lane_with_next_lane, tier4_autoware_utils::getPose(path.points.at(closest_idx).point)); - - const double start_arc_length = intersection_exit_length - ignore_dist > closest_arc_coords.length - ? intersection_exit_length - ignore_dist - : closest_arc_coords.length; - - const double end_arc_length = getLaneletLength3d(ego_lane_with_next_lane.front()) + extra_dist; + Polygon2d polygon{}; + if (path_lanelets.conflicting_interval_and_remaining.size() == 0) { + return polygon; + } + double target_polygon_length = + getLaneletLength3d(path_lanelets.conflicting_interval_and_remaining); + lanelet::ConstLanelets targets = path_lanelets.conflicting_interval_and_remaining; + if (path_lanelets.next) { + targets.push_back(path_lanelets.next.value()); + const double next_arc_length = + std::min(stuck_vehicle_detect_dist, getLaneletLength3d(path_lanelets.next.value())); + target_polygon_length += next_arc_length; + } const auto target_polygon = - to2D(getPolygonFromArcLength(ego_lane_with_next_lane, start_arc_length, end_arc_length)) - .basicPolygon(); - - Polygon2d polygon{}; + to2D(getPolygonFromArcLength(targets, 0, target_polygon_length)).basicPolygon(); if (target_polygon.empty()) { return polygon; @@ -963,45 +1247,53 @@ Polygon2d generateStuckVehicleDetectAreaPolygon( return polygon; } -bool checkAngleForTargetLanelets( +std::optional checkAngleForTargetLanelets( const geometry_msgs::msg::Pose & pose, const lanelet::ConstLanelets & target_lanelets, const double detection_area_angle_thr, const bool consider_wrong_direction_vehicle, - const double margin) + const double dist_margin, const bool is_parked_vehicle) { - for (const auto & ll : target_lanelets) { - if (!lanelet::utils::isInLanelet(pose, ll, margin)) { + for (unsigned i = 0; i < target_lanelets.size(); ++i) { + const auto & ll = target_lanelets.at(i); + if (!lanelet::utils::isInLanelet(pose, ll, dist_margin)) { continue; } const double ll_angle = lanelet::utils::getLaneletAngle(ll, pose.position); const double pose_angle = tf2::getYaw(pose.orientation); - const double angle_diff = tier4_autoware_utils::normalizeRadian(ll_angle - pose_angle); + const double angle_diff = tier4_autoware_utils::normalizeRadian(ll_angle - pose_angle, -M_PI); if (consider_wrong_direction_vehicle) { if (std::fabs(angle_diff) > 1.57 || std::fabs(angle_diff) < detection_area_angle_thr) { - return true; + return std::make_optional(i); } } else { if (std::fabs(angle_diff) < detection_area_angle_thr) { - return true; + return std::make_optional(i); + } + // NOTE: sometimes parked vehicle direction is reversed even if its longitudinal velocity is + // positive + if ( + is_parked_vehicle && (std::fabs(angle_diff) < detection_area_angle_thr || + (std::fabs(angle_diff + M_PI) < detection_area_angle_thr))) { + return std::make_optional(i); } } } - return false; + return std::nullopt; } void cutPredictPathWithDuration( - autoware_auto_perception_msgs::msg::PredictedObjects * objects_ptr, - const rclcpp::Clock::SharedPtr clock, const double time_thr) + util::TargetObjects * target_objects, const rclcpp::Clock::SharedPtr clock, const double time_thr) { const rclcpp::Time current_time = clock->now(); - for (auto & object : objects_ptr->objects) { // each objects - for (auto & predicted_path : object.kinematics.predicted_paths) { // each predicted paths + for (auto & target_object : target_objects->all_attention_objects) { // each objects + for (auto & predicted_path : + target_object.object.kinematics.predicted_paths) { // each predicted paths const auto origin_path = predicted_path; predicted_path.path.clear(); for (size_t k = 0; k < origin_path.path.size(); ++k) { // each path points const auto & predicted_pose = origin_path.path.at(k); const auto predicted_time = - rclcpp::Time(objects_ptr->header.stamp) + + rclcpp::Time(target_objects->header.stamp) + rclcpp::Duration(origin_path.time_step) * static_cast(k); if ((predicted_time - current_time).seconds() < time_thr) { predicted_path.path.push_back(predicted_pose); @@ -1014,8 +1306,9 @@ void cutPredictPathWithDuration( TimeDistanceArray calcIntersectionPassingTime( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const std::shared_ptr & planner_data, const std::set & associative_ids, - const int closest_idx, const double time_delay, const double intersection_velocity, - const double minimum_ego_velocity) + const size_t closest_idx, const size_t last_intersection_stop_line_candidate_idx, + const double time_delay, const double intersection_velocity, const double minimum_ego_velocity, + const bool use_upstream_velocity, const double minimum_upstream_velocity) { double dist_sum = 0.0; int assigned_lane_found = false; @@ -1025,7 +1318,9 @@ TimeDistanceArray calcIntersectionPassingTime( PathWithLaneId reference_path; for (size_t i = closest_idx; i < path.points.size(); ++i) { auto reference_point = path.points.at(i); - reference_point.point.longitudinal_velocity_mps = intersection_velocity; + if (!use_upstream_velocity) { + reference_point.point.longitudinal_velocity_mps = intersection_velocity; + } reference_path.points.push_back(reference_point); bool has_objective_lane_id = hasLaneIds(path.points.at(i), associative_ids); if (assigned_lane_found && !has_objective_lane_id) { @@ -1046,6 +1341,19 @@ TimeDistanceArray calcIntersectionPassingTime( dist_sum = 0.0; double passing_time = time_delay; time_distance_array.emplace_back(passing_time, dist_sum); + + // NOTE: `reference_path` is resampled in `reference_smoothed_path`, so + // `last_intersection_stop_line_candidate_idx` makes no sense + const auto last_intersection_stop_line_candidate_point_orig = + path.points.at(last_intersection_stop_line_candidate_idx).point.pose; + const auto last_intersection_stop_line_candidate_nearest_ind_opt = motion_utils::findNearestIndex( + smoothed_reference_path.points, last_intersection_stop_line_candidate_point_orig, 3.0, M_PI_4); + if (!last_intersection_stop_line_candidate_nearest_ind_opt) { + return time_distance_array; + } + const auto last_intersection_stop_line_candidate_nearest_ind = + last_intersection_stop_line_candidate_nearest_ind_opt.value(); + for (size_t i = 1; i < smoothed_reference_path.points.size(); ++i) { const auto & p1 = smoothed_reference_path.points.at(i - 1); const auto & p2 = smoothed_reference_path.points.at(i); @@ -1056,10 +1364,13 @@ TimeDistanceArray calcIntersectionPassingTime( // use average velocity between p1 and p2 const double average_velocity = (p1.point.longitudinal_velocity_mps + p2.point.longitudinal_velocity_mps) / 2.0; - passing_time += - (dist / std::max( - minimum_ego_velocity, - average_velocity)); // to avoid zero-division + const double minimum_ego_velocity_division = + (use_upstream_velocity && i > last_intersection_stop_line_candidate_nearest_ind) + ? minimum_upstream_velocity /* to avoid null division */ + : minimum_ego_velocity; + const double passing_velocity = + std::max(minimum_ego_velocity_division, average_velocity); + passing_time += (dist / passing_velocity); time_distance_array.emplace_back(passing_time, dist_sum); } @@ -1095,22 +1406,25 @@ double calcDistanceUntilIntersectionLanelet( } void IntersectionLanelets::update( - const bool tl_arrow_solid_on, const InterpolatedPathInfo & interpolated_path_info) + const bool is_prioritized, const InterpolatedPathInfo & interpolated_path_info, + const tier4_autoware_utils::LinearRing2d & footprint) { - tl_arrow_solid_on_ = tl_arrow_solid_on; + is_prioritized_ = is_prioritized; // find the first conflicting/detection area polygon intersecting the path - const auto & path = interpolated_path_info.path; - const auto & lane_interval = interpolated_path_info.lane_id_interval.value(); - { - auto first = getFirstPointInsidePolygons(path, lane_interval, conflicting_area_); - if (first && !first_conflicting_area_) { - first_conflicting_area_ = first.value().second; + if (!first_conflicting_area_) { + auto first = + getFirstPointInsidePolygonsByFootprint(conflicting_area_, interpolated_path_info, footprint); + if (first) { + first_conflicting_lane_ = conflicting_.at(first.value().second); + first_conflicting_area_ = conflicting_area_.at(first.value().second); } } - { - auto first = getFirstPointInsidePolygons(path, lane_interval, attention_area_); - if (first && !first_attention_area_) { - first_attention_area_ = first.value().second; + if (!first_attention_area_) { + auto first = getFirstPointInsidePolygonsByFootprint( + attention_non_preceding_area_, interpolated_path_info, footprint); + if (first) { + first_attention_lane_ = attention_non_preceding_.at(first.value().second); + first_attention_area_ = attention_non_preceding_area_.at(first.value().second); } } } @@ -1130,13 +1444,19 @@ static lanelet::ConstLanelets getPrevLanelets( std::optional generatePathLanelets( const lanelet::ConstLanelets & lanelets_on_path, - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const std::set & associative_ids, const size_t closest_idx, const double width) + const util::InterpolatedPathInfo & interpolated_path_info, const std::set & associative_ids, + const lanelet::CompoundPolygon3d & first_conflicting_area, + const std::vector & conflicting_areas, + const std::optional & first_attention_area, + const std::vector & attention_areas, const size_t closest_idx, + const double width) { - const auto assigned_lane_interval_opt = findLaneIdsInterval(path, associative_ids); + const auto & assigned_lane_interval_opt = interpolated_path_info.lane_id_interval; if (!assigned_lane_interval_opt) { return std::nullopt; } + const auto assigned_lane_interval = assigned_lane_interval_opt.value(); + const auto & path = interpolated_path_info.path; PathLanelets path_lanelets; // prev @@ -1144,7 +1464,7 @@ std::optional generatePathLanelets( path_lanelets.all = path_lanelets.prev; // entry2ego if exist - const auto [assigned_lane_start, assigned_lane_end] = assigned_lane_interval_opt.value(); + const auto [assigned_lane_start, assigned_lane_end] = assigned_lane_interval; if (closest_idx > assigned_lane_start) { path_lanelets.all.push_back( planning_utils::generatePathLanelet(path, assigned_lane_start, closest_idx, width)); @@ -1162,14 +1482,51 @@ std::optional generatePathLanelets( const auto next_lane_interval_opt = findLaneIdsInterval(path, {next_id}); if (next_lane_interval_opt) { const auto [next_start, next_end] = next_lane_interval_opt.value(); - path_lanelets.next = - planning_utils::generatePathLanelet(path, next_start + 1, next_end, width); + path_lanelets.next = planning_utils::generatePathLanelet(path, next_start, next_end, width); path_lanelets.all.push_back(path_lanelets.next.value()); } } + const auto first_inside_conflicting_idx_opt = + first_attention_area.has_value() + ? getFirstPointInsidePolygon(path, assigned_lane_interval, first_attention_area.value()) + : getFirstPointInsidePolygon(path, assigned_lane_interval, first_conflicting_area); + const auto last_inside_conflicting_idx_opt = + first_attention_area.has_value() + ? getFirstPointInsidePolygons(path, assigned_lane_interval, attention_areas, false) + : getFirstPointInsidePolygons(path, assigned_lane_interval, conflicting_areas, false); + if (first_inside_conflicting_idx_opt && last_inside_conflicting_idx_opt) { + const auto first_inside_conflicting_idx = first_inside_conflicting_idx_opt.value(); + const auto last_inside_conflicting_idx = last_inside_conflicting_idx_opt.value().first; + lanelet::ConstLanelet conflicting_interval = planning_utils::generatePathLanelet( + path, first_inside_conflicting_idx, last_inside_conflicting_idx, width); + path_lanelets.conflicting_interval_and_remaining.push_back(std::move(conflicting_interval)); + if (last_inside_conflicting_idx < assigned_lane_end) { + lanelet::ConstLanelet remaining_interval = planning_utils::generatePathLanelet( + path, last_inside_conflicting_idx, assigned_lane_end, width); + path_lanelets.conflicting_interval_and_remaining.push_back(std::move(remaining_interval)); + } + } return path_lanelets; } +void TargetObject::calc_dist_to_stop_line() +{ + if (!attention_lanelet || !stop_line) { + return; + } + const auto attention_lanelet_val = attention_lanelet.value(); + const auto object_arc_coords = lanelet::utils::getArcCoordinates( + {attention_lanelet_val}, object.kinematics.initial_pose_with_covariance.pose); + const auto stop_line_val = stop_line.value(); + geometry_msgs::msg::Pose stopline_center; + stopline_center.position.x = (stop_line_val.front().x() + stop_line_val.back().x()) / 2.0; + stopline_center.position.y = (stop_line_val.front().y() + stop_line_val.back().y()) / 2.0; + stopline_center.position.z = (stop_line_val.front().z() + stop_line_val.back().z()) / 2.0; + const auto stopline_arc_coords = + lanelet::utils::getArcCoordinates({attention_lanelet_val}, stopline_center); + dist_to_stop_line = (stopline_arc_coords.length - object_arc_coords.length); +} + } // namespace util } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_intersection_module/src/util.hpp b/planning/behavior_velocity_intersection_module/src/util.hpp index 287a75979927b..5faacd4325b06 100644 --- a/planning/behavior_velocity_intersection_module/src/util.hpp +++ b/planning/behavior_velocity_intersection_module/src/util.hpp @@ -20,14 +20,6 @@ #include -#include - -#ifdef ROS_DISTRO_GALACTIC -#include -#else -#include -#endif - #include #include #include @@ -78,12 +70,14 @@ std::optional generateIntersectionStopLines( const lanelet::CompoundPolygon3d & first_detection_area, const std::shared_ptr & planner_data, const InterpolatedPathInfo & interpolated_path_info, const bool use_stuck_stopline, - const double stop_line_margin, const double peeking_offset, + const double stop_line_margin, const double max_accel, const double max_jerk, + const double delay_response_time, const double peeking_offset, autoware_auto_planning_msgs::msg::PathWithLaneId * original_path); std::optional getFirstPointInsidePolygon( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const std::pair lane_interval, const lanelet::CompoundPolygon3d & polygon); + const std::pair lane_interval, const lanelet::CompoundPolygon3d & polygon, + const bool search_forward = true); /** * @brief check if ego is over the target_idx. If the index is same, compare the exact pose @@ -110,13 +104,14 @@ std::optional getIntersectionArea( lanelet::ConstLanelet assigned_lane, lanelet::LaneletMapConstPtr lanelet_map_ptr); bool hasAssociatedTrafficLight(lanelet::ConstLanelet lane); -bool isTrafficLightArrowActivated( - lanelet::ConstLanelet lane, - const std::map & tl_infos); -std::vector generateDetectionLaneDivisions( +TrafficPrioritizedLevel getTrafficPrioritizedLevel( + lanelet::ConstLanelet lane, const std::map & tl_infos); + +std::vector generateDetectionLaneDivisions( lanelet::ConstLanelets detection_lanelets, - const lanelet::routing::RoutingGraphPtr routing_graph_ptr, const double resolution); + const lanelet::routing::RoutingGraphPtr routing_graph_ptr, const double resolution, + const double curvature_threshold, const double curvature_calculation_ds); std::optional generateInterpolatedPath( const int lane_id, const std::set & associative_lane_ids, @@ -131,26 +126,30 @@ bool checkStuckVehicleInIntersection( const Polygon2d & stuck_vehicle_detect_area, const double stuck_vehicle_vel_thr, DebugData * debug_data); +bool checkYieldStuckVehicleInIntersection( + const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr objects_ptr, + const lanelet::BasicPolygon2d & ego_poly, const lanelet::CompoundPolygon3d & first_attention_area, + const double stuck_vehicle_vel_thr, const double yield_stuck_distance_thr, + DebugData * debug_data); + Polygon2d generateStuckVehicleDetectAreaPolygon( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const lanelet::ConstLanelets & ego_lane_with_next_lane, const int closest_idx, - const double stuck_vehicle_detect_dist, const double stuck_vehicle_ignore_dist, - const double vehicle_length_m); + const util::PathLanelets & path_lanelets, const double stuck_vehicle_detect_dist); -bool checkAngleForTargetLanelets( +std::optional checkAngleForTargetLanelets( const geometry_msgs::msg::Pose & pose, const lanelet::ConstLanelets & target_lanelets, const double detection_area_angle_thr, const bool consider_wrong_direction_vehicle, - const double margin = 0.0); + const double dist_margin, const bool is_parked_vehicle); void cutPredictPathWithDuration( - autoware_auto_perception_msgs::msg::PredictedObjects * objects_ptr, - const rclcpp::Clock::SharedPtr clock, const double time_thr); + util::TargetObjects * target_objects, const rclcpp::Clock::SharedPtr clock, + const double time_thr); TimeDistanceArray calcIntersectionPassingTime( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const std::shared_ptr & planner_data, const std::set & associative_ids, - const int closest_idx, const double time_delay, const double intersection_velocity, - const double minimum_ego_velocity); + const size_t closest_idx, const size_t last_intersection_stop_line_candidate_idx, + const double time_delay, const double intersection_velocity, const double minimum_ego_velocity, + const bool use_upstream_velocity, const double minimum_upstream_velocity); double calcDistanceUntilIntersectionLanelet( const lanelet::ConstLanelet & assigned_lanelet, @@ -158,8 +157,12 @@ double calcDistanceUntilIntersectionLanelet( std::optional generatePathLanelets( const lanelet::ConstLanelets & lanelets_on_path, - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const std::set & associative_ids, const size_t closest_idx, const double width); + const util::InterpolatedPathInfo & interpolated_path_info, const std::set & associative_ids, + const lanelet::CompoundPolygon3d & first_conflicting_area, + const std::vector & conflicting_areas, + const std::optional & first_attention_area, + const std::vector & attention_areas, const size_t closest_idx, + const double width); } // namespace util } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_intersection_module/src/util_type.hpp b/planning/behavior_velocity_intersection_module/src/util_type.hpp index 26187d75ff53c..73e60aea6471a 100644 --- a/planning/behavior_velocity_intersection_module/src/util_type.hpp +++ b/planning/behavior_velocity_intersection_module/src/util_type.hpp @@ -15,6 +15,8 @@ #ifndef UTIL_TYPE_HPP_ #define UTIL_TYPE_HPP_ +#include + #include #include #include @@ -34,50 +36,62 @@ namespace behavior_velocity_planner::util struct DebugData { - std::optional collision_stop_wall_pose = std::nullopt; - std::optional occlusion_stop_wall_pose = std::nullopt; - std::optional occlusion_first_stop_wall_pose = std::nullopt; - std::optional pass_judge_wall_pose = std::nullopt; - std::optional> attention_area = std::nullopt; - std::optional intersection_area = std::nullopt; - std::optional ego_lane = std::nullopt; - std::optional> adjacent_area = std::nullopt; - std::optional stuck_vehicle_detect_area = std::nullopt; - std::optional candidate_collision_ego_lane_polygon = std::nullopt; + std::optional collision_stop_wall_pose{std::nullopt}; + std::optional occlusion_stop_wall_pose{std::nullopt}; + std::optional occlusion_first_stop_wall_pose{std::nullopt}; + std::optional pass_judge_wall_pose{std::nullopt}; + std::optional> attention_area{std::nullopt}; + std::optional> occlusion_attention_area{std::nullopt}; + std::optional ego_lane{std::nullopt}; + std::optional> adjacent_area{std::nullopt}; + std::optional stuck_vehicle_detect_area{std::nullopt}; + std::optional candidate_collision_ego_lane_polygon{std::nullopt}; std::vector candidate_collision_object_polygons; autoware_auto_perception_msgs::msg::PredictedObjects conflicting_targets; + autoware_auto_perception_msgs::msg::PredictedObjects amber_ignore_targets; + autoware_auto_perception_msgs::msg::PredictedObjects red_overshoot_ignore_targets; autoware_auto_perception_msgs::msg::PredictedObjects stuck_targets; - std::optional nearest_occlusion_point = std::nullopt; - std::optional nearest_occlusion_projection_point = std::nullopt; + autoware_auto_perception_msgs::msg::PredictedObjects yield_stuck_targets; std::vector occlusion_polygons; + std::optional> + nearest_occlusion_projection{std::nullopt}; + autoware_auto_perception_msgs::msg::PredictedObjects blocking_attention_objects; + std::optional absence_traffic_light_creep_wall{std::nullopt}; + std::optional static_occlusion_with_traffic_light_timeout{std::nullopt}; }; struct InterpolatedPathInfo { autoware_auto_planning_msgs::msg::PathWithLaneId path; - double ds; - int lane_id; - std::set associative_lane_ids; - std::optional> lane_id_interval; + double ds{0.0}; + int lane_id{0}; + std::set associative_lane_ids{}; + std::optional> lane_id_interval{std::nullopt}; }; struct IntersectionLanelets { public: - void update(const bool tl_arrow_solid_on, const InterpolatedPathInfo & interpolated_path_info); + void update( + const bool is_prioritized, const InterpolatedPathInfo & interpolated_path_info, + const tier4_autoware_utils::LinearRing2d & footprint); const lanelet::ConstLanelets & attention() const { - return tl_arrow_solid_on_ ? attention_non_preceding_ : attention_; + return is_prioritized_ ? attention_non_preceding_ : attention_; + } + const std::vector> & attention_stop_lines() const + { + return is_prioritized_ ? attention_non_preceding_stop_lines_ : attention_stop_lines_; } const lanelet::ConstLanelets & conflicting() const { return conflicting_; } const lanelet::ConstLanelets & adjacent() const { return adjacent_; } const lanelet::ConstLanelets & occlusion_attention() const { - return tl_arrow_solid_on_ ? attention_non_preceding_ : occlusion_attention_; + return is_prioritized_ ? attention_non_preceding_ : occlusion_attention_; } const std::vector & attention_area() const { - return tl_arrow_solid_on_ ? attention_non_preceding_area_ : attention_area_; + return is_prioritized_ ? attention_non_preceding_area_ : attention_area_; } const std::vector & conflicting_area() const { @@ -88,48 +102,64 @@ struct IntersectionLanelets { return occlusion_attention_area_; } + const std::optional & first_conflicting_lane() const + { + return first_conflicting_lane_; + } const std::optional & first_conflicting_area() const { return first_conflicting_area_; } + const std::optional & first_attention_lane() const + { + return first_attention_lane_; + } const std::optional & first_attention_area() const { return first_attention_area_; } - lanelet::ConstLanelets attention_; + lanelet::ConstLanelets attention_; // topologically merged lanelets + std::vector> + attention_stop_lines_; // the stop lines for each attention_ lanelets lanelet::ConstLanelets attention_non_preceding_; + std::vector> + attention_non_preceding_stop_lines_; // the stop lines for each attention_non_preceding_ + // lanelets lanelet::ConstLanelets conflicting_; lanelet::ConstLanelets adjacent_; - lanelet::ConstLanelets occlusion_attention_; // for occlusion detection - std::vector attention_area_; + lanelet::ConstLanelets occlusion_attention_; // topologically merged lanelets + std::vector occlusion_attention_size_; // the area() of each occlusion attention lanelets + std::vector attention_area_; // topologically merged lanelets std::vector attention_non_preceding_area_; std::vector conflicting_area_; std::vector adjacent_area_; - std::vector occlusion_attention_area_; + std::vector + occlusion_attention_area_; // topologically merged lanelets // the first area intersecting with the path // even if lane change/re-routing happened on the intersection, these areas area are supposed to // be invariant under the 'associative' lanes. - bool tl_arrow_solid_on_ = false; - std::optional first_conflicting_area_ = std::nullopt; - std::optional first_attention_area_ = std::nullopt; -}; - -struct DiscretizedLane -{ - int lane_id; - // discrete fine lines from left to right - std::vector divisions; + std::optional first_conflicting_lane_{std::nullopt}; + std::optional first_conflicting_area_{std::nullopt}; + std::optional first_attention_lane_{std::nullopt}; + std::optional first_attention_area_{std::nullopt}; + bool is_prioritized_ = false; }; struct IntersectionStopLines { // NOTE: for baselink - size_t closest_idx; - size_t stuck_stop_line; - size_t default_stop_line; - size_t occlusion_peeking_stop_line; - size_t pass_judge_line; + size_t closest_idx{0}; + // NOTE: null if path does not conflict with first_conflicting_area + std::optional stuck_stop_line{std::nullopt}; + // NOTE: null if path is over map stop_line OR its value is calculated negative + std::optional default_stop_line{std::nullopt}; + // NOTE: null if the index is calculated negative + std::optional first_attention_stop_line{std::nullopt}; + // NOTE: null if footprints do not change from outside to inside of detection area + std::optional occlusion_peeking_stop_line{std::nullopt}; + // if the value is calculated negative, its value is 0 + size_t pass_judge_line{0}; }; struct PathLanelets @@ -142,8 +172,37 @@ struct PathLanelets std::optional next = std::nullopt; // this is nullopt is the goal is inside intersection lanelet::ConstLanelets all; + lanelet::ConstLanelets + conflicting_interval_and_remaining; // the left/right-most interval of path conflicting with + // conflicting lanelets plus the next lane part of the path +}; + +struct TargetObject +{ + autoware_auto_perception_msgs::msg::PredictedObject object; + std::optional attention_lanelet{std::nullopt}; + std::optional stop_line{std::nullopt}; + std::optional dist_to_stop_line{std::nullopt}; + void calc_dist_to_stop_line(); }; +struct TargetObjects +{ + std_msgs::msg::Header header; + std::vector attention_objects; + std::vector parked_attention_objects; + std::vector intersection_area_objects; + std::vector all_attention_objects; // TODO(Mamoru Sobue): avoid copy +}; + +enum class TrafficPrioritizedLevel { + // The target lane's traffic signal is red or the ego's traffic signal has an arrow. + FULLY_PRIORITIZED = 0, + // The target lane's traffic signal is amber + PARTIALLY_PRIORITIZED, + // The target lane's traffic signal is green + NOT_PRIORITIZED +}; } // namespace behavior_velocity_planner::util #endif // UTIL_TYPE_HPP_ diff --git a/planning/behavior_velocity_no_drivable_lane_module/src/debug.cpp b/planning/behavior_velocity_no_drivable_lane_module/src/debug.cpp index a4f693dfaa883..0c6bb747a854b 100644 --- a/planning/behavior_velocity_no_drivable_lane_module/src/debug.cpp +++ b/planning/behavior_velocity_no_drivable_lane_module/src/debug.cpp @@ -15,8 +15,9 @@ #include "scene.hpp" #include -#include -#include +#include +#include +#include #include diff --git a/planning/behavior_velocity_no_drivable_lane_module/src/manager.cpp b/planning/behavior_velocity_no_drivable_lane_module/src/manager.cpp index 418f05baeaf20..f1fa4c47e951b 100644 --- a/planning/behavior_velocity_no_drivable_lane_module/src/manager.cpp +++ b/planning/behavior_velocity_no_drivable_lane_module/src/manager.cpp @@ -14,17 +14,21 @@ #include "manager.hpp" +#include +#include + #include namespace behavior_velocity_planner { +using tier4_autoware_utils::getOrDeclareParameter; NoDrivableLaneModuleManager::NoDrivableLaneModuleManager(rclcpp::Node & node) : SceneModuleManagerInterface(node, getModuleName()) { const std::string ns(getModuleName()); - planner_param_.stop_margin = node.declare_parameter(ns + ".stop_margin", 1.5); - planner_param_.print_debug_info = node.declare_parameter(ns + ".print_debug_info", false); + planner_param_.stop_margin = getOrDeclareParameter(node, ns + ".stop_margin"); + planner_param_.print_debug_info = getOrDeclareParameter(node, ns + ".print_debug_info"); } void NoDrivableLaneModuleManager::launchNewModules( diff --git a/planning/behavior_velocity_no_drivable_lane_module/src/scene.cpp b/planning/behavior_velocity_no_drivable_lane_module/src/scene.cpp index f08154cda336c..0d0e7cc6cadba 100644 --- a/planning/behavior_velocity_no_drivable_lane_module/src/scene.cpp +++ b/planning/behavior_velocity_no_drivable_lane_module/src/scene.cpp @@ -14,10 +14,11 @@ #include "scene.hpp" -#include "motion_utils/motion_utils.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" +#include "motion_utils/trajectory/trajectory.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" #include "util.hpp" +#include #include namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_no_drivable_lane_module/src/util.cpp b/planning/behavior_velocity_no_drivable_lane_module/src/util.cpp index 9b50a73a725db..4c5efad94dfc8 100644 --- a/planning/behavior_velocity_no_drivable_lane_module/src/util.cpp +++ b/planning/behavior_velocity_no_drivable_lane_module/src/util.cpp @@ -14,9 +14,13 @@ #include "util.hpp" -#include "motion_utils/motion_utils.hpp" +#include "motion_utils/trajectory/path_with_lane_id.hpp" +#include "motion_utils/trajectory/trajectory.hpp" #include +#include + +#include #include diff --git a/planning/behavior_velocity_no_stopping_area_module/config/no_stopping_area.param.yaml b/planning/behavior_velocity_no_stopping_area_module/config/no_stopping_area.param.yaml index f550188d4f2c1..c84848f8cc31d 100644 --- a/planning/behavior_velocity_no_stopping_area_module/config/no_stopping_area.param.yaml +++ b/planning/behavior_velocity_no_stopping_area_module/config/no_stopping_area.param.yaml @@ -8,4 +8,4 @@ stop_line_margin: 1.0 # [m] margin to auto-gen stop line at no stopping area detection_area_length: 200.0 # [m] used to create detection area polygon stuck_vehicle_front_margin: 6.0 # [m] obstacle stop max distance(5.0m) - enable_rtc: true # If set to true, the scene modules require approval from the rtc (request to cooperate) function. If set to false, the modules can be executed without requiring rtc approval + enable_rtc: false # If set to true, the scene modules require approval from the rtc (request to cooperate) function. If set to false, the modules can be executed without requiring rtc approval diff --git a/planning/behavior_velocity_no_stopping_area_module/src/debug.cpp b/planning/behavior_velocity_no_stopping_area_module/src/debug.cpp index 859ac4b4026f8..9e51afca2d475 100644 --- a/planning/behavior_velocity_no_stopping_area_module/src/debug.cpp +++ b/planning/behavior_velocity_no_stopping_area_module/src/debug.cpp @@ -16,8 +16,9 @@ #include #include -#include - +#include +#include +#include #ifdef ROS_DISTRO_GALACTIC #include #else diff --git a/planning/behavior_velocity_no_stopping_area_module/src/manager.cpp b/planning/behavior_velocity_no_stopping_area_module/src/manager.cpp index c7ff0f496f551..d5e9eeddb6377 100644 --- a/planning/behavior_velocity_no_stopping_area_module/src/manager.cpp +++ b/planning/behavior_velocity_no_stopping_area_module/src/manager.cpp @@ -14,7 +14,9 @@ #include "manager.hpp" +#include #include +#include #include @@ -29,23 +31,24 @@ namespace behavior_velocity_planner { using lanelet::autoware::NoStoppingArea; +using tier4_autoware_utils::getOrDeclareParameter; NoStoppingAreaModuleManager::NoStoppingAreaModuleManager(rclcpp::Node & node) : SceneModuleManagerInterfaceWithRTC( node, getModuleName(), - node.declare_parameter(std::string(getModuleName()) + ".enable_rtc")) + getOrDeclareParameter(node, std::string(getModuleName()) + ".enable_rtc")) { const std::string ns(getModuleName()); auto & pp = planner_param_; const auto & vi = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo(); - pp.state_clear_time = node.declare_parameter(ns + ".state_clear_time"); - pp.stuck_vehicle_vel_thr = node.declare_parameter(ns + ".stuck_vehicle_vel_thr"); - pp.stop_margin = node.declare_parameter(ns + ".stop_margin"); - pp.dead_line_margin = node.declare_parameter(ns + ".dead_line_margin"); - pp.stop_line_margin = node.declare_parameter(ns + ".stop_line_margin"); - pp.detection_area_length = node.declare_parameter(ns + ".detection_area_length"); + pp.state_clear_time = getOrDeclareParameter(node, ns + ".state_clear_time"); + pp.stuck_vehicle_vel_thr = getOrDeclareParameter(node, ns + ".stuck_vehicle_vel_thr"); + pp.stop_margin = getOrDeclareParameter(node, ns + ".stop_margin"); + pp.dead_line_margin = getOrDeclareParameter(node, ns + ".dead_line_margin"); + pp.stop_line_margin = getOrDeclareParameter(node, ns + ".stop_line_margin"); + pp.detection_area_length = getOrDeclareParameter(node, ns + ".detection_area_length"); pp.stuck_vehicle_front_margin = - node.declare_parameter(ns + ".stuck_vehicle_front_margin"); + getOrDeclareParameter(node, ns + ".stuck_vehicle_front_margin"); pp.path_expand_width = vi.vehicle_width_m * 0.5; } diff --git a/planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp b/planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp index 77fbb18be2b5b..ceef50094e374 100644 --- a/planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp +++ b/planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp @@ -18,10 +18,12 @@ #include #include #include -#include +#include +#include +#include +#include #include - #ifdef ROS_DISTRO_GALACTIC #include #else diff --git a/planning/behavior_velocity_occlusion_spot_module/src/debug.cpp b/planning/behavior_velocity_occlusion_spot_module/src/debug.cpp index a8e4aee2d2815..a6e13d0dfdcdc 100644 --- a/planning/behavior_velocity_occlusion_spot_module/src/debug.cpp +++ b/planning/behavior_velocity_occlusion_spot_module/src/debug.cpp @@ -17,7 +17,7 @@ #include #include -#include +#include #include #include @@ -32,6 +32,7 @@ using builtin_interfaces::msg::Time; using BasicPolygons = std::vector; using occlusion_spot_utils::PossibleCollisionInfo; 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; diff --git a/planning/behavior_velocity_occlusion_spot_module/src/manager.cpp b/planning/behavior_velocity_occlusion_spot_module/src/manager.cpp index 5b1b3d489041f..f257012c05519 100644 --- a/planning/behavior_velocity_occlusion_spot_module/src/manager.cpp +++ b/planning/behavior_velocity_occlusion_spot_module/src/manager.cpp @@ -17,6 +17,7 @@ #include "scene_occlusion_spot.hpp" #include +#include #include @@ -28,6 +29,7 @@ namespace behavior_velocity_planner { using occlusion_spot_utils::DETECTION_METHOD; using occlusion_spot_utils::PASS_JUDGE; +using tier4_autoware_utils::getOrDeclareParameter; OcclusionSpotModuleManager::OcclusionSpotModuleManager(rclcpp::Node & node) : SceneModuleManagerInterface(node, getModuleName()) @@ -36,7 +38,7 @@ OcclusionSpotModuleManager::OcclusionSpotModuleManager(rclcpp::Node & node) auto & pp = planner_param_; // for detection type { - const std::string method = node.declare_parameter(ns + ".detection_method"); + const std::string method = getOrDeclareParameter(node, ns + ".detection_method"); if (method == "occupancy_grid") { // module id 0 pp.detection_method = DETECTION_METHOD::OCCUPANCY_GRID; module_id_ = DETECTION_METHOD::OCCUPANCY_GRID; @@ -50,7 +52,7 @@ OcclusionSpotModuleManager::OcclusionSpotModuleManager(rclcpp::Node & node) } // for passable judgement { - const std::string pass_judge = node.declare_parameter(ns + ".pass_judge"); + const std::string pass_judge = getOrDeclareParameter(node, ns + ".pass_judge"); if (pass_judge == "current_velocity") { pp.pass_judge = PASS_JUDGE::CURRENT_VELOCITY; } else if (pass_judge == "smooth_velocity") { @@ -60,48 +62,52 @@ OcclusionSpotModuleManager::OcclusionSpotModuleManager(rclcpp::Node & node) "[behavior_velocity]: occlusion spot pass judge method has invalid argument"}; } } - pp.use_object_info = node.declare_parameter(ns + ".use_object_info"); - pp.use_moving_object_ray_cast = node.declare_parameter(ns + ".use_moving_object_ray_cast"); - pp.use_partition_lanelet = node.declare_parameter(ns + ".use_partition_lanelet"); - pp.pedestrian_vel = node.declare_parameter(ns + ".pedestrian_vel"); - pp.pedestrian_radius = node.declare_parameter(ns + ".pedestrian_radius"); + pp.use_object_info = getOrDeclareParameter(node, ns + ".use_object_info"); + pp.use_moving_object_ray_cast = + getOrDeclareParameter(node, ns + ".use_moving_object_ray_cast"); + pp.use_partition_lanelet = getOrDeclareParameter(node, ns + ".use_partition_lanelet"); + pp.pedestrian_vel = getOrDeclareParameter(node, ns + ".pedestrian_vel"); + pp.pedestrian_radius = getOrDeclareParameter(node, ns + ".pedestrian_radius"); // debug - pp.is_show_occlusion = node.declare_parameter(ns + ".debug.is_show_occlusion"); - pp.is_show_cv_window = node.declare_parameter(ns + ".debug.is_show_cv_window"); - pp.is_show_processing_time = node.declare_parameter(ns + ".debug.is_show_processing_time"); + pp.is_show_occlusion = getOrDeclareParameter(node, ns + ".debug.is_show_occlusion"); + pp.is_show_cv_window = getOrDeclareParameter(node, ns + ".debug.is_show_cv_window"); + pp.is_show_processing_time = + getOrDeclareParameter(node, ns + ".debug.is_show_processing_time"); // threshold pp.detection_area_offset = - node.declare_parameter(ns + ".threshold.detection_area_offset"); + getOrDeclareParameter(node, ns + ".threshold.detection_area_offset"); pp.detection_area_length = - node.declare_parameter(ns + ".threshold.detection_area_length"); - pp.stuck_vehicle_vel = node.declare_parameter(ns + ".threshold.stuck_vehicle_vel"); - pp.lateral_distance_thr = node.declare_parameter(ns + ".threshold.lateral_distance"); - pp.dist_thr = node.declare_parameter(ns + ".threshold.search_dist"); - pp.angle_thr = node.declare_parameter(ns + ".threshold.search_angle"); + getOrDeclareParameter(node, ns + ".threshold.detection_area_length"); + pp.stuck_vehicle_vel = getOrDeclareParameter(node, ns + ".threshold.stuck_vehicle_vel"); + pp.lateral_distance_thr = getOrDeclareParameter(node, ns + ".threshold.lateral_distance"); + pp.dist_thr = getOrDeclareParameter(node, ns + ".threshold.search_dist"); + pp.angle_thr = getOrDeclareParameter(node, ns + ".threshold.search_angle"); // ego additional velocity config - pp.v.safety_ratio = node.declare_parameter(ns + ".motion.safety_ratio"); - pp.v.safe_margin = node.declare_parameter(ns + ".motion.safe_margin"); - pp.v.max_slow_down_jerk = node.declare_parameter(ns + ".motion.max_slow_down_jerk"); - pp.v.max_slow_down_accel = node.declare_parameter(ns + ".motion.max_slow_down_accel"); - pp.v.non_effective_jerk = node.declare_parameter(ns + ".motion.non_effective_jerk"); + pp.v.safety_ratio = getOrDeclareParameter(node, ns + ".motion.safety_ratio"); + pp.v.safe_margin = getOrDeclareParameter(node, ns + ".motion.safe_margin"); + pp.v.max_slow_down_jerk = getOrDeclareParameter(node, ns + ".motion.max_slow_down_jerk"); + pp.v.max_slow_down_accel = + getOrDeclareParameter(node, ns + ".motion.max_slow_down_accel"); + pp.v.non_effective_jerk = getOrDeclareParameter(node, ns + ".motion.non_effective_jerk"); pp.v.non_effective_accel = - node.declare_parameter(ns + ".motion.non_effective_acceleration"); - pp.v.min_allowed_velocity = node.declare_parameter(ns + ".motion.min_allowed_velocity"); + getOrDeclareParameter(node, ns + ".motion.non_effective_acceleration"); + pp.v.min_allowed_velocity = + getOrDeclareParameter(node, ns + ".motion.min_allowed_velocity"); // detection_area param pp.detection_area.min_occlusion_spot_size = - node.declare_parameter(ns + ".detection_area.min_occlusion_spot_size"); + getOrDeclareParameter(node, ns + ".detection_area.min_occlusion_spot_size"); pp.detection_area.min_longitudinal_offset = - node.declare_parameter(ns + ".detection_area.min_longitudinal_offset"); + getOrDeclareParameter(node, ns + ".detection_area.min_longitudinal_offset"); pp.detection_area.max_lateral_distance = - node.declare_parameter(ns + ".detection_area.max_lateral_distance"); + getOrDeclareParameter(node, ns + ".detection_area.max_lateral_distance"); pp.detection_area.slice_length = - node.declare_parameter(ns + ".detection_area.slice_length"); + getOrDeclareParameter(node, ns + ".detection_area.slice_length"); // occupancy grid param - pp.grid.free_space_max = node.declare_parameter(ns + ".grid.free_space_max"); - pp.grid.occupied_min = node.declare_parameter(ns + ".grid.occupied_min"); + pp.grid.free_space_max = getOrDeclareParameter(node, ns + ".grid.free_space_max"); + pp.grid.occupied_min = getOrDeclareParameter(node, ns + ".grid.occupied_min"); const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo(); pp.baselink_to_front = vehicle_info.max_longitudinal_offset_m; diff --git a/planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp b/planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp index 21a8cce93a1be..fa8be96c02859 100644 --- a/planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp +++ b/planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp @@ -20,6 +20,7 @@ #include #include #include +#include #include #include diff --git a/planning/behavior_velocity_occlusion_spot_module/test/src/test_occlusion_spot_utils.cpp b/planning/behavior_velocity_occlusion_spot_module/test/src/test_occlusion_spot_utils.cpp index ef2e776112766..85ae85f1895d9 100644 --- a/planning/behavior_velocity_occlusion_spot_module/test/src/test_occlusion_spot_utils.cpp +++ b/planning/behavior_velocity_occlusion_spot_module/test/src/test_occlusion_spot_utils.cpp @@ -14,7 +14,6 @@ #include "gtest/gtest.h" #include "occlusion_spot_utils.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" #include "utils.hpp" #include diff --git a/planning/behavior_velocity_out_of_lane_module/CMakeLists.txt b/planning/behavior_velocity_out_of_lane_module/CMakeLists.txt index 633d5fdd613af..8ce1a334c4b35 100644 --- a/planning/behavior_velocity_out_of_lane_module/CMakeLists.txt +++ b/planning/behavior_velocity_out_of_lane_module/CMakeLists.txt @@ -6,13 +6,7 @@ autoware_package() pluginlib_export_plugin_description_file(behavior_velocity_planner plugins.xml) ament_auto_add_library(${PROJECT_NAME} SHARED - src/debug.cpp - src/decisions.cpp - src/footprint.cpp - src/lanelets_selection.cpp - src/manager.cpp - src/overlapping_range.cpp - src/scene_out_of_lane.cpp + DIRECTORY src ) ament_auto_package(INSTALL_TO_SHARE config) diff --git a/planning/behavior_velocity_out_of_lane_module/README.md b/planning/behavior_velocity_out_of_lane_module/README.md index da46055460a99..99396eb0edf22 100644 --- a/planning/behavior_velocity_out_of_lane_module/README.md +++ b/planning/behavior_velocity_out_of_lane_module/README.md @@ -140,23 +140,22 @@ Moreover, parameter `action.distance_buffer` adds an extra distance between the | Parameter /objects | Type | Description | | ------------------------------- | ------ | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | -| `minimum_velocity` | double | [m/s] consider objects with an estimated time to collision bellow this value while on the overlap | -| `use_predicted_paths` | bool | [-] if true, use the predicted paths to estimate future positions; if false, assume the object moves at constant velocity along _all_ lanelets it currently is located in | +| `minimum_velocity` | double | [m/s] ignore objects with a velocity lower than this value | | `predicted_path_min_confidence` | double | [-] minimum confidence required for a predicted path to be considered | +| `use_predicted_paths` | bool | [-] if true, use the predicted paths to estimate future positions; if false, assume the object moves at constant velocity along _all_ lanelets it currently is located in | | Parameter /overlap | Type | Description | | ------------------ | ------ | ---------------------------------------------------------------------------------------------------- | | `minimum_distance` | double | [m] minimum distance inside a lanelet for an overlap to be considered | | `extra_length` | double | [m] extra arc length to add to the front and back of an overlap (used to calculate enter/exit times) | -| Parameter /action | Type | Description | -| ----------------------------- | ------ | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------ | -| `skip_if_over_max_decel` | bool | [-] if true, do not take an action that would cause more deceleration than the maximum allowed | -| `strict` | bool | [-] if true, when a decision is taken to avoid entering a lane, the stop point will make sure no lane at all is entered by ego; if false, ego stops just before entering a lane but may then be overlapping another lane | -| `distance_buffer` | double | [m] buffer distance to try to keep between the ego footprint and lane | -| `slowdown.distance_threshold` | double | [m] insert a slow down when closer than this distance from an overlap | -| `slowdown.velocity` | double | [m] slow down velocity | -| `stop.distance_threshold` | double | [m] insert a stop when closer than this distance from an overlap | +| Parameter /action | Type | Description | +| ----------------------------- | ------ | ---------------------------------------------------------------------------------------------- | +| `skip_if_over_max_decel` | bool | [-] if true, do not take an action that would cause more deceleration than the maximum allowed | +| `distance_buffer` | double | [m] buffer distance to try to keep between the ego footprint and lane | +| `slowdown.distance_threshold` | double | [m] insert a slow down when closer than this distance from an overlap | +| `slowdown.velocity` | double | [m] slow down velocity | +| `stop.distance_threshold` | double | [m] insert a stop when closer than this distance from an overlap | | Parameter /ego | Type | Description | | -------------------- | ------ | ---------------------------------------------------- | diff --git a/planning/behavior_velocity_out_of_lane_module/config/out_of_lane.param.yaml b/planning/behavior_velocity_out_of_lane_module/config/out_of_lane.param.yaml index dd4c1c610261c..c74c5b3d87c1d 100644 --- a/planning/behavior_velocity_out_of_lane_module/config/out_of_lane.param.yaml +++ b/planning/behavior_velocity_out_of_lane_module/config/out_of_lane.param.yaml @@ -24,8 +24,7 @@ action: # action to insert in the path if an object causes a conflict at an overlap skip_if_over_max_decel: true # if true, do not take an action that would cause more deceleration than the maximum allowed - strict: true # if true, when a decision is taken to avoid entering a lane, the stop point will make sure no lane at all is entered by ego - # if false, ego stops just before entering a lane but may then be overlapping another lane. + precision: 0.1 # [m] precision when inserting a stop pose in the path distance_buffer: 1.5 # [m] buffer distance to try to keep between the ego footprint and lane slowdown: distance_threshold: 30.0 # [m] insert a slowdown when closer than this distance from an overlap @@ -34,6 +33,7 @@ distance_threshold: 15.0 # [m] insert a stop when closer than this distance from an overlap ego: + min_assumed_velocity: 2.0 # [m/s] minimum velocity used to calculate the enter and exit times of ego extra_front_offset: 0.0 # [m] extra front distance extra_rear_offset: 0.0 # [m] extra rear distance extra_right_offset: 0.0 # [m] extra right distance diff --git a/planning/behavior_velocity_out_of_lane_module/src/calculate_slowdown_points.hpp b/planning/behavior_velocity_out_of_lane_module/src/calculate_slowdown_points.hpp new file mode 100644 index 0000000000000..5d22f01222919 --- /dev/null +++ b/planning/behavior_velocity_out_of_lane_module/src/calculate_slowdown_points.hpp @@ -0,0 +1,87 @@ +// Copyright 2023 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 CALCULATE_SLOWDOWN_POINTS_HPP_ +#define CALCULATE_SLOWDOWN_POINTS_HPP_ + +#include "footprint.hpp" +#include "types.hpp" + +#include +#include + +#include + +#include +#include + +namespace behavior_velocity_planner::out_of_lane +{ + +bool can_decelerate( + const EgoData & ego_data, const PathPointWithLaneId & point, const double target_vel) +{ + if (ego_data.velocity < 0.5) return true; + const auto dist_ahead_of_ego = motion_utils::calcSignedArcLength( + ego_data.path.points, ego_data.pose.position, point.point.pose.position); + const auto acc_to_target_vel = + (ego_data.velocity * ego_data.velocity - target_vel * target_vel) / (2 * dist_ahead_of_ego); + return acc_to_target_vel < std::abs(ego_data.max_decel); +} + +std::optional calculate_last_in_lane_pose( + const EgoData & ego_data, const Slowdown & decision, const Polygon2d & footprint, + const PlannerParam & params) +{ + const auto from_arc_length = + motion_utils::calcSignedArcLength(ego_data.path.points, 0, ego_data.first_path_idx); + const auto to_arc_length = + motion_utils::calcSignedArcLength(ego_data.path.points, 0, decision.target_path_idx); + PathPointWithLaneId interpolated_point; + for (auto l = to_arc_length - params.precision; l > from_arc_length; l -= params.precision) { + // TODO(Maxime): binary search + interpolated_point.point.pose = motion_utils::calcInterpolatedPose(ego_data.path.points, l); + const auto respect_decel_limit = + !params.skip_if_over_max_decel || + can_decelerate(ego_data, interpolated_point, decision.velocity); + if ( + respect_decel_limit && !boost::geometry::overlaps( + project_to_pose(footprint, interpolated_point.point.pose), + decision.lane_to_avoid.polygon2d().basicPolygon())) + return interpolated_point; + } + return std::nullopt; +} + +/// @brief calculate the slowdown point to insert in the path +/// @param ego_data ego data (path, velocity, etc) +/// @param decisions decision (before which point to stop, what lane to avoid entering, etc) +/// @param params parameters +/// @return optional slowdown point to insert in the path +std::optional calculate_slowdown_point( + const EgoData & ego_data, const std::vector & decisions, PlannerParam params) +{ + params.extra_front_offset += params.dist_buffer; + const auto base_footprint = make_base_footprint(params); + + // search for the first slowdown decision for which a stop point can be inserted + for (const auto & decision : decisions) { + const auto last_in_lane_pose = + calculate_last_in_lane_pose(ego_data, decision, base_footprint, params); + if (last_in_lane_pose) return SlowdownToInsert{decision, *last_in_lane_pose}; + } + return std::nullopt; +} +} // namespace behavior_velocity_planner::out_of_lane +#endif // CALCULATE_SLOWDOWN_POINTS_HPP_ diff --git a/planning/behavior_velocity_out_of_lane_module/src/debug.cpp b/planning/behavior_velocity_out_of_lane_module/src/debug.cpp index 475e341d76528..0574a5226dc4a 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/debug.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/debug.cpp @@ -14,6 +14,8 @@ #include "debug.hpp" +#include + #include namespace behavior_velocity_planner::out_of_lane::debug diff --git a/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp b/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp index 163c8632e40c7..78d9b73c86a17 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp @@ -15,27 +15,35 @@ #include "decisions.hpp" #include +#include +#include +#include + +#include + +#include +#include +#include #include #include #include #include #include - namespace behavior_velocity_planner::out_of_lane { double distance_along_path(const EgoData & ego_data, const size_t target_idx) { return motion_utils::calcSignedArcLength( - ego_data.path->points, ego_data.pose.position, ego_data.first_path_idx + target_idx); + ego_data.path.points, ego_data.pose.position, ego_data.first_path_idx + target_idx); } -double time_along_path(const EgoData & ego_data, const size_t target_idx) +double time_along_path(const EgoData & ego_data, const size_t target_idx, const double min_velocity) { const auto dist = distance_along_path(ego_data, target_idx); const auto v = std::max( - ego_data.velocity, - ego_data.path->points[ego_data.first_path_idx + target_idx].point.longitudinal_velocity_mps * + std::max(ego_data.velocity, min_velocity), + ego_data.path.points[ego_data.first_path_idx + target_idx].point.longitudinal_velocity_mps * 0.5); return dist / v; } @@ -55,8 +63,7 @@ bool object_is_incoming( std::optional> object_time_to_range( const autoware_auto_perception_msgs::msg::PredictedObject & object, const OverlapRange & range, - const std::shared_ptr route_handler, const double min_confidence, - const rclcpp::Logger & logger) + const std::shared_ptr route_handler, const rclcpp::Logger & logger) { // skip the dynamic object if it is not in a lane preceding the overlapped lane // lane changes are intentionally not considered @@ -65,24 +72,24 @@ std::optional> object_time_to_range( object.kinematics.initial_pose_with_covariance.pose.position.y); if (!object_is_incoming(object_point, route_handler, range.lane)) return {}; - const auto max_deviation = object.shape.dimensions.y * 2.0; + const auto max_deviation = object.shape.dimensions.y + range.inside_distance; auto worst_enter_time = std::optional(); auto worst_exit_time = std::optional(); for (const auto & predicted_path : object.kinematics.predicted_paths) { - if (predicted_path.confidence < min_confidence) continue; + const auto unique_path_points = motion_utils::removeOverlapPoints(predicted_path.path); const auto time_step = rclcpp::Duration(predicted_path.time_step).seconds(); const auto enter_point = geometry_msgs::msg::Point().set__x(range.entering_point.x()).set__y(range.entering_point.y()); const auto enter_segment_idx = - motion_utils::findNearestSegmentIndex(predicted_path.path, enter_point); + motion_utils::findNearestSegmentIndex(unique_path_points, enter_point); const auto enter_offset = motion_utils::calcLongitudinalOffsetToSegment( - predicted_path.path, enter_segment_idx, enter_point); - const auto enter_lat_dist = std::abs( - motion_utils::calcLateralOffset(predicted_path.path, enter_point, enter_segment_idx)); + unique_path_points, enter_segment_idx, enter_point); + const auto enter_lat_dist = + std::abs(motion_utils::calcLateralOffset(unique_path_points, enter_point, enter_segment_idx)); const auto enter_segment_length = tier4_autoware_utils::calcDistance2d( - predicted_path.path[enter_segment_idx], predicted_path.path[enter_segment_idx + 1]); + unique_path_points[enter_segment_idx], unique_path_points[enter_segment_idx + 1]); const auto enter_offset_ratio = enter_offset / enter_segment_length; const auto enter_time = static_cast(enter_segment_idx) * time_step + enter_offset_ratio * time_step; @@ -90,13 +97,13 @@ std::optional> object_time_to_range( const auto exit_point = geometry_msgs::msg::Point().set__x(range.exiting_point.x()).set__y(range.exiting_point.y()); const auto exit_segment_idx = - motion_utils::findNearestSegmentIndex(predicted_path.path, exit_point); + motion_utils::findNearestSegmentIndex(unique_path_points, exit_point); const auto exit_offset = motion_utils::calcLongitudinalOffsetToSegment( - predicted_path.path, exit_segment_idx, exit_point); + unique_path_points, exit_segment_idx, exit_point); const auto exit_lat_dist = - std::abs(motion_utils::calcLateralOffset(predicted_path.path, exit_point, exit_segment_idx)); + std::abs(motion_utils::calcLateralOffset(unique_path_points, exit_point, exit_segment_idx)); const auto exit_segment_length = tier4_autoware_utils::calcDistance2d( - predicted_path.path[exit_segment_idx], predicted_path.path[exit_segment_idx + 1]); + unique_path_points[exit_segment_idx], unique_path_points[exit_segment_idx + 1]); const auto exit_offset_ratio = exit_offset / static_cast(exit_segment_length); const auto exit_time = static_cast(exit_segment_idx) * time_step + exit_offset_ratio * time_step; @@ -119,11 +126,9 @@ std::optional> object_time_to_range( const auto same_driving_direction_as_ego = enter_time < exit_time; if (same_driving_direction_as_ego) { - RCLCPP_DEBUG(logger, " / SAME DIR \\\n"); worst_enter_time = worst_enter_time ? std::min(*worst_enter_time, enter_time) : enter_time; worst_exit_time = worst_exit_time ? std::max(*worst_exit_time, exit_time) : exit_time; } else { - RCLCPP_DEBUG(logger, " / OPPOSITE DIR \\\n"); worst_enter_time = worst_enter_time ? std::max(*worst_enter_time, enter_time) : enter_time; worst_exit_time = worst_exit_time ? std::min(*worst_exit_time, exit_time) : exit_time; } @@ -205,8 +210,11 @@ std::optional> object_time_to_range( bool threshold_condition(const RangeTimes & range_times, const PlannerParam & params) { - return std::min(range_times.object.enter_time, range_times.object.exit_time) < - params.time_threshold; + const auto enter_within_threshold = + range_times.object.enter_time > 0.0 && range_times.object.enter_time < params.time_threshold; + const auto exit_within_threshold = + range_times.object.exit_time > 0.0 && range_times.object.exit_time < params.time_threshold; + return enter_within_threshold || exit_within_threshold; } bool intervals_condition( @@ -281,8 +289,10 @@ bool should_not_enter( const rclcpp::Logger & logger) { RangeTimes range_times{}; - range_times.ego.enter_time = time_along_path(inputs.ego_data, range.entering_path_idx); - range_times.ego.exit_time = time_along_path(inputs.ego_data, range.exiting_path_idx); + range_times.ego.enter_time = + time_along_path(inputs.ego_data, range.entering_path_idx, params.ego_min_velocity); + range_times.ego.exit_time = + time_along_path(inputs.ego_data, range.exiting_path_idx, params.ego_min_velocity); RCLCPP_DEBUG( logger, "\t[%lu -> %lu] %ld | ego enters at %2.2f, exits at %2.2f\n", range.entering_path_idx, range.exiting_path_idx, range.lane.id(), range_times.ego.enter_time, range_times.ego.exit_time); @@ -298,8 +308,7 @@ bool should_not_enter( // skip objects that are already on the interval const auto enter_exit_time = params.objects_use_predicted_paths - ? object_time_to_range( - object, range, inputs.route_handler, params.objects_min_confidence, logger) + ? object_time_to_range(object, range, inputs.route_handler, logger) : object_time_to_range(object, range, inputs, logger); if (!enter_exit_time) { RCLCPP_DEBUG(logger, " SKIP (no enter/exit times found)\n"); @@ -349,11 +358,10 @@ std::optional calculate_decision( { std::optional decision; if (should_not_enter(range, inputs, params, logger)) { - const auto stop_before_range = params.strict ? find_most_preceding_range(range, inputs) : range; decision.emplace(); - decision->target_path_idx = inputs.ego_data.first_path_idx + - stop_before_range.entering_path_idx; // add offset from curr pose - decision->lane_to_avoid = stop_before_range.lane; + decision->target_path_idx = + inputs.ego_data.first_path_idx + range.entering_path_idx; // add offset from curr pose + decision->lane_to_avoid = range.lane; const auto ego_dist_to_range = distance_along_path(inputs.ego_data, range.entering_path_idx); set_decision_velocity(decision, ego_dist_to_range, params); } diff --git a/planning/behavior_velocity_out_of_lane_module/src/decisions.hpp b/planning/behavior_velocity_out_of_lane_module/src/decisions.hpp index bf6bf81e506cf..2898595c2e74c 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/decisions.hpp +++ b/planning/behavior_velocity_out_of_lane_module/src/decisions.hpp @@ -25,6 +25,7 @@ #include #include +#include #include #include @@ -48,6 +49,7 @@ double distance_along_path(const EgoData & ego_data, const size_t target_idx); /// @brief estimate the time when ego will reach some target path index /// @param [in] ego_data data related to the ego vehicle /// @param [in] target_idx target ego path index +/// @param [in] min_velocity minimum ego velocity used to estimate the time /// @return time taken by ego to reach the target [s] double time_along_path(const EgoData & ego_data, const size_t target_idx); /// @brief use an object's predicted paths to estimate the times it will reach the enter and exit @@ -57,14 +59,12 @@ double time_along_path(const EgoData & ego_data, const size_t target_idx); /// @param [in] object dynamic object /// @param [in] range overlapping range /// @param [in] route_handler route handler used to estimate the path of the dynamic object -/// @param [in] min_confidence minimum confidence to consider a predicted path /// @param [in] logger ros logger /// @return an optional pair (time at enter [s], time at exit [s]). If the dynamic object drives in /// the opposite direction, time at enter > time at exit std::optional> object_time_to_range( const autoware_auto_perception_msgs::msg::PredictedObject & object, const OverlapRange & range, - const std::shared_ptr route_handler, const double min_confidence, - const rclcpp::Logger & logger); + const std::shared_ptr route_handler, const rclcpp::Logger & logger); /// @brief use the lanelet map to estimate the times when an object will reach the enter and exit /// points of an overlapping range /// @param [in] object dynamic object diff --git a/planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.hpp b/planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.hpp new file mode 100644 index 0000000000000..13bad0d047922 --- /dev/null +++ b/planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.hpp @@ -0,0 +1,70 @@ +// Copyright 2023 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 FILTER_PREDICTED_OBJECTS_HPP_ +#define FILTER_PREDICTED_OBJECTS_HPP_ + +#include "types.hpp" + +#include + +#include + +namespace behavior_velocity_planner::out_of_lane +{ +/// @brief filter predicted objects and their predicted paths +/// @param [in] objects predicted objects to filter +/// @param [in] ego_data ego data +/// @param [in] params parameters +/// @return filtered predicted objects +autoware_auto_perception_msgs::msg::PredictedObjects filter_predicted_objects( + const autoware_auto_perception_msgs::msg::PredictedObjects & objects, const EgoData & ego_data, + const PlannerParam & params) +{ + autoware_auto_perception_msgs::msg::PredictedObjects filtered_objects; + filtered_objects.header = objects.header; + for (const auto & object : objects.objects) { + const auto is_pedestrian = + std::find_if(object.classification.begin(), object.classification.end(), [](const auto & c) { + return c.label == autoware_auto_perception_msgs::msg::ObjectClassification::PEDESTRIAN; + }) != object.classification.end(); + if (is_pedestrian) continue; + + auto filtered_object = object; + const auto is_invalid_predicted_path = [&](const auto & predicted_path) { + const auto is_low_confidence = predicted_path.confidence < params.objects_min_confidence; + const auto lat_offset_to_current_ego = + std::abs(motion_utils::calcLateralOffset(predicted_path.path, ego_data.pose.position)); + const auto is_crossing_ego = + lat_offset_to_current_ego <= + object.shape.dimensions.y / 2.0 + std::max( + params.left_offset + params.extra_left_offset, + params.right_offset + params.extra_right_offset); + return is_low_confidence || is_crossing_ego; + }; + if (params.objects_use_predicted_paths) { + auto & predicted_paths = filtered_object.kinematics.predicted_paths; + const auto new_end = + std::remove_if(predicted_paths.begin(), predicted_paths.end(), is_invalid_predicted_path); + predicted_paths.erase(new_end, predicted_paths.end()); + } + if (!params.objects_use_predicted_paths || !filtered_object.kinematics.predicted_paths.empty()) + filtered_objects.objects.push_back(filtered_object); + } + return filtered_objects; +} + +} // namespace behavior_velocity_planner::out_of_lane + +#endif // FILTER_PREDICTED_OBJECTS_HPP_ diff --git a/planning/behavior_velocity_out_of_lane_module/src/footprint.cpp b/planning/behavior_velocity_out_of_lane_module/src/footprint.cpp index 929fbd944af9f..31cc035703a7b 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/footprint.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/footprint.cpp @@ -14,7 +14,6 @@ #include "footprint.hpp" -#include #include #include @@ -58,9 +57,9 @@ std::vector calculate_path_footprints( { const auto base_footprint = make_base_footprint(params); std::vector path_footprints; - path_footprints.reserve(ego_data.path->points.size()); - for (auto i = ego_data.first_path_idx; i < ego_data.path->points.size(); ++i) { - const auto & path_pose = ego_data.path->points[i].point.pose; + path_footprints.reserve(ego_data.path.points.size()); + for (auto i = ego_data.first_path_idx; i < ego_data.path.points.size(); ++i) { + const auto & path_pose = ego_data.path.points[i].point.pose; const auto angle = tf2::getYaw(path_pose.orientation); const auto rotated_footprint = tier4_autoware_utils::rotatePolygon(base_footprint, angle); lanelet::BasicPolygon2d footprint; diff --git a/planning/behavior_velocity_out_of_lane_module/src/lanelets_selection.cpp b/planning/behavior_velocity_out_of_lane_module/src/lanelets_selection.cpp index a0e2b86caa623..c5dedf23785ab 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/lanelets_selection.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/lanelets_selection.cpp @@ -17,8 +17,10 @@ #include #include +#include #include +#include namespace behavior_velocity_planner::out_of_lane { @@ -54,16 +56,23 @@ lanelet::ConstLanelets calculate_other_lanelets( const auto lanelets_within_range = lanelet::geometry::findWithin2d( route_handler.getLaneletMapPtr()->laneletLayer, ego_point, std::max(params.slow_dist_threshold, params.stop_dist_threshold)); + const auto is_overlapped_by_path_lanelets = [&](const auto & l) { + for (const auto & path_ll : path_lanelets) { + if ( + boost::geometry::overlaps( + path_ll.polygon2d().basicPolygon(), l.polygon2d().basicPolygon()) || + boost::geometry::within(path_ll.polygon2d().basicPolygon(), l.polygon2d().basicPolygon()) || + boost::geometry::within(l.polygon2d().basicPolygon(), path_ll.polygon2d().basicPolygon())) { + return true; + } + } + return false; + }; for (const auto & ll : lanelets_within_range) { + if (std::string(ll.second.attributeOr(lanelet::AttributeName::Subtype, "none")) != "road") + continue; const auto is_path_lanelet = contains_lanelet(path_lanelets, ll.second.id()); const auto is_ignored_lanelet = contains_lanelet(ignored_lanelets, ll.second.id()); - const auto is_overlapped_by_path_lanelets = [&](const auto & l) { - for (const auto & path_ll : path_lanelets) - if (boost::geometry::overlaps( - path_ll.polygon2d().basicPolygon(), l.polygon2d().basicPolygon())) - return true; - return false; - }; if (!is_path_lanelet && !is_ignored_lanelet && !is_overlapped_by_path_lanelets(ll.second)) other_lanelets.push_back(ll.second); } diff --git a/planning/behavior_velocity_out_of_lane_module/src/manager.cpp b/planning/behavior_velocity_out_of_lane_module/src/manager.cpp index 22fb29110466a..2422c6fe84e56 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/manager.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/manager.cpp @@ -17,6 +17,7 @@ #include "scene_out_of_lane.hpp" #include +#include #include @@ -26,42 +27,48 @@ namespace behavior_velocity_planner { +using tier4_autoware_utils::getOrDeclareParameter; + OutOfLaneModuleManager::OutOfLaneModuleManager(rclcpp::Node & node) : SceneModuleManagerInterface(node, getModuleName()), module_id_(0UL) { const std::string ns(getModuleName()); auto & pp = planner_param_; - pp.mode = node.declare_parameter(ns + ".mode"); + pp.mode = getOrDeclareParameter(node, ns + ".mode"); pp.skip_if_already_overlapping = - node.declare_parameter(ns + ".skip_if_already_overlapping"); + getOrDeclareParameter(node, ns + ".skip_if_already_overlapping"); - pp.time_threshold = node.declare_parameter(ns + ".threshold.time_threshold"); - pp.intervals_ego_buffer = node.declare_parameter(ns + ".intervals.ego_time_buffer"); - pp.intervals_obj_buffer = node.declare_parameter(ns + ".intervals.objects_time_buffer"); - pp.ttc_threshold = node.declare_parameter(ns + ".ttc.threshold"); + pp.time_threshold = getOrDeclareParameter(node, ns + ".threshold.time_threshold"); + pp.intervals_ego_buffer = getOrDeclareParameter(node, ns + ".intervals.ego_time_buffer"); + pp.intervals_obj_buffer = + getOrDeclareParameter(node, ns + ".intervals.objects_time_buffer"); + pp.ttc_threshold = getOrDeclareParameter(node, ns + ".ttc.threshold"); - pp.objects_min_vel = node.declare_parameter(ns + ".objects.minimum_velocity"); + pp.objects_min_vel = getOrDeclareParameter(node, ns + ".objects.minimum_velocity"); pp.objects_use_predicted_paths = - node.declare_parameter(ns + ".objects.use_predicted_paths"); + getOrDeclareParameter(node, ns + ".objects.use_predicted_paths"); pp.objects_min_confidence = - node.declare_parameter(ns + ".objects.predicted_path_min_confidence"); + getOrDeclareParameter(node, ns + ".objects.predicted_path_min_confidence"); - pp.overlap_min_dist = node.declare_parameter(ns + ".overlap.minimum_distance"); - pp.overlap_extra_length = node.declare_parameter(ns + ".overlap.extra_length"); + pp.overlap_min_dist = getOrDeclareParameter(node, ns + ".overlap.minimum_distance"); + pp.overlap_extra_length = getOrDeclareParameter(node, ns + ".overlap.extra_length"); - pp.skip_if_over_max_decel = node.declare_parameter(ns + ".action.skip_if_over_max_decel"); - pp.strict = node.declare_parameter(ns + ".action.strict"); - pp.dist_buffer = node.declare_parameter(ns + ".action.distance_buffer"); - pp.slow_velocity = node.declare_parameter(ns + ".action.slowdown.velocity"); + pp.skip_if_over_max_decel = + getOrDeclareParameter(node, ns + ".action.skip_if_over_max_decel"); + pp.precision = getOrDeclareParameter(node, ns + ".action.precision"); + pp.dist_buffer = getOrDeclareParameter(node, ns + ".action.distance_buffer"); + pp.slow_velocity = getOrDeclareParameter(node, ns + ".action.slowdown.velocity"); pp.slow_dist_threshold = - node.declare_parameter(ns + ".action.slowdown.distance_threshold"); - pp.stop_dist_threshold = node.declare_parameter(ns + ".action.stop.distance_threshold"); + getOrDeclareParameter(node, ns + ".action.slowdown.distance_threshold"); + pp.stop_dist_threshold = + getOrDeclareParameter(node, ns + ".action.stop.distance_threshold"); - pp.extra_front_offset = node.declare_parameter(ns + ".ego.extra_front_offset"); - pp.extra_rear_offset = node.declare_parameter(ns + ".ego.extra_rear_offset"); - pp.extra_left_offset = node.declare_parameter(ns + ".ego.extra_left_offset"); - pp.extra_right_offset = node.declare_parameter(ns + ".ego.extra_right_offset"); + pp.ego_min_velocity = getOrDeclareParameter(node, ns + ".ego.min_assumed_velocity"); + pp.extra_front_offset = getOrDeclareParameter(node, ns + ".ego.extra_front_offset"); + pp.extra_rear_offset = getOrDeclareParameter(node, ns + ".ego.extra_rear_offset"); + pp.extra_left_offset = getOrDeclareParameter(node, ns + ".ego.extra_left_offset"); + pp.extra_right_offset = getOrDeclareParameter(node, ns + ".ego.extra_right_offset"); const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo(); pp.front_offset = vehicle_info.max_longitudinal_offset_m; pp.rear_offset = vehicle_info.min_longitudinal_offset_m; diff --git a/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.cpp b/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.cpp index 5922a5b9b4488..1487d9e63c8b0 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.cpp @@ -14,8 +14,10 @@ #include "scene_out_of_lane.hpp" +#include "calculate_slowdown_points.hpp" #include "debug.hpp" #include "decisions.hpp" +#include "filter_predicted_objects.hpp" #include "footprint.hpp" #include "lanelets_selection.hpp" #include "overlapping_range.hpp" @@ -25,8 +27,13 @@ #include #include #include +#include +#include +#include #include +#include + #include #include @@ -58,9 +65,9 @@ bool OutOfLaneModule::modifyPathVelocity( stopwatch.tic(); EgoData ego_data; ego_data.pose = planner_data_->current_odometry->pose; - ego_data.path = path; + ego_data.path.points = path->points; ego_data.first_path_idx = - motion_utils::findNearestSegmentIndex(path->points, ego_data.pose.position); + motion_utils::findNearestSegmentIndex(ego_data.path.points, ego_data.pose.position); ego_data.velocity = planner_data_->current_velocity->twist.linear.x; ego_data.max_decel = -planner_data_->max_stop_acceleration_threshold; stopwatch.tic("calculate_path_footprints"); @@ -69,7 +76,7 @@ bool OutOfLaneModule::modifyPathVelocity( const auto calculate_path_footprints_us = stopwatch.toc("calculate_path_footprints"); // Calculate lanelets to ignore and consider const auto path_lanelets = planning_utils::getLaneletsOnPath( - *path, planner_data_->route_handler_->getLaneletMapPtr(), + ego_data.path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_odometry->pose); const auto ignored_lanelets = calculate_ignored_lanelets(ego_data, path_lanelets, *planner_data_->route_handler_, params_); @@ -103,28 +110,33 @@ bool OutOfLaneModule::modifyPathVelocity( DecisionInputs inputs; inputs.ranges = ranges; inputs.ego_data = ego_data; - inputs.objects = *planner_data_->predicted_objects; + inputs.objects = filter_predicted_objects(*planner_data_->predicted_objects, ego_data, params_); inputs.route_handler = planner_data_->route_handler_; inputs.lanelets = other_lanelets; auto decisions = calculate_decisions(inputs, params_, logger_); const auto calculate_decisions_us = stopwatch.toc("calculate_decisions"); stopwatch.tic("calc_slowdown_points"); - const auto points_to_insert = calculate_slowdown_points(ego_data, decisions, params_); - debug_data_.slowdowns = points_to_insert; + const auto point_to_insert = calculate_slowdown_point(ego_data, decisions, params_); const auto calc_slowdown_points_us = stopwatch.toc("calc_slowdown_points"); stopwatch.tic("insert_slowdown_points"); - for (const auto & point : points_to_insert) { - auto path_idx = point.slowdown.target_path_idx; - planning_utils::insertVelocity(*ego_data.path, point.point, point.slowdown.velocity, path_idx); - if (point.slowdown.velocity == 0.0) { + debug_data_.slowdowns.clear(); + if (point_to_insert) { + debug_data_.slowdowns = {*point_to_insert}; + auto path_idx = motion_utils::findNearestSegmentIndex( + path->points, point_to_insert->point.point.pose.position) + + 1; + planning_utils::insertVelocity( + *path, point_to_insert->point, point_to_insert->slowdown.velocity, path_idx, + params_.precision); + if (point_to_insert->slowdown.velocity == 0.0) { tier4_planning_msgs::msg::StopFactor stop_factor; - stop_factor.stop_pose = point.point.point.pose; + stop_factor.stop_pose = point_to_insert->point.point.pose; stop_factor.dist_to_stop_pose = motion_utils::calcSignedArcLength( - ego_data.path->points, ego_data.pose.position, point.point.point.pose.position); + ego_data.path.points, ego_data.pose.position, point_to_insert->point.point.pose.position); planning_utils::appendStopReason(stop_factor, stop_reason); } velocity_factor_.set( - path->points, planner_data_->current_odometry->pose, point.point.point.pose, + path->points, planner_data_->current_odometry->pose, point_to_insert->point.point.pose, VelocityFactor::UNKNOWN); } const auto insert_slowdown_points_us = stopwatch.toc("insert_slowdown_points"); @@ -177,62 +189,4 @@ motion_utils::VirtualWalls OutOfLaneModule::createVirtualWalls() return virtual_walls; } -std::vector calculate_slowdown_points( - const EgoData & ego_data, const std::vector & decisions, PlannerParam params) -{ - std::vector to_insert; - params.extra_front_offset += params.dist_buffer; - const auto base_footprint = make_base_footprint(params); - - const auto can_decel = [&](const auto dist_ahead_of_ego, const auto target_vel) { - const auto acc_to_target_vel = - (ego_data.velocity * ego_data.velocity - target_vel * target_vel) / (2 * dist_ahead_of_ego); - return acc_to_target_vel < std::abs(ego_data.max_decel); - }; - const auto insert_decision = [&](const auto & path_point, const auto & decision) -> bool { - const auto dist_ahead_of_ego = motion_utils::calcSignedArcLength( - ego_data.path->points, ego_data.pose.position, path_point.point.pose.position); - if (!params.skip_if_over_max_decel || can_decel(dist_ahead_of_ego, decision.velocity)) { - to_insert.push_back({decision, path_point}); - return true; - } - return false; - }; - const auto insert_interpolated_decision = - [&](const auto & path_point, const auto & decision) -> bool { - auto interpolated_point = path_point; - const auto & path_pose = path_point.point.pose; - const auto & prev_path_pose = ego_data.path->points[decision.target_path_idx - 1].point.pose; - constexpr auto precision = 0.1; - for (auto ratio = precision; ratio <= 1.0; ratio += precision) { - interpolated_point.point.pose = - tier4_autoware_utils::calcInterpolatedPose(path_pose, prev_path_pose, ratio, false); - const auto is_overlap = boost::geometry::overlaps( - project_to_pose(base_footprint, interpolated_point.point.pose), - decision.lane_to_avoid.polygon2d().basicPolygon()); - if (!is_overlap) { - return insert_decision(path_point, decision); - } - } - return false; - }; - for (const auto & decision : decisions) { - const auto & path_point = ego_data.path->points[decision.target_path_idx]; - const auto decision_is_at_beginning_of_path = - decision.target_path_idx == ego_data.first_path_idx; - bool inserted = false; - if (decision_is_at_beginning_of_path) { - inserted = insert_decision(path_point, decision); - } else { - inserted = insert_interpolated_decision(path_point, decision); - // if no valid point found, fallback to using the previous index (known to not overlap) - if (!inserted) - inserted = insert_decision(ego_data.path->points[decision.target_path_idx], decision); - } - // only insert the first (i.e., lowest arc length) decision - if (inserted) break; - } - return to_insert; -} - } // namespace behavior_velocity_planner::out_of_lane diff --git a/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.hpp b/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.hpp index 79c66148926fc..cdf8eef8f277b 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.hpp +++ b/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.hpp @@ -18,6 +18,7 @@ #include "types.hpp" #include +#include #include #include @@ -56,15 +57,6 @@ class OutOfLaneModule : public SceneModuleInterface // Debug mutable DebugData debug_data_; }; - -/// @brief calculate points along the path where we want ego to slowdown/stop -/// @param ego_data ego data (path, velocity, etc) -/// @param decisions decisions (before which point to stop, what lane to avoid entering, etc) -/// @param params parameters -/// @return precise points to insert in the path -std::vector calculate_slowdown_points( - const EgoData & ego_data, const std::vector & decisions, PlannerParam params); - } // namespace behavior_velocity_planner::out_of_lane #endif // SCENE_OUT_OF_LANE_HPP_ diff --git a/planning/behavior_velocity_out_of_lane_module/src/types.hpp b/planning/behavior_velocity_out_of_lane_module/src/types.hpp index 85266d779f34e..64d88f15e6966 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/types.hpp +++ b/planning/behavior_velocity_out_of_lane_module/src/types.hpp @@ -42,20 +42,21 @@ struct PlannerParam double intervals_ego_buffer; // [s](mode="intervals") buffer to extend the ego time range double intervals_obj_buffer; // [s](mode="intervals") buffer to extend the objects time range double ttc_threshold; // [s](mode="ttc") threshold on time to collision between ego and an object + double ego_min_velocity; // [m/s] minimum velocity of ego used to calculate its ttc or time range - bool objects_use_predicted_paths; // # whether to use the objects' predicted paths - double objects_min_vel; // # [m/s] objects lower than this velocity will be ignored - double objects_min_confidence; // # minimum confidence to consider a predicted path + bool objects_use_predicted_paths; // whether to use the objects' predicted paths + double objects_min_vel; // [m/s] objects lower than this velocity will be ignored + double objects_min_confidence; // minimum confidence to consider a predicted path double overlap_extra_length; // [m] extra length to add around an overlap range double overlap_min_dist; // [m] min distance inside another lane to consider an overlap // action to insert in the path if an object causes a conflict at an overlap bool skip_if_over_max_decel; // if true, skip the action if it causes more than the max decel - bool strict; // if true stop before entering *any* other lane, not only the lane to avoid double dist_buffer; double slow_velocity; double slow_dist_threshold; double stop_dist_threshold; + double precision; // [m] precision when inserting a stop pose in the path // ego dimensions used to create its polygon footprint double front_offset; // [m] front offset (from vehicle info) double rear_offset; // [m] rear offset (from vehicle info) @@ -134,7 +135,7 @@ struct OtherLane /// @brief data related to the ego vehicle struct EgoData { - autoware_auto_planning_msgs::msg::PathWithLaneId * path{}; + autoware_auto_planning_msgs::msg::PathWithLaneId path{}; size_t first_path_idx{}; double velocity{}; // [m/s] double max_decel{}; // [m/s²] diff --git a/planning/behavior_velocity_planner/CMakeLists.txt b/planning/behavior_velocity_planner/CMakeLists.txt index fbdb17f93db65..ee7dd7c07a77a 100644 --- a/planning/behavior_velocity_planner/CMakeLists.txt +++ b/planning/behavior_velocity_planner/CMakeLists.txt @@ -2,25 +2,43 @@ cmake_minimum_required(VERSION 3.14) project(behavior_velocity_planner) find_package(autoware_cmake REQUIRED) +find_package(rosidl_default_generators REQUIRED) + +rosidl_generate_interfaces( + ${PROJECT_NAME} + "srv/LoadPlugin.srv" + "srv/UnloadPlugin.srv" + DEPENDENCIES +) + autoware_package() -ament_auto_add_library(${PROJECT_NAME} SHARED +ament_auto_add_library(${PROJECT_NAME}_lib SHARED src/node.cpp src/planner_manager.cpp ) -rclcpp_components_register_node(${PROJECT_NAME} +rclcpp_components_register_node(${PROJECT_NAME}_lib PLUGIN "behavior_velocity_planner::BehaviorVelocityPlannerNode" EXECUTABLE ${PROJECT_NAME}_node ) +if(${rosidl_cmake_VERSION} VERSION_LESS 2.5.0) + rosidl_target_interfaces(${PROJECT_NAME}_lib + ${PROJECT_NAME} "rosidl_typesupport_cpp") +else() + rosidl_get_typesupport_target( + cpp_typesupport_target ${PROJECT_NAME} "rosidl_typesupport_cpp") + target_link_libraries(${PROJECT_NAME}_lib "${cpp_typesupport_target}") +endif() + if(BUILD_TESTING) ament_add_ros_isolated_gtest(test_${PROJECT_NAME} test/src/test_node_interface.cpp ) target_link_libraries(test_${PROJECT_NAME} gtest_main - ${PROJECT_NAME} + ${PROJECT_NAME}_lib ) target_include_directories(test_${PROJECT_NAME} PRIVATE src) endif() diff --git a/planning/behavior_velocity_planner/README.md b/planning/behavior_velocity_planner/README.md index 25546c95074c3..519e68764b450 100644 --- a/planning/behavior_velocity_planner/README.md +++ b/planning/behavior_velocity_planner/README.md @@ -29,15 +29,15 @@ So for example, in order to stop at a stop line with the vehicles' front on the ## Input topics -| Name | Type | Description | -| ----------------------------------------- | ------------------------------------------------------ | ------------------------------------------------------------------------------------------------------------------------------- | -| `~input/path_with_lane_id` | autoware_auto_planning_msgs::msg::PathWithLaneId | path with lane_id | -| `~input/vector_map` | autoware_auto_mapping_msgs::msg::HADMapBin | vector map | -| `~input/vehicle_odometry` | nav_msgs::msg::Odometry | vehicle velocity | -| `~input/dynamic_objects` | autoware_auto_perception_msgs::msg::PredictedObjects | dynamic objects | -| `~input/no_ground_pointcloud` | sensor_msgs::msg::PointCloud2 | obstacle pointcloud | -| `~/input/compare_map_filtered_pointcloud` | sensor_msgs::msg::PointCloud2 | obstacle pointcloud filtered by compare map. Note that this is used only when the detection method of run out module is Points. | -| `~input/traffic_signals` | autoware_auto_perception_msgs::msg::TrafficSignalArray | traffic light states | +| Name | Type | Description | +| ----------------------------------------- | ---------------------------------------------------- | ------------------------------------------------------------------------------------------------------------------------------- | +| `~input/path_with_lane_id` | autoware_auto_planning_msgs::msg::PathWithLaneId | path with lane_id | +| `~input/vector_map` | autoware_auto_mapping_msgs::msg::HADMapBin | vector map | +| `~input/vehicle_odometry` | nav_msgs::msg::Odometry | vehicle velocity | +| `~input/dynamic_objects` | autoware_auto_perception_msgs::msg::PredictedObjects | dynamic objects | +| `~input/no_ground_pointcloud` | sensor_msgs::msg::PointCloud2 | obstacle pointcloud | +| `~/input/compare_map_filtered_pointcloud` | sensor_msgs::msg::PointCloud2 | obstacle pointcloud filtered by compare map. Note that this is used only when the detection method of run out module is Points. | +| `~input/traffic_signals` | autoware_perception_msgs::msg::TrafficSignalArray | traffic light states | ## Output topics diff --git a/planning/behavior_velocity_planner/config/behavior_velocity_planner.param.yaml b/planning/behavior_velocity_planner/config/behavior_velocity_planner.param.yaml index e8377f841aa77..276e8f8e3145a 100644 --- a/planning/behavior_velocity_planner/config/behavior_velocity_planner.param.yaml +++ b/planning/behavior_velocity_planner/config/behavior_velocity_planner.param.yaml @@ -22,5 +22,6 @@ # behavior_velocity_planner::OcclusionSpotModulePlugin # behavior_velocity_planner::RunOutModulePlugin # behavior_velocity_planner::SpeedBumpModulePlugin + # behavior_velocity_planner::TemplateModulePlugin - behavior_velocity_planner::OutOfLaneModulePlugin - behavior_velocity_planner::NoDrivableLaneModulePlugin diff --git a/planning/behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml b/planning/behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml index b517b77b328c7..69d9c27331e52 100644 --- a/planning/behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml +++ b/planning/behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml @@ -19,6 +19,7 @@ + @@ -40,6 +41,7 @@ + @@ -68,6 +70,7 @@ + diff --git a/planning/behavior_velocity_planner/package.xml b/planning/behavior_velocity_planner/package.xml index 5a307398e9afd..d83f74a71b66f 100644 --- a/planning/behavior_velocity_planner/package.xml +++ b/planning/behavior_velocity_planner/package.xml @@ -33,6 +33,8 @@ autoware_cmake eigen3_cmake_module + rosidl_default_generators + autoware_auto_mapping_msgs autoware_auto_perception_msgs autoware_auto_planning_msgs @@ -60,6 +62,8 @@ tier4_v2x_msgs visualization_msgs + rosidl_default_runtime + ament_cmake_ros ament_lint_auto autoware_lint_common @@ -77,6 +81,9 @@ behavior_velocity_traffic_light_module behavior_velocity_virtual_traffic_light_module behavior_velocity_walkway_module + + + rosidl_interface_packages ament_cmake diff --git a/planning/behavior_velocity_planner/src/node.cpp b/planning/behavior_velocity_planner/src/node.cpp index 12503f743b0f2..ec28656d8f866 100644 --- a/planning/behavior_velocity_planner/src/node.cpp +++ b/planning/behavior_velocity_planner/src/node.cpp @@ -16,7 +16,11 @@ #include #include +#include +#include +#include #include +#include #include #include @@ -24,7 +28,6 @@ #include #include #include - #ifdef ROS_DISTRO_GALACTIC #include #else @@ -72,6 +75,8 @@ BehaviorVelocityPlannerNode::BehaviorVelocityPlannerNode(const rclcpp::NodeOptio planner_data_(*this) { using std::placeholders::_1; + using std::placeholders::_2; + // Trigger Subscriber trigger_sub_path_with_lane_id_ = this->create_subscription( @@ -99,7 +104,7 @@ BehaviorVelocityPlannerNode::BehaviorVelocityPlannerNode(const rclcpp::NodeOptio std::bind(&BehaviorVelocityPlannerNode::onLaneletMap, this, _1), createSubscriptionOptions(this)); sub_traffic_signals_ = - this->create_subscription( + this->create_subscription( "~/input/traffic_signals", 1, std::bind(&BehaviorVelocityPlannerNode::onTrafficSignals, this, _1), createSubscriptionOptions(this)); @@ -115,6 +120,12 @@ BehaviorVelocityPlannerNode::BehaviorVelocityPlannerNode(const rclcpp::NodeOptio "~/input/occupancy_grid", 1, std::bind(&BehaviorVelocityPlannerNode::onOccupancyGrid, this, _1), createSubscriptionOptions(this)); + srv_load_plugin_ = create_service( + "~/service/load_plugin", std::bind(&BehaviorVelocityPlannerNode::onLoadPlugin, this, _1, _2)); + srv_unload_plugin_ = create_service( + "~/service/unload_plugin", + std::bind(&BehaviorVelocityPlannerNode::onUnloadPlugin, this, _1, _2)); + // set velocity smoother param onParam(); @@ -140,6 +151,24 @@ BehaviorVelocityPlannerNode::BehaviorVelocityPlannerNode(const rclcpp::NodeOptio for (const auto & name : declare_parameter>("launch_modules")) { planner_manager_.launchScenePlugin(*this, name); } + + logger_configure_ = std::make_unique(this); +} + +void BehaviorVelocityPlannerNode::onLoadPlugin( + const LoadPlugin::Request::SharedPtr request, + [[maybe_unused]] const LoadPlugin::Response::SharedPtr response) +{ + std::unique_lock lk(mutex_); + planner_manager_.launchScenePlugin(*this, request->plugin_name); +} + +void BehaviorVelocityPlannerNode::onUnloadPlugin( + const UnloadPlugin::Request::SharedPtr request, + [[maybe_unused]] const UnloadPlugin::Response::SharedPtr response) +{ + std::unique_lock lk(mutex_); + planner_manager_.removeScenePlugin(*this, request->plugin_name); } // NOTE: argument planner_data must not be referenced for multithreading @@ -287,15 +316,15 @@ void BehaviorVelocityPlannerNode::onLaneletMap( } void BehaviorVelocityPlannerNode::onTrafficSignals( - const autoware_auto_perception_msgs::msg::TrafficSignalArray::ConstSharedPtr msg) + const autoware_perception_msgs::msg::TrafficSignalArray::ConstSharedPtr msg) { std::lock_guard lock(mutex_); for (const auto & signal : msg->signals) { - autoware_auto_perception_msgs::msg::TrafficSignalStamped traffic_signal; - traffic_signal.header = msg->header; + TrafficSignalStamped traffic_signal; + traffic_signal.stamp = msg->stamp; traffic_signal.signal = signal; - planner_data_.traffic_light_id_map[signal.map_primitive_id] = traffic_signal; + planner_data_.traffic_light_id_map[signal.traffic_signal_id] = traffic_signal; } } diff --git a/planning/behavior_velocity_planner/src/node.hpp b/planning/behavior_velocity_planner/src/node.hpp index 91172fdd77da8..ded1d08700a1d 100644 --- a/planning/behavior_velocity_planner/src/node.hpp +++ b/planning/behavior_velocity_planner/src/node.hpp @@ -16,7 +16,10 @@ #define NODE_HPP_ #include "planner_manager.hpp" +#include "tier4_autoware_utils/ros/logger_level_configure.hpp" +#include +#include #include #include @@ -30,6 +33,7 @@ #include #include #include +#include #include #include @@ -37,11 +41,15 @@ #include #include #include +#include namespace behavior_velocity_planner { using autoware_auto_mapping_msgs::msg::HADMapBin; +using behavior_velocity_planner::srv::LoadPlugin; +using behavior_velocity_planner::srv::UnloadPlugin; using tier4_planning_msgs::msg::VelocityLimit; + class BehaviorVelocityPlannerNode : public rclcpp::Node { public: @@ -61,7 +69,7 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node rclcpp::Subscription::SharedPtr sub_vehicle_odometry_; rclcpp::Subscription::SharedPtr sub_acceleration_; rclcpp::Subscription::SharedPtr sub_lanelet_map_; - rclcpp::Subscription::SharedPtr + rclcpp::Subscription::SharedPtr sub_traffic_signals_; rclcpp::Subscription::SharedPtr sub_virtual_traffic_light_states_; @@ -77,7 +85,7 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node void onAcceleration(const geometry_msgs::msg::AccelWithCovarianceStamped::ConstSharedPtr msg); void onLaneletMap(const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg); void onTrafficSignals( - const autoware_auto_perception_msgs::msg::TrafficSignalArray::ConstSharedPtr msg); + const autoware_perception_msgs::msg::TrafficSignalArray::ConstSharedPtr msg); void onVirtualTrafficLightStates( const tier4_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr msg); void onOccupancyGrid(const nav_msgs::msg::OccupancyGrid::ConstSharedPtr msg); @@ -102,6 +110,14 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node HADMapBin::ConstSharedPtr map_ptr_{nullptr}; bool has_received_map_; + rclcpp::Service::SharedPtr srv_load_plugin_; + rclcpp::Service::SharedPtr srv_unload_plugin_; + void onUnloadPlugin( + const UnloadPlugin::Request::SharedPtr request, + const UnloadPlugin::Response::SharedPtr response); + void onLoadPlugin( + const LoadPlugin::Request::SharedPtr request, const LoadPlugin::Response::SharedPtr response); + // mutex for planner_data_ std::mutex mutex_; @@ -111,6 +127,8 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node autoware_auto_planning_msgs::msg::Path generatePath( const autoware_auto_planning_msgs::msg::PathWithLaneId::ConstSharedPtr input_path_msg, const PlannerData & planner_data); + + std::unique_ptr logger_configure_; }; } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/src/planner_manager.cpp b/planning/behavior_velocity_planner/src/planner_manager.cpp index 39d4dc345e899..e1c801a27dca3 100644 --- a/planning/behavior_velocity_planner/src/planner_manager.cpp +++ b/planning/behavior_velocity_planner/src/planner_manager.cpp @@ -60,12 +60,42 @@ void BehaviorVelocityPlannerManager::launchScenePlugin( if (plugin_loader_.isClassAvailable(name)) { const auto plugin = plugin_loader_.createSharedInstance(name); plugin->init(node); + + // Check if the plugin is already registered. + for (const auto & running_plugin : scene_manager_plugins_) { + if (plugin->getModuleName() == running_plugin->getModuleName()) { + RCLCPP_WARN_STREAM(node.get_logger(), "The plugin '" << name << "' is already loaded."); + return; + } + } + + // register scene_manager_plugins_.push_back(plugin); + RCLCPP_INFO_STREAM(node.get_logger(), "The scene plugin '" << name << "' is loaded."); } else { RCLCPP_ERROR_STREAM(node.get_logger(), "The scene plugin '" << name << "' is not available."); } } +void BehaviorVelocityPlannerManager::removeScenePlugin( + rclcpp::Node & node, const std::string & name) +{ + auto it = std::remove_if( + scene_manager_plugins_.begin(), scene_manager_plugins_.end(), + [&](const std::shared_ptr plugin) { + return plugin->getModuleName() == name; + }); + + if (it == scene_manager_plugins_.end()) { + RCLCPP_WARN_STREAM( + node.get_logger(), + "The scene plugin '" << name << "' is not found in the registered modules."); + } else { + scene_manager_plugins_.erase(it, scene_manager_plugins_.end()); + RCLCPP_INFO_STREAM(node.get_logger(), "The scene plugin '" << name << "' is unloaded."); + } +} + autoware_auto_planning_msgs::msg::PathWithLaneId BehaviorVelocityPlannerManager::planPathVelocity( const std::shared_ptr & planner_data, const autoware_auto_planning_msgs::msg::PathWithLaneId & input_path_msg) diff --git a/planning/behavior_velocity_planner/src/planner_manager.hpp b/planning/behavior_velocity_planner/src/planner_manager.hpp index e57d90d5cdfee..7485e5faba8bf 100644 --- a/planning/behavior_velocity_planner/src/planner_manager.hpp +++ b/planning/behavior_velocity_planner/src/planner_manager.hpp @@ -43,6 +43,7 @@ class BehaviorVelocityPlannerManager public: BehaviorVelocityPlannerManager(); void launchScenePlugin(rclcpp::Node & node, const std::string & name); + void removeScenePlugin(rclcpp::Node & node, const std::string & name); autoware_auto_planning_msgs::msg::PathWithLaneId planPathVelocity( const std::shared_ptr & planner_data, diff --git a/planning/behavior_velocity_planner/srv/LoadPlugin.srv b/planning/behavior_velocity_planner/srv/LoadPlugin.srv new file mode 100644 index 0000000000000..7b9f08ab0ded8 --- /dev/null +++ b/planning/behavior_velocity_planner/srv/LoadPlugin.srv @@ -0,0 +1,3 @@ +string plugin_name +--- +bool success diff --git a/planning/behavior_velocity_planner/srv/UnloadPlugin.srv b/planning/behavior_velocity_planner/srv/UnloadPlugin.srv new file mode 100644 index 0000000000000..7b9f08ab0ded8 --- /dev/null +++ b/planning/behavior_velocity_planner/srv/UnloadPlugin.srv @@ -0,0 +1,3 @@ +string plugin_name +--- +bool success diff --git a/planning/behavior_velocity_planner/test/src/test_node_interface.cpp b/planning/behavior_velocity_planner/test/src/test_node_interface.cpp index eb405f06c8c4b..6756a82ca20d4 100644 --- a/planning/behavior_velocity_planner/test/src/test_node_interface.cpp +++ b/planning/behavior_velocity_planner/test/src/test_node_interface.cpp @@ -79,7 +79,8 @@ std::shared_ptr generateNode() get_behavior_velocity_module_config("stop_line"), get_behavior_velocity_module_config("traffic_light"), get_behavior_velocity_module_config("virtual_traffic_light"), - get_behavior_velocity_module_config("out_of_lane")}); + get_behavior_velocity_module_config("out_of_lane"), + get_behavior_velocity_module_config("no_drivable_lane")}); // TODO(Takagi, Isamu): set launch_modules // TODO(Kyoichi Sugahara) set to true launch_virtual_traffic_light diff --git a/planning/behavior_velocity_planner_common/CMakeLists.txt b/planning/behavior_velocity_planner_common/CMakeLists.txt index 334ee35329ba3..e958df74afe2d 100644 --- a/planning/behavior_velocity_planner_common/CMakeLists.txt +++ b/planning/behavior_velocity_planner_common/CMakeLists.txt @@ -5,7 +5,12 @@ find_package(autoware_cmake REQUIRED) autoware_package() ament_auto_add_library(${PROJECT_NAME} SHARED + src/scene_module_interface.cpp + src/velocity_factor_interface.cpp src/utilization/path_utilization.cpp + src/utilization/trajectory_utils.cpp + src/utilization/arc_lane_util.cpp + src/utilization/boost_geometry_helper.cpp src/utilization/util.cpp src/utilization/debug.cpp ) diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp index 44d3184a5f6b7..620fc61ec5360 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp @@ -17,14 +17,13 @@ #include "route_handler/route_handler.hpp" -#include +#include #include #include #include #include -#include -#include +#include #include #include #include @@ -38,13 +37,8 @@ #include -#include -#include -#include -#include #include #include -#include #include #include @@ -83,7 +77,7 @@ struct PlannerData double ego_nearest_yaw_threshold; // other internal data - std::map traffic_light_id_map; + std::map traffic_light_id_map; boost::optional external_velocity_limit; tier4_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr virtual_traffic_light_states; @@ -132,14 +126,12 @@ struct PlannerData return true; } - std::shared_ptr getTrafficSignal( - const int id) const + std::shared_ptr getTrafficSignal(const int id) const { if (traffic_light_id_map.count(id) == 0) { return {}; } - return std::make_shared( - traffic_light_id_map.at(id)); + return std::make_shared(traffic_light_id_map.at(id)); } }; } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/scene_module_interface.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/scene_module_interface.hpp index c33ce84ff09de..90e78215cae2d 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/scene_module_interface.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/scene_module_interface.hpp @@ -16,12 +16,11 @@ #define BEHAVIOR_VELOCITY_PLANNER_COMMON__SCENE_MODULE_INTERFACE_HPP_ #include -#include #include #include #include #include -#include +#include #include #include @@ -33,10 +32,7 @@ #include #include -#include -#include #include -#include #include #include #include @@ -46,7 +42,6 @@ #include #include - namespace behavior_velocity_planner { @@ -54,7 +49,6 @@ using autoware_auto_planning_msgs::msg::PathWithLaneId; using builtin_interfaces::msg::Time; using rtc_interface::RTCInterface; using tier4_autoware_utils::DebugPublisher; -using tier4_autoware_utils::StopWatch; using tier4_debug_msgs::msg::Float64Stamped; using tier4_planning_msgs::msg::StopFactor; using tier4_planning_msgs::msg::StopReason; @@ -65,15 +59,7 @@ class SceneModuleInterface { public: explicit SceneModuleInterface( - const int64_t module_id, rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock) - : module_id_(module_id), - activated_(false), - safe_(false), - distance_(std::numeric_limits::lowest()), - logger_(logger), - clock_(clock) - { - } + const int64_t module_id, rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock); virtual ~SceneModuleInterface() = default; virtual bool modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) = 0; @@ -123,44 +109,14 @@ class SceneModuleInterface void setSafe(const bool safe) { safe_ = safe; } void setDistance(const double distance) { distance_ = distance; } - template - size_t findEgoSegmentIndex(const std::vector & points) const - { - const auto & p = planner_data_; - return motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - points, p->current_odometry->pose, p->ego_nearest_dist_threshold); - } + size_t findEgoSegmentIndex( + const std::vector & points) const; }; class SceneModuleManagerInterface { public: - SceneModuleManagerInterface(rclcpp::Node & node, [[maybe_unused]] const char * module_name) - : node_(node), clock_(node.get_clock()), logger_(node.get_logger()) - { - const auto ns = std::string("~/debug/") + module_name; - pub_debug_ = node.create_publisher(ns, 1); - if (!node.has_parameter("is_publish_debug_path")) { - is_publish_debug_path_ = node.declare_parameter("is_publish_debug_path"); - } else { - is_publish_debug_path_ = node.get_parameter("is_publish_debug_path").as_bool(); - } - if (is_publish_debug_path_) { - pub_debug_path_ = node.create_publisher( - std::string("~/debug/path_with_lane_id/") + module_name, 1); - } - pub_virtual_wall_ = node.create_publisher( - std::string("~/virtual_wall/") + module_name, 5); - pub_velocity_factor_ = node.create_publisher( - std::string("/planning/velocity_factors/") + module_name, 1); - pub_stop_reason_ = - node.create_publisher("~/output/stop_reasons", 1); - pub_infrastructure_commands_ = - node.create_publisher( - "~/output/infrastructure_commands", 1); - - processing_time_publisher_ = std::make_shared(&node, "~/debug"); - } + SceneModuleManagerInterface(rclcpp::Node & node, [[maybe_unused]] const char * module_name); virtual ~SceneModuleManagerInterface() = default; @@ -170,13 +126,7 @@ class SceneModuleManagerInterface void updateSceneModuleInstances( const std::shared_ptr & planner_data, - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) - { - planner_data_ = planner_data; - - launchNewModules(path); - deleteExpiredModules(path); - } + const autoware_auto_planning_msgs::msg::PathWithLaneId & path); virtual void plan(autoware_auto_planning_msgs::msg::PathWithLaneId * path) { @@ -184,120 +134,26 @@ class SceneModuleManagerInterface } protected: - virtual void modifyPathVelocity(autoware_auto_planning_msgs::msg::PathWithLaneId * path) - { - StopWatch stop_watch; - stop_watch.tic("Total"); - visualization_msgs::msg::MarkerArray debug_marker_array; - tier4_planning_msgs::msg::StopReasonArray stop_reason_array; - autoware_adapi_v1_msgs::msg::VelocityFactorArray velocity_factor_array; - stop_reason_array.header.frame_id = "map"; - stop_reason_array.header.stamp = clock_->now(); - velocity_factor_array.header.frame_id = "map"; - velocity_factor_array.header.stamp = clock_->now(); - - tier4_v2x_msgs::msg::InfrastructureCommandArray infrastructure_command_array; - infrastructure_command_array.stamp = clock_->now(); - - first_stop_path_point_index_ = static_cast(path->points.size()) - 1; - for (const auto & scene_module : scene_modules_) { - tier4_planning_msgs::msg::StopReason stop_reason; - scene_module->resetVelocityFactor(); - scene_module->setPlannerData(planner_data_); - scene_module->modifyPathVelocity(path, &stop_reason); - - // The velocity factor must be called after modifyPathVelocity. - const auto velocity_factor = scene_module->getVelocityFactor(); - if (velocity_factor.type != VelocityFactor::UNKNOWN) { - velocity_factor_array.factors.emplace_back(velocity_factor); - } - if (stop_reason.reason != "") { - stop_reason_array.stop_reasons.emplace_back(stop_reason); - } - - if (const auto command = scene_module->getInfrastructureCommand()) { - infrastructure_command_array.commands.push_back(*command); - } - - if (scene_module->getFirstStopPathPointIndex() < first_stop_path_point_index_) { - first_stop_path_point_index_ = scene_module->getFirstStopPathPointIndex(); - } - - for (const auto & marker : scene_module->createDebugMarkerArray().markers) { - debug_marker_array.markers.push_back(marker); - } - - virtual_wall_marker_creator_.add_virtual_walls(scene_module->createVirtualWalls()); - } - - if (!stop_reason_array.stop_reasons.empty()) { - pub_stop_reason_->publish(stop_reason_array); - } - pub_velocity_factor_->publish(velocity_factor_array); - pub_infrastructure_commands_->publish(infrastructure_command_array); - pub_debug_->publish(debug_marker_array); - if (is_publish_debug_path_) { - autoware_auto_planning_msgs::msg::PathWithLaneId debug_path; - debug_path.header = path->header; - debug_path.points = path->points; - pub_debug_path_->publish(debug_path); - } - pub_virtual_wall_->publish(virtual_wall_marker_creator_.create_markers(clock_->now())); - processing_time_publisher_->publish( - std::string(getModuleName()) + "/processing_time_ms", stop_watch.toc("Total")); - } + virtual void modifyPathVelocity(autoware_auto_planning_msgs::msg::PathWithLaneId * path); virtual void launchNewModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) = 0; virtual std::function &)> getModuleExpiredFunction(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) = 0; - virtual void deleteExpiredModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) - { - const auto isModuleExpired = getModuleExpiredFunction(path); - - // Copy container to avoid iterator corruption - // due to scene_modules_.erase() in unregisterModule() - const auto copied_scene_modules = scene_modules_; - - for (const auto & scene_module : copied_scene_modules) { - if (isModuleExpired(scene_module)) { - unregisterModule(scene_module); - } - } - } + virtual void deleteExpiredModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path); bool isModuleRegistered(const int64_t module_id) { return registered_module_id_set_.count(module_id) != 0; } - void registerModule(const std::shared_ptr & scene_module) - { - RCLCPP_INFO( - logger_, "register task: module = %s, id = %lu", getModuleName(), - scene_module->getModuleId()); - registered_module_id_set_.emplace(scene_module->getModuleId()); - scene_modules_.insert(scene_module); - } + void registerModule(const std::shared_ptr & scene_module); - void unregisterModule(const std::shared_ptr & scene_module) - { - RCLCPP_INFO( - logger_, "unregister task: module = %s, id = %lu", getModuleName(), - scene_module->getModuleId()); - registered_module_id_set_.erase(scene_module->getModuleId()); - scene_modules_.erase(scene_module); - } + void unregisterModule(const std::shared_ptr & scene_module); - template - size_t findEgoSegmentIndex(const std::vector & points) const - { - const auto & p = planner_data_; - return motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - points, p->current_odometry->pose, p->ego_nearest_dist_threshold, - p->ego_nearest_yaw_threshold); - } + size_t findEgoSegmentIndex( + const std::vector & points) const; std::set> scene_modules_; std::set registered_module_id_set_; @@ -327,38 +183,17 @@ class SceneModuleManagerInterfaceWithRTC : public SceneModuleManagerInterface { public: SceneModuleManagerInterfaceWithRTC( - rclcpp::Node & node, const char * module_name, const bool enable_rtc = true) - : SceneModuleManagerInterface(node, module_name), rtc_interface_(&node, module_name, enable_rtc) - { - } + rclcpp::Node & node, const char * module_name, const bool enable_rtc = true); - void plan(autoware_auto_planning_msgs::msg::PathWithLaneId * path) override - { - setActivation(); - modifyPathVelocity(path); - sendRTC(path->header.stamp); - } + void plan(autoware_auto_planning_msgs::msg::PathWithLaneId * path) override; protected: RTCInterface rtc_interface_; std::unordered_map map_uuid_; - virtual void sendRTC(const Time & stamp) - { - for (const auto & scene_module : scene_modules_) { - const UUID uuid = getUUID(scene_module->getModuleId()); - updateRTCStatus(uuid, scene_module->isSafe(), scene_module->getDistance(), stamp); - } - publishRTCStatus(stamp); - } + virtual void sendRTC(const Time & stamp); - virtual void setActivation() - { - for (const auto & scene_module : scene_modules_) { - const UUID uuid = getUUID(scene_module->getModuleId()); - scene_module->setActivation(rtc_interface_.isActivated(uuid)); - } - } + virtual void setActivation(); void updateRTCStatus( const UUID & uuid, const bool safe, const double distance, const Time & stamp) @@ -370,45 +205,13 @@ class SceneModuleManagerInterfaceWithRTC : public SceneModuleManagerInterface void publishRTCStatus(const Time & stamp) { rtc_interface_.publishCooperateStatus(stamp); } - UUID getUUID(const int64_t & module_id) const - { - if (map_uuid_.count(module_id) == 0) { - const UUID uuid; - return uuid; - } - return map_uuid_.at(module_id); - } + UUID getUUID(const int64_t & module_id) const; - void generateUUID(const int64_t & module_id) - { - map_uuid_.insert({module_id, tier4_autoware_utils::generateUUID()}); - } + void generateUUID(const int64_t & module_id); - void removeUUID(const int64_t & module_id) - { - const auto result = map_uuid_.erase(module_id); - if (result == 0) { - RCLCPP_WARN_STREAM( - logger_, "[removeUUID] module_id = " << module_id << " is not registered."); - } - } + void removeUUID(const int64_t & module_id); - void deleteExpiredModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override - { - const auto isModuleExpired = getModuleExpiredFunction(path); - - // Copy container to avoid iterator corruption - // due to scene_modules_.erase() in unregisterModule() - const auto copied_scene_modules = scene_modules_; - - for (const auto & scene_module : copied_scene_modules) { - if (isModuleExpired(scene_module)) { - removeRTCStatus(getUUID(scene_module->getModuleId())); - removeUUID(scene_module->getModuleId()); - unregisterModule(scene_module); - } - } - } + void deleteExpiredModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; }; } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/arc_lane_util.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/arc_lane_util.hpp index ffc216e863500..63d7c08b945fd 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/arc_lane_util.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/arc_lane_util.hpp @@ -16,22 +16,12 @@ #define BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__ARC_LANE_UTIL_HPP_ #include -#include #include #include #include -#ifdef ROS_DISTRO_GALACTIC -#include -#include -#else -#include - -#include -#endif - #include #include #include @@ -46,7 +36,7 @@ namespace bg = boost::geometry; namespace { -geometry_msgs::msg::Point convertToGeomPoint(const Point2d & p) +geometry_msgs::msg::Point convertToGeomPoint(const tier4_autoware_utils::Point2d & p) { geometry_msgs::msg::Point geom_p; geom_p.x = p.x(); @@ -55,73 +45,23 @@ geometry_msgs::msg::Point convertToGeomPoint(const Point2d & p) return geom_p; } -geometry_msgs::msg::Point operator+( - const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2) -{ - geometry_msgs::msg::Point p; - p.x = p1.x + p2.x; - p.y = p1.y + p2.y; - p.z = p1.z + p2.z; - - return p; -} - -[[maybe_unused]] geometry_msgs::msg::Point operator*( - const geometry_msgs::msg::Point & p, const double v) -{ - geometry_msgs::msg::Point multiplied_p; - multiplied_p.x = p.x * v; - multiplied_p.y = p.y * v; - multiplied_p.z = p.z * v; - - return multiplied_p; -} - -[[maybe_unused]] geometry_msgs::msg::Point operator*( - const double v, const geometry_msgs::msg::Point & p) -{ - return p * v; -} } // namespace namespace arc_lane_utils { -using PathIndexWithPose = std::pair; // front index, pose -using PathIndexWithPoint2d = std::pair; // front index, point2d +using PathIndexWithPose = std::pair; // front index, pose +using PathIndexWithPoint2d = + std::pair; // front index, point2d using PathIndexWithPoint = std::pair; // front index, point2d using PathIndexWithOffset = std::pair; // front index, offset -inline double calcSignedDistance( - const geometry_msgs::msg::Pose & p1, const geometry_msgs::msg::Point & p2) -{ - Eigen::Affine3d map2p1; - tf2::fromMsg(p1, map2p1); - const auto basecoords_p2 = map2p1.inverse() * Eigen::Vector3d(p2.x, p2.y, p2.z); - return basecoords_p2.x() >= 0 ? basecoords_p2.norm() : -basecoords_p2.norm(); -} +double calcSignedDistance( + const geometry_msgs::msg::Pose & p1, const geometry_msgs::msg::Point & p2); // calculate one collision point between the line (from p1 to p2) and the line (from p3 to p4) -inline boost::optional checkCollision( +boost::optional checkCollision( const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2, - const geometry_msgs::msg::Point & p3, const geometry_msgs::msg::Point & p4) -{ - const double det = (p2.x - p1.x) * (p4.y - p3.y) - (p2.y - p1.y) * (p4.x - p3.x); - - if (det == 0.0) { - // collision is not one point. - return boost::none; - } - - const double t1 = ((p4.y - p3.y) * (p4.x - p1.x) - (p4.x - p3.x) * (p4.y - p1.y)) / det; - const double t2 = ((p2.x - p1.x) * (p4.y - p1.y) - (p2.y - p1.y) * (p4.x - p1.x)) / det; - - // check collision is outside the segment line - if (t1 < 0.0 || 1.0 < t1 || t2 < 0.0 || 1.0 < t2) { - return boost::none; - } - - return p1 * (1.0 - t1) + p2 * t1; -} + const geometry_msgs::msg::Point & p3, const geometry_msgs::msg::Point & p4); template boost::optional findCollisionSegment( @@ -223,16 +163,9 @@ boost::optional findOffsetSegment( tier4_autoware_utils::calcDistance2d(path.points.at(collision_idx + 1), collision_point)); } -inline boost::optional findOffsetSegment( +boost::optional findOffsetSegment( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t index, - const double offset) -{ - if (offset >= 0) { - return findForwardOffsetSegment(path, index, offset); - } - - return findBackwardOffsetSegment(path, index, -offset); -} + const double offset); template geometry_msgs::msg::Pose calcTargetPose(const T & path, const PathIndexWithOffset & offset_segment) @@ -263,33 +196,10 @@ geometry_msgs::msg::Pose calcTargetPose(const T & path, const PathIndexWithOffse return target_pose; } -inline boost::optional createTargetPoint( +boost::optional createTargetPoint( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const LineString2d & stop_line, - const size_t lane_id, const double margin, const double vehicle_offset) -{ - // Find collision segment - const auto collision_segment = findCollisionSegment(path, stop_line, lane_id); - if (!collision_segment) { - // No collision - return {}; - } - - // Calculate offset length from stop line - // Use '-' to make the positive direction is forward - const double offset_length = -(margin + vehicle_offset); + const size_t lane_id, const double margin, const double vehicle_offset); - // Find offset segment - const auto offset_segment = findOffsetSegment(path, *collision_segment, offset_length); - if (!offset_segment) { - // No enough path length - return {}; - } - - const auto target_pose = calcTargetPose(path, *offset_segment); - - const auto front_idx = offset_segment->first; - return std::make_pair(front_idx, target_pose); -} } // namespace arc_lane_utils } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp index 193890ec70ceb..0f3cc7203ef1f 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp @@ -24,24 +24,11 @@ #include #include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include #include -#include -#include - -#include #include - // cppcheck-suppress unknownMacro BOOST_GEOMETRY_REGISTER_POINT_3D(geometry_msgs::msg::Point, double, cs::cartesian, x, y, z) BOOST_GEOMETRY_REGISTER_POINT_3D( @@ -83,48 +70,13 @@ LineString2d to_bg2d(const std::vector & vec) return ps; } -inline Polygon2d lines2polygon(const LineString2d & left_line, const LineString2d & right_line) -{ - Polygon2d polygon; - - polygon.outer().push_back(left_line.front()); +Polygon2d lines2polygon(const LineString2d & left_line, const LineString2d & right_line); - for (auto itr = right_line.begin(); itr != right_line.end(); ++itr) { - polygon.outer().push_back(*itr); - } - - for (auto itr = left_line.rbegin(); itr != left_line.rend(); ++itr) { - polygon.outer().push_back(*itr); - } +Polygon2d upScalePolygon( + const geometry_msgs::msg::Point & position, const Polygon2d & polygon, const double scale); - bg::correct(polygon); - return polygon; -} +geometry_msgs::msg::Polygon toGeomPoly(const Polygon2d & polygon); -inline Polygon2d upScalePolygon( - const geometry_msgs::msg::Point & position, const Polygon2d & polygon, const double scale) -{ - Polygon2d transformed_polygon; - // upscale - for (size_t i = 0; i < polygon.outer().size(); i++) { - const double upscale_x = (polygon.outer().at(i).x() - position.x) * scale + position.x; - const double upscale_y = (polygon.outer().at(i).y() - position.y) * scale + position.y; - transformed_polygon.outer().emplace_back(Point2d(upscale_x, upscale_y)); - } - return transformed_polygon; -} - -inline geometry_msgs::msg::Polygon toGeomPoly(const Polygon2d & polygon) -{ - geometry_msgs::msg::Polygon polygon_msg; - geometry_msgs::msg::Point32 point_msg; - for (const auto & p : polygon.outer()) { - point_msg.x = p.x(); - point_msg.y = p.y(); - polygon_msg.points.push_back(point_msg); - } - return polygon_msg; -} } // namespace behavior_velocity_planner #endif // BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__BOOST_GEOMETRY_HELPER_HPP_ diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/debug.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/debug.hpp index e840b6f8a5995..98df29d1c44c0 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/debug.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/debug.hpp @@ -15,8 +15,12 @@ #ifndef BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__DEBUG_HPP_ #define BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__DEBUG_HPP_ -#include +#include +#include +#include +#include +#include #include #include diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/path_utilization.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/path_utilization.hpp index 4156f7df9540b..591ef8667b81b 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/path_utilization.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/path_utilization.hpp @@ -16,7 +16,6 @@ #define BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__PATH_UTILIZATION_HPP_ #include -#include #include #include diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/trajectory_utils.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/trajectory_utils.hpp index 7d5278c3cf972..c86272cff1c2e 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/trajectory_utils.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/trajectory_utils.hpp @@ -16,32 +16,18 @@ #define BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__TRAJECTORY_UTILS_HPP_ #include -#include -#include -#include -#include -#include -#include #include #include #include -#include #include -#include -#include - -#include -#include #include -#include #include #include namespace behavior_velocity_planner { -using autoware_auto_planning_msgs::msg::PathPointWithLaneId; using autoware_auto_planning_msgs::msg::PathWithLaneId; using autoware_auto_planning_msgs::msg::Trajectory; using autoware_auto_planning_msgs::msg::TrajectoryPoint; @@ -49,83 +35,15 @@ using TrajectoryPoints = std::vector; using geometry_msgs::msg::Quaternion; using TrajectoryPointWithIdx = std::pair; -inline TrajectoryPoints convertPathToTrajectoryPoints(const PathWithLaneId & path) -{ - TrajectoryPoints tps; - for (const auto & p : path.points) { - TrajectoryPoint tp; - tp.pose = p.point.pose; - tp.longitudinal_velocity_mps = p.point.longitudinal_velocity_mps; - // since path point doesn't have acc for now - tp.acceleration_mps2 = 0; - tps.emplace_back(tp); - } - return tps; -} -inline PathWithLaneId convertTrajectoryPointsToPath(const TrajectoryPoints & trajectory) -{ - PathWithLaneId path; - for (const auto & p : trajectory) { - PathPointWithLaneId pp; - pp.point.pose = p.pose; - pp.point.longitudinal_velocity_mps = p.longitudinal_velocity_mps; - path.points.emplace_back(pp); - } - return path; -} - -inline Quaternion lerpOrientation( - const Quaternion & o_from, const Quaternion & o_to, const double ratio) -{ - tf2::Quaternion q_from, q_to; - tf2::fromMsg(o_from, q_from); - tf2::fromMsg(o_to, q_to); +TrajectoryPoints convertPathToTrajectoryPoints(const PathWithLaneId & path); +PathWithLaneId convertTrajectoryPointsToPath(const TrajectoryPoints & trajectory); - const auto q_interpolated = q_from.slerp(q_to, ratio); - return tf2::toMsg(q_interpolated); -} +Quaternion lerpOrientation(const Quaternion & o_from, const Quaternion & o_to, const double ratio); //! smooth path point with lane id starts from ego position on path to the path end -inline bool smoothPath( +bool smoothPath( const PathWithLaneId & in_path, PathWithLaneId & out_path, - const std::shared_ptr & planner_data) -{ - const geometry_msgs::msg::Pose current_pose = planner_data->current_odometry->pose; - const double v0 = planner_data->current_velocity->twist.linear.x; - const double a0 = planner_data->current_acceleration->accel.accel.linear.x; - const auto & external_v_limit = planner_data->external_velocity_limit; - const auto & smoother = planner_data->velocity_smoother_; - - auto trajectory = convertPathToTrajectoryPoints(in_path); - if (external_v_limit) { - motion_velocity_smoother::trajectory_utils::applyMaximumVelocityLimit( - 0, trajectory.size(), external_v_limit->max_velocity, trajectory); - } - const auto traj_lateral_acc_filtered = smoother->applyLateralAccelerationFilter(trajectory); - - // Resample trajectory with ego-velocity based interval distances - auto traj_resampled = smoother->resampleTrajectory( - traj_lateral_acc_filtered, v0, current_pose, planner_data->ego_nearest_dist_threshold, - planner_data->ego_nearest_yaw_threshold); - const size_t traj_resampled_closest = motion_utils::findFirstNearestIndexWithSoftConstraints( - traj_resampled, current_pose, planner_data->ego_nearest_dist_threshold, - planner_data->ego_nearest_yaw_threshold); - std::vector debug_trajectories; - // Clip trajectory from closest point - TrajectoryPoints clipped; - TrajectoryPoints traj_smoothed; - clipped.insert( - clipped.end(), traj_resampled.begin() + traj_resampled_closest, traj_resampled.end()); - if (!smoother->apply(v0, a0, clipped, traj_smoothed, debug_trajectories)) { - std::cerr << "[behavior_velocity][trajectory_utils]: failed to smooth" << std::endl; - return false; - } - traj_smoothed.insert( - traj_smoothed.begin(), traj_resampled.begin(), traj_resampled.begin() + traj_resampled_closest); - - out_path = convertTrajectoryPointsToPath(traj_smoothed); - return true; -} + const std::shared_ptr & planner_data); } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/util.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/util.hpp index 14b009ceb0748..ceba86c27f864 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/util.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/util.hpp @@ -15,34 +15,17 @@ #ifndef BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__UTIL_HPP_ #define BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__UTIL_HPP_ -#include -#include -#include -#include +#include -#include #include -#include -#include #include -#include -#include +#include #include -#include -#include #include -#include - -#include -#include -#include +#include #include -#include -#include -#include -#include -#include +#include #include #include @@ -55,11 +38,6 @@ namespace behavior_velocity_planner { -struct SearchRangeIndex -{ - size_t min_idx; - size_t max_idx; -}; struct DetectionRange { bool use_right = true; @@ -72,81 +50,37 @@ struct DetectionRange double right_overhang; double left_overhang; }; -struct PointWithSearchRangeIndex + +struct TrafficSignalStamped { - geometry_msgs::msg::Point point; - SearchRangeIndex index; + builtin_interfaces::msg::Time stamp; + autoware_perception_msgs::msg::TrafficSignal signal; }; -using geometry_msgs::msg::Pose; +using Pose = geometry_msgs::msg::Pose; +using Point2d = tier4_autoware_utils::Point2d; +using LineString2d = tier4_autoware_utils::LineString2d; +using Polygon2d = tier4_autoware_utils::Polygon2d; using BasicPolygons2d = std::vector; using Polygons2d = std::vector; -using autoware_auto_perception_msgs::msg::PredictedObject; using autoware_auto_perception_msgs::msg::PredictedObjects; -using autoware_auto_perception_msgs::msg::Shape; -using autoware_auto_planning_msgs::msg::Path; -using autoware_auto_planning_msgs::msg::PathPoint; using autoware_auto_planning_msgs::msg::PathPointWithLaneId; using autoware_auto_planning_msgs::msg::PathWithLaneId; -using autoware_auto_planning_msgs::msg::Trajectory; -using motion_utils::calcLongitudinalOffsetToSegment; -using motion_utils::calcSignedArcLength; -using motion_utils::validateNonEmpty; -using tier4_autoware_utils::calcAzimuthAngle; -using tier4_autoware_utils::calcDistance2d; -using tier4_autoware_utils::calcOffsetPose; -using tier4_autoware_utils::calcSquaredDistance2d; -using tier4_autoware_utils::createQuaternionFromYaw; -using tier4_autoware_utils::getPoint; using tier4_planning_msgs::msg::StopFactor; using tier4_planning_msgs::msg::StopReason; namespace planning_utils { -template -size_t calcPointIndexFromSegmentIndex( - const std::vector & points, const geometry_msgs::msg::Point & point, const size_t seg_idx) -{ - const size_t prev_point_idx = seg_idx; - const size_t next_point_idx = seg_idx + 1; - - const double prev_dist = tier4_autoware_utils::calcDistance2d(point, points.at(prev_point_idx)); - const double next_dist = tier4_autoware_utils::calcDistance2d(point, points.at(next_point_idx)); - - if (prev_dist < next_dist) { - return prev_point_idx; - } - return next_point_idx; -} - -template size_t calcSegmentIndexFromPointIndex( - const std::vector & points, const geometry_msgs::msg::Point & point, const size_t idx) -{ - if (idx == 0) { - return 0; - } - if (idx == points.size() - 1) { - return idx - 1; - } - if (points.size() < 3) { - return 0; - } - - const double offset_to_seg = motion_utils::calcLongitudinalOffsetToSegment(points, idx, point); - if (0 < offset_to_seg) { - return idx; - } - return idx - 1; -} - + const std::vector & points, + const geometry_msgs::msg::Point & point, const size_t idx); // create detection area from given range return false if creation failure bool createDetectionAreaPolygons( Polygons2d & da_polys, const PathWithLaneId & path, const geometry_msgs::msg::Pose & target_pose, const size_t target_seg_idx, const DetectionRange & da_range, const double obstacle_vel_mps, const double min_velocity = 1.0); -PathPoint getLerpPathPointWithLaneId(const PathPoint p0, const PathPoint p1, const double ratio); -Point2d calculateOffsetPoint2d(const Pose & pose, const double offset_x, const double offset_y); +Point2d calculateOffsetPoint2d( + const geometry_msgs::msg::Pose & pose, const double offset_x, const double offset_y); void extractClosePartition( const geometry_msgs::msg::Point position, const BasicPolygons2d & all_partitions, BasicPolygons2d & close_partition, const double distance_thresh = 30.0); @@ -160,12 +94,6 @@ inline int64_t bitShift(int64_t original_id) return original_id << (sizeof(int32_t) * 8 / 2); } -geometry_msgs::msg::Pose transformRelCoordinate2D( - const geometry_msgs::msg::Pose & target, const geometry_msgs::msg::Pose & origin); -geometry_msgs::msg::Pose transformAbsCoordinate2D( - const geometry_msgs::msg::Pose & relative, const geometry_msgs::msg::Pose & origin); -SearchRangeIndex getPathIndexRangeIncludeLaneId(const PathWithLaneId & path, const int64_t lane_id); - bool isAheadOf(const geometry_msgs::msg::Pose & target, const geometry_msgs::msg::Pose & origin); geometry_msgs::msg::Pose getAheadPose( const size_t start_idx, const double ahead_dist, diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/velocity_factor_interface.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/velocity_factor_interface.hpp index 28ac40f0868a0..5804c0cdafcf4 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/velocity_factor_interface.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/velocity_factor_interface.hpp @@ -16,13 +16,11 @@ #ifndef BEHAVIOR_VELOCITY_PLANNER_COMMON__VELOCITY_FACTOR_INTERFACE_HPP_ #define BEHAVIOR_VELOCITY_PLANNER_COMMON__VELOCITY_FACTOR_INTERFACE_HPP_ -#include - #include #include +#include #include -#include #include #include @@ -44,19 +42,10 @@ class VelocityFactorInterface void init(const VelocityFactorType type) { type_ = type; } void reset() { velocity_factor_.type = VelocityFactor::UNKNOWN; } - template void set( - const T & points, const Pose & curr_pose, const Pose & stop_pose, - const VelocityFactorStatus status, const std::string detail = "") - { - const auto & curr_point = curr_pose.position; - const auto & stop_point = stop_pose.position; - velocity_factor_.type = type_; - velocity_factor_.pose = stop_pose; - velocity_factor_.distance = motion_utils::calcSignedArcLength(points, curr_point, stop_point); - velocity_factor_.status = status; - velocity_factor_.detail = detail; - } + const std::vector & points, + const Pose & curr_pose, const Pose & stop_pose, const VelocityFactorStatus status, + const std::string detail = ""); private: VelocityFactorType type_; diff --git a/planning/behavior_velocity_planner_common/package.xml b/planning/behavior_velocity_planner_common/package.xml index 6a7a6f698bfbc..aedbab65068fb 100644 --- a/planning/behavior_velocity_planner_common/package.xml +++ b/planning/behavior_velocity_planner_common/package.xml @@ -19,9 +19,9 @@ autoware_adapi_v1_msgs autoware_auto_mapping_msgs - autoware_auto_perception_msgs autoware_auto_planning_msgs autoware_auto_tf2 + autoware_perception_msgs diagnostic_msgs eigen geometry_msgs diff --git a/planning/behavior_velocity_planner_common/src/scene_module_interface.cpp b/planning/behavior_velocity_planner_common/src/scene_module_interface.cpp new file mode 100644 index 0000000000000..6ba982294ccfd --- /dev/null +++ b/planning/behavior_velocity_planner_common/src/scene_module_interface.cpp @@ -0,0 +1,264 @@ +// Copyright 2023 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 +#include +#include +#include +#include + +#include +#include + +namespace behavior_velocity_planner +{ + +using tier4_autoware_utils::StopWatch; + +SceneModuleInterface::SceneModuleInterface( + const int64_t module_id, rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock) +: module_id_(module_id), + activated_(false), + safe_(false), + distance_(std::numeric_limits::lowest()), + logger_(logger), + clock_(clock) +{ +} + +size_t SceneModuleInterface::findEgoSegmentIndex( + const std::vector & points) const +{ + const auto & p = planner_data_; + return motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + points, p->current_odometry->pose, p->ego_nearest_dist_threshold); +} + +SceneModuleManagerInterface::SceneModuleManagerInterface( + rclcpp::Node & node, [[maybe_unused]] const char * module_name) +: node_(node), clock_(node.get_clock()), logger_(node.get_logger()) +{ + const auto ns = std::string("~/debug/") + module_name; + pub_debug_ = node.create_publisher(ns, 1); + if (!node.has_parameter("is_publish_debug_path")) { + is_publish_debug_path_ = node.declare_parameter("is_publish_debug_path"); + } else { + is_publish_debug_path_ = node.get_parameter("is_publish_debug_path").as_bool(); + } + if (is_publish_debug_path_) { + pub_debug_path_ = node.create_publisher( + std::string("~/debug/path_with_lane_id/") + module_name, 1); + } + pub_virtual_wall_ = node.create_publisher( + std::string("~/virtual_wall/") + module_name, 5); + pub_velocity_factor_ = node.create_publisher( + std::string("/planning/velocity_factors/") + module_name, 1); + pub_stop_reason_ = + node.create_publisher("~/output/stop_reasons", 1); + pub_infrastructure_commands_ = + node.create_publisher( + "~/output/infrastructure_commands", 1); + + processing_time_publisher_ = std::make_shared(&node, "~/debug"); +} + +size_t SceneModuleManagerInterface::findEgoSegmentIndex( + const std::vector & points) const +{ + const auto & p = planner_data_; + return motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + points, p->current_odometry->pose, p->ego_nearest_dist_threshold, p->ego_nearest_yaw_threshold); +} + +void SceneModuleManagerInterface::updateSceneModuleInstances( + const std::shared_ptr & planner_data, + const autoware_auto_planning_msgs::msg::PathWithLaneId & path) +{ + planner_data_ = planner_data; + + launchNewModules(path); + deleteExpiredModules(path); +} + +void SceneModuleManagerInterface::modifyPathVelocity( + autoware_auto_planning_msgs::msg::PathWithLaneId * path) +{ + StopWatch stop_watch; + stop_watch.tic("Total"); + visualization_msgs::msg::MarkerArray debug_marker_array; + tier4_planning_msgs::msg::StopReasonArray stop_reason_array; + autoware_adapi_v1_msgs::msg::VelocityFactorArray velocity_factor_array; + stop_reason_array.header.frame_id = "map"; + stop_reason_array.header.stamp = clock_->now(); + velocity_factor_array.header.frame_id = "map"; + velocity_factor_array.header.stamp = clock_->now(); + + tier4_v2x_msgs::msg::InfrastructureCommandArray infrastructure_command_array; + infrastructure_command_array.stamp = clock_->now(); + + first_stop_path_point_index_ = static_cast(path->points.size()) - 1; + for (const auto & scene_module : scene_modules_) { + tier4_planning_msgs::msg::StopReason stop_reason; + scene_module->resetVelocityFactor(); + scene_module->setPlannerData(planner_data_); + scene_module->modifyPathVelocity(path, &stop_reason); + + // The velocity factor must be called after modifyPathVelocity. + const auto velocity_factor = scene_module->getVelocityFactor(); + if (velocity_factor.type != VelocityFactor::UNKNOWN) { + velocity_factor_array.factors.emplace_back(velocity_factor); + } + if (stop_reason.reason != "") { + stop_reason_array.stop_reasons.emplace_back(stop_reason); + } + + if (const auto command = scene_module->getInfrastructureCommand()) { + infrastructure_command_array.commands.push_back(*command); + } + + if (scene_module->getFirstStopPathPointIndex() < first_stop_path_point_index_) { + first_stop_path_point_index_ = scene_module->getFirstStopPathPointIndex(); + } + + for (const auto & marker : scene_module->createDebugMarkerArray().markers) { + debug_marker_array.markers.push_back(marker); + } + + virtual_wall_marker_creator_.add_virtual_walls(scene_module->createVirtualWalls()); + } + + if (!stop_reason_array.stop_reasons.empty()) { + pub_stop_reason_->publish(stop_reason_array); + } + pub_velocity_factor_->publish(velocity_factor_array); + pub_infrastructure_commands_->publish(infrastructure_command_array); + pub_debug_->publish(debug_marker_array); + if (is_publish_debug_path_) { + autoware_auto_planning_msgs::msg::PathWithLaneId debug_path; + debug_path.header = path->header; + debug_path.points = path->points; + pub_debug_path_->publish(debug_path); + } + pub_virtual_wall_->publish(virtual_wall_marker_creator_.create_markers(clock_->now())); + processing_time_publisher_->publish( + std::string(getModuleName()) + "/processing_time_ms", stop_watch.toc("Total")); +} + +void SceneModuleManagerInterface::deleteExpiredModules( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path) +{ + const auto isModuleExpired = getModuleExpiredFunction(path); + + // Copy container to avoid iterator corruption + // due to scene_modules_.erase() in unregisterModule() + const auto copied_scene_modules = scene_modules_; + + for (const auto & scene_module : copied_scene_modules) { + if (isModuleExpired(scene_module)) { + unregisterModule(scene_module); + } + } +} + +void SceneModuleManagerInterface::registerModule( + const std::shared_ptr & scene_module) +{ + RCLCPP_INFO( + logger_, "register task: module = %s, id = %lu", getModuleName(), scene_module->getModuleId()); + registered_module_id_set_.emplace(scene_module->getModuleId()); + scene_modules_.insert(scene_module); +} + +void SceneModuleManagerInterface::unregisterModule( + const std::shared_ptr & scene_module) +{ + RCLCPP_INFO( + logger_, "unregister task: module = %s, id = %lu", getModuleName(), + scene_module->getModuleId()); + registered_module_id_set_.erase(scene_module->getModuleId()); + scene_modules_.erase(scene_module); +} + +SceneModuleManagerInterfaceWithRTC::SceneModuleManagerInterfaceWithRTC( + rclcpp::Node & node, const char * module_name, const bool enable_rtc) +: SceneModuleManagerInterface(node, module_name), rtc_interface_(&node, module_name, enable_rtc) +{ +} + +void SceneModuleManagerInterfaceWithRTC::plan( + autoware_auto_planning_msgs::msg::PathWithLaneId * path) +{ + setActivation(); + modifyPathVelocity(path); + sendRTC(path->header.stamp); +} + +void SceneModuleManagerInterfaceWithRTC::sendRTC(const Time & stamp) +{ + for (const auto & scene_module : scene_modules_) { + const UUID uuid = getUUID(scene_module->getModuleId()); + updateRTCStatus(uuid, scene_module->isSafe(), scene_module->getDistance(), stamp); + } + publishRTCStatus(stamp); +} + +void SceneModuleManagerInterfaceWithRTC::setActivation() +{ + for (const auto & scene_module : scene_modules_) { + const UUID uuid = getUUID(scene_module->getModuleId()); + scene_module->setActivation(rtc_interface_.isActivated(uuid)); + } +} + +UUID SceneModuleManagerInterfaceWithRTC::getUUID(const int64_t & module_id) const +{ + if (map_uuid_.count(module_id) == 0) { + const UUID uuid; + return uuid; + } + return map_uuid_.at(module_id); +} + +void SceneModuleManagerInterfaceWithRTC::generateUUID(const int64_t & module_id) +{ + map_uuid_.insert({module_id, tier4_autoware_utils::generateUUID()}); +} + +void SceneModuleManagerInterfaceWithRTC::removeUUID(const int64_t & module_id) +{ + const auto result = map_uuid_.erase(module_id); + if (result == 0) { + RCLCPP_WARN_STREAM(logger_, "[removeUUID] module_id = " << module_id << " is not registered."); + } +} + +void SceneModuleManagerInterfaceWithRTC::deleteExpiredModules( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path) +{ + const auto isModuleExpired = getModuleExpiredFunction(path); + + // Copy container to avoid iterator corruption + // due to scene_modules_.erase() in unregisterModule() + const auto copied_scene_modules = scene_modules_; + + for (const auto & scene_module : copied_scene_modules) { + if (isModuleExpired(scene_module)) { + removeRTCStatus(getUUID(scene_module->getModuleId())); + removeUUID(scene_module->getModuleId()); + unregisterModule(scene_module); + } + } +} + +} // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner_common/src/utilization/arc_lane_util.cpp b/planning/behavior_velocity_planner_common/src/utilization/arc_lane_util.cpp new file mode 100644 index 0000000000000..05f634a81c5b7 --- /dev/null +++ b/planning/behavior_velocity_planner_common/src/utilization/arc_lane_util.cpp @@ -0,0 +1,138 @@ +// Copyright 2023 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 +#include + +#include + +#ifdef ROS_DISTRO_GALACTIC +#include +#include +#else +#include + +#include +#endif + +#include +#include +#include +#include + +namespace +{ +geometry_msgs::msg::Point operator+( + const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2) +{ + geometry_msgs::msg::Point p; + p.x = p1.x + p2.x; + p.y = p1.y + p2.y; + p.z = p1.z + p2.z; + + return p; +} + +geometry_msgs::msg::Point operator*(const geometry_msgs::msg::Point & p, const double v) +{ + geometry_msgs::msg::Point multiplied_p; + multiplied_p.x = p.x * v; + multiplied_p.y = p.y * v; + multiplied_p.z = p.z * v; + + return multiplied_p; +} + +/* +geometry_msgs::msg::Point operator*(const double v, const geometry_msgs::msg::Point & p) +{ +return p * v; +} +*/ +} // namespace + +namespace behavior_velocity_planner::arc_lane_utils +{ + +double calcSignedDistance(const geometry_msgs::msg::Pose & p1, const geometry_msgs::msg::Point & p2) +{ + Eigen::Affine3d map2p1; + tf2::fromMsg(p1, map2p1); + const auto basecoords_p2 = map2p1.inverse() * Eigen::Vector3d(p2.x, p2.y, p2.z); + return basecoords_p2.x() >= 0 ? basecoords_p2.norm() : -basecoords_p2.norm(); +} + +// calculate one collision point between the line (from p1 to p2) and the line (from p3 to p4) + +boost::optional checkCollision( + const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2, + const geometry_msgs::msg::Point & p3, const geometry_msgs::msg::Point & p4) +{ + const double det = (p2.x - p1.x) * (p4.y - p3.y) - (p2.y - p1.y) * (p4.x - p3.x); + + if (det == 0.0) { + // collision is not one point. + return boost::none; + } + + const double t1 = ((p4.y - p3.y) * (p4.x - p1.x) - (p4.x - p3.x) * (p4.y - p1.y)) / det; + const double t2 = ((p2.x - p1.x) * (p4.y - p1.y) - (p2.y - p1.y) * (p4.x - p1.x)) / det; + + // check collision is outside the segment line + if (t1 < 0.0 || 1.0 < t1 || t2 < 0.0 || 1.0 < t2) { + return boost::none; + } + + return p1 * (1.0 - t1) + p2 * t1; +} + +boost::optional findOffsetSegment( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t index, + const double offset) +{ + if (offset >= 0) { + return findForwardOffsetSegment(path, index, offset); + } + + return findBackwardOffsetSegment(path, index, -offset); +} + +boost::optional createTargetPoint( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const LineString2d & stop_line, + const size_t lane_id, const double margin, const double vehicle_offset) +{ + // Find collision segment + const auto collision_segment = findCollisionSegment(path, stop_line, lane_id); + if (!collision_segment) { + // No collision + return {}; + } + + // Calculate offset length from stop line + // Use '-' to make the positive direction is forward + const double offset_length = -(margin + vehicle_offset); + + // Find offset segment + const auto offset_segment = findOffsetSegment(path, *collision_segment, offset_length); + if (!offset_segment) { + // No enough path length + return {}; + } + + const auto target_pose = calcTargetPose(path, *offset_segment); + + const auto front_idx = offset_segment->first; + return std::make_pair(front_idx, target_pose); +} +} // namespace behavior_velocity_planner::arc_lane_utils diff --git a/planning/behavior_velocity_planner_common/src/utilization/boost_geometry_helper.cpp b/planning/behavior_velocity_planner_common/src/utilization/boost_geometry_helper.cpp new file mode 100644 index 0000000000000..903cf5aab80e8 --- /dev/null +++ b/planning/behavior_velocity_planner_common/src/utilization/boost_geometry_helper.cpp @@ -0,0 +1,66 @@ +// Copyright 2023 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 + +#include + +#include +#include +namespace behavior_velocity_planner +{ + +Polygon2d lines2polygon(const LineString2d & left_line, const LineString2d & right_line) +{ + Polygon2d polygon; + + polygon.outer().push_back(left_line.front()); + + for (auto itr = right_line.begin(); itr != right_line.end(); ++itr) { + polygon.outer().push_back(*itr); + } + + for (auto itr = left_line.rbegin(); itr != left_line.rend(); ++itr) { + polygon.outer().push_back(*itr); + } + + bg::correct(polygon); + return polygon; +} + +Polygon2d upScalePolygon( + const geometry_msgs::msg::Point & position, const Polygon2d & polygon, const double scale) +{ + Polygon2d transformed_polygon; + // upscale + for (size_t i = 0; i < polygon.outer().size(); i++) { + const double upscale_x = (polygon.outer().at(i).x() - position.x) * scale + position.x; + const double upscale_y = (polygon.outer().at(i).y() - position.y) * scale + position.y; + transformed_polygon.outer().emplace_back(Point2d(upscale_x, upscale_y)); + } + return transformed_polygon; +} + +geometry_msgs::msg::Polygon toGeomPoly(const Polygon2d & polygon) +{ + geometry_msgs::msg::Polygon polygon_msg; + geometry_msgs::msg::Point32 point_msg; + for (const auto & p : polygon.outer()) { + point_msg.x = p.x(); + point_msg.y = p.y(); + polygon_msg.points.push_back(point_msg); + } + return polygon_msg; +} +} // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner_common/src/utilization/debug.cpp b/planning/behavior_velocity_planner_common/src/utilization/debug.cpp index dc1eaeacb2f71..6ff2ef53176c4 100644 --- a/planning/behavior_velocity_planner_common/src/utilization/debug.cpp +++ b/planning/behavior_velocity_planner_common/src/utilization/debug.cpp @@ -13,8 +13,8 @@ // limitations under the License. #include -#include - +#include +#include namespace behavior_velocity_planner { namespace debug diff --git a/planning/behavior_velocity_planner_common/src/utilization/path_utilization.cpp b/planning/behavior_velocity_planner_common/src/utilization/path_utilization.cpp index 34d4cf0efa7ac..59c9e4861fbfd 100644 --- a/planning/behavior_velocity_planner_common/src/utilization/path_utilization.cpp +++ b/planning/behavior_velocity_planner_common/src/utilization/path_utilization.cpp @@ -13,20 +13,10 @@ // limitations under the License. #include -#include -#include -#include #include +#include #include -#include - -#ifdef ROS_DISTRO_GALACTIC -#include -#else -#include -#endif - #include #include #include diff --git a/planning/behavior_velocity_planner_common/src/utilization/trajectory_utils.cpp b/planning/behavior_velocity_planner_common/src/utilization/trajectory_utils.cpp new file mode 100644 index 0000000000000..71cdd9650a598 --- /dev/null +++ b/planning/behavior_velocity_planner_common/src/utilization/trajectory_utils.cpp @@ -0,0 +1,121 @@ +// Copyright 2023 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 +#include +#include +#include +#include + +#include +#include + +#include + +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif + +#include + +namespace behavior_velocity_planner +{ +using autoware_auto_planning_msgs::msg::PathPointWithLaneId; +using autoware_auto_planning_msgs::msg::PathWithLaneId; +using autoware_auto_planning_msgs::msg::Trajectory; +using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using TrajectoryPoints = std::vector; +using geometry_msgs::msg::Quaternion; +using TrajectoryPointWithIdx = std::pair; + +TrajectoryPoints convertPathToTrajectoryPoints(const PathWithLaneId & path) +{ + TrajectoryPoints tps; + for (const auto & p : path.points) { + TrajectoryPoint tp; + tp.pose = p.point.pose; + tp.longitudinal_velocity_mps = p.point.longitudinal_velocity_mps; + // since path point doesn't have acc for now + tp.acceleration_mps2 = 0; + tps.emplace_back(tp); + } + return tps; +} +PathWithLaneId convertTrajectoryPointsToPath(const TrajectoryPoints & trajectory) +{ + PathWithLaneId path; + for (const auto & p : trajectory) { + PathPointWithLaneId pp; + pp.point.pose = p.pose; + pp.point.longitudinal_velocity_mps = p.longitudinal_velocity_mps; + path.points.emplace_back(pp); + } + return path; +} + +Quaternion lerpOrientation(const Quaternion & o_from, const Quaternion & o_to, const double ratio) +{ + tf2::Quaternion q_from, q_to; + tf2::fromMsg(o_from, q_from); + tf2::fromMsg(o_to, q_to); + + const auto q_interpolated = q_from.slerp(q_to, ratio); + return tf2::toMsg(q_interpolated); +} + +//! smooth path point with lane id starts from ego position on path to the path end +bool smoothPath( + const PathWithLaneId & in_path, PathWithLaneId & out_path, + const std::shared_ptr & planner_data) +{ + const geometry_msgs::msg::Pose current_pose = planner_data->current_odometry->pose; + const double v0 = planner_data->current_velocity->twist.linear.x; + const double a0 = planner_data->current_acceleration->accel.accel.linear.x; + const auto & external_v_limit = planner_data->external_velocity_limit; + const auto & smoother = planner_data->velocity_smoother_; + + auto trajectory = convertPathToTrajectoryPoints(in_path); + if (external_v_limit) { + motion_velocity_smoother::trajectory_utils::applyMaximumVelocityLimit( + 0, trajectory.size(), external_v_limit->max_velocity, trajectory); + } + const auto traj_lateral_acc_filtered = smoother->applyLateralAccelerationFilter(trajectory); + + // Resample trajectory with ego-velocity based interval distances + auto traj_resampled = smoother->resampleTrajectory( + traj_lateral_acc_filtered, v0, current_pose, planner_data->ego_nearest_dist_threshold, + planner_data->ego_nearest_yaw_threshold); + const size_t traj_resampled_closest = motion_utils::findFirstNearestIndexWithSoftConstraints( + traj_resampled, current_pose, planner_data->ego_nearest_dist_threshold, + planner_data->ego_nearest_yaw_threshold); + std::vector debug_trajectories; + // Clip trajectory from closest point + TrajectoryPoints clipped; + TrajectoryPoints traj_smoothed; + clipped.insert( + clipped.end(), traj_resampled.begin() + traj_resampled_closest, traj_resampled.end()); + if (!smoother->apply(v0, a0, clipped, traj_smoothed, debug_trajectories)) { + std::cerr << "[behavior_velocity][trajectory_utils]: failed to smooth" << std::endl; + return false; + } + traj_smoothed.insert( + traj_smoothed.begin(), traj_resampled.begin(), traj_resampled.begin() + traj_resampled_closest); + + out_path = convertTrajectoryPointsToPath(traj_smoothed); + return true; +} + +} // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner_common/src/utilization/util.cpp b/planning/behavior_velocity_planner_common/src/utilization/util.cpp index d98e9002b2057..a3a5a8a169759 100644 --- a/planning/behavior_velocity_planner_common/src/utilization/util.cpp +++ b/planning/behavior_velocity_planner_common/src/utilization/util.cpp @@ -12,22 +12,50 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include +#include +#include + +#include + +#include + +#include +#include +#include + +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif #include #include #include #include -namespace behavior_velocity_planner +namespace { -namespace planning_utils +size_t calcPointIndexFromSegmentIndex( + const std::vector & points, + const geometry_msgs::msg::Point & point, const size_t seg_idx) { -Point2d calculateOffsetPoint2d(const Pose & pose, const double offset_x, const double offset_y) -{ - return to_bg2d(calcOffsetPose(pose, offset_x, offset_y, 0.0)); + const size_t prev_point_idx = seg_idx; + const size_t next_point_idx = seg_idx + 1; + + const double prev_dist = tier4_autoware_utils::calcDistance2d(point, points.at(prev_point_idx)); + const double next_dist = tier4_autoware_utils::calcDistance2d(point, points.at(next_point_idx)); + + if (prev_dist < next_dist) { + return prev_point_idx; + } + return next_point_idx; } +using autoware_auto_planning_msgs::msg::PathPoint; + PathPoint getLerpPathPointWithLaneId(const PathPoint p0, const PathPoint p1, const double ratio) { auto lerp = [](const double a, const double b, const double t) { return a + t * (b - a); }; @@ -38,6 +66,71 @@ PathPoint getLerpPathPointWithLaneId(const PathPoint p0, const PathPoint p1, con return p; } +geometry_msgs::msg::Pose transformRelCoordinate2D( + const geometry_msgs::msg::Pose & target, const geometry_msgs::msg::Pose & origin) +{ + // translation + geometry_msgs::msg::Point trans_p; + trans_p.x = target.position.x - origin.position.x; + trans_p.y = target.position.y - origin.position.y; + + // rotation (use inverse matrix of rotation) + double yaw = tf2::getYaw(origin.orientation); + + geometry_msgs::msg::Pose res; + res.position.x = (std::cos(yaw) * trans_p.x) + (std::sin(yaw) * trans_p.y); + res.position.y = ((-1.0) * std::sin(yaw) * trans_p.x) + (std::cos(yaw) * trans_p.y); + res.position.z = target.position.z - origin.position.z; + res.orientation = + tier4_autoware_utils::createQuaternionFromYaw(tf2::getYaw(target.orientation) - yaw); + + return res; +} + +} // namespace + +namespace behavior_velocity_planner +{ +namespace planning_utils +{ +using autoware_auto_planning_msgs::msg::PathPoint; +using motion_utils::calcLongitudinalOffsetToSegment; +using motion_utils::calcSignedArcLength; +using motion_utils::validateNonEmpty; +using tier4_autoware_utils::calcAzimuthAngle; +using tier4_autoware_utils::calcDistance2d; +using tier4_autoware_utils::calcOffsetPose; +using tier4_autoware_utils::calcSquaredDistance2d; +using tier4_autoware_utils::createQuaternionFromYaw; +using tier4_autoware_utils::getPoint; + +size_t calcSegmentIndexFromPointIndex( + const std::vector & points, + const geometry_msgs::msg::Point & point, const size_t idx) +{ + if (idx == 0) { + return 0; + } + if (idx == points.size() - 1) { + return idx - 1; + } + if (points.size() < 3) { + return 0; + } + + const double offset_to_seg = motion_utils::calcLongitudinalOffsetToSegment(points, idx, point); + if (0 < offset_to_seg) { + return idx; + } + return idx - 1; +} + +Point2d calculateOffsetPoint2d( + const geometry_msgs::msg::Pose & pose, const double offset_x, const double offset_y) +{ + return to_bg2d(calcOffsetPose(pose, offset_x, offset_y, 0.0)); +} + bool createDetectionAreaPolygons( Polygons2d & da_polys, const PathWithLaneId & path, const geometry_msgs::msg::Pose & target_pose, const size_t target_seg_idx, const DetectionRange & da_range, const double obstacle_vel_mps, @@ -167,30 +260,6 @@ void getAllPartitionLanelets(const lanelet::LaneletMapConstPtr ll, BasicPolygons } } -SearchRangeIndex getPathIndexRangeIncludeLaneId(const PathWithLaneId & path, const int64_t lane_id) -{ - /** - * @brief find path index range include given lane_id - * |<-min_idx |<-max_idx - * ------|oooooooooooooooo|------- - */ - SearchRangeIndex search_range = {0, path.points.size() - 1}; - bool found_first_idx = false; - for (size_t i = 0; i < path.points.size(); i++) { - const auto & p = path.points.at(i); - for (const auto & id : p.lane_ids) { - if (id == lane_id) { - if (!found_first_idx) { - search_range.min_idx = i; - found_first_idx = true; - } - search_range.max_idx = i; - } - } - } - return search_range; -} - void setVelocityFromIndex(const size_t begin_idx, const double vel, PathWithLaneId * input) { for (size_t i = begin_idx; i < input->points.size(); ++i) { @@ -228,7 +297,7 @@ void insertVelocity( bool isAheadOf(const geometry_msgs::msg::Pose & target, const geometry_msgs::msg::Pose & origin) { - geometry_msgs::msg::Pose p = planning_utils::transformRelCoordinate2D(target, origin); + geometry_msgs::msg::Pose p = transformRelCoordinate2D(target, origin); const bool is_target_ahead = (p.position.x > 0.0); return is_target_ahead; } @@ -313,47 +382,6 @@ lanelet::ConstLanelet generatePathLanelet( return lanelet::Lanelet(lanelet::InvalId, left, right); } -geometry_msgs::msg::Pose transformRelCoordinate2D( - const geometry_msgs::msg::Pose & target, const geometry_msgs::msg::Pose & origin) -{ - // translation - geometry_msgs::msg::Point trans_p; - trans_p.x = target.position.x - origin.position.x; - trans_p.y = target.position.y - origin.position.y; - - // rotation (use inverse matrix of rotation) - double yaw = tf2::getYaw(origin.orientation); - - geometry_msgs::msg::Pose res; - res.position.x = (std::cos(yaw) * trans_p.x) + (std::sin(yaw) * trans_p.y); - res.position.y = ((-1.0) * std::sin(yaw) * trans_p.x) + (std::cos(yaw) * trans_p.y); - res.position.z = target.position.z - origin.position.z; - res.orientation = - tier4_autoware_utils::createQuaternionFromYaw(tf2::getYaw(target.orientation) - yaw); - - return res; -} - -geometry_msgs::msg::Pose transformAbsCoordinate2D( - const geometry_msgs::msg::Pose & relative, const geometry_msgs::msg::Pose & origin) -{ - // rotation - geometry_msgs::msg::Point rot_p; - double yaw = tf2::getYaw(origin.orientation); - rot_p.x = (std::cos(yaw) * relative.position.x) + (-std::sin(yaw) * relative.position.y); - rot_p.y = (std::sin(yaw) * relative.position.x) + (std::cos(yaw) * relative.position.y); - - // translation - geometry_msgs::msg::Pose absolute; - absolute.position.x = rot_p.x + origin.position.x; - absolute.position.y = rot_p.y + origin.position.y; - absolute.position.z = relative.position.z + origin.position.z; - absolute.orientation = - tier4_autoware_utils::createQuaternionFromYaw(tf2::getYaw(relative.orientation) + yaw); - - return absolute; -} - double calcJudgeLineDistWithAccLimit( const double velocity, const double max_stop_acceleration, const double delay_response_time) { diff --git a/planning/behavior_velocity_planner_common/src/velocity_factor_interface.cpp b/planning/behavior_velocity_planner_common/src/velocity_factor_interface.cpp new file mode 100644 index 0000000000000..35c4bc00d8096 --- /dev/null +++ b/planning/behavior_velocity_planner_common/src/velocity_factor_interface.cpp @@ -0,0 +1,33 @@ +// Copyright 2023 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 +#include +namespace behavior_velocity_planner +{ +void VelocityFactorInterface::set( + const std::vector & points, + const Pose & curr_pose, const Pose & stop_pose, const VelocityFactorStatus status, + const std::string detail) +{ + const auto & curr_point = curr_pose.position; + const auto & stop_point = stop_pose.position; + velocity_factor_.type = type_; + velocity_factor_.pose = stop_pose; + velocity_factor_.distance = motion_utils::calcSignedArcLength(points, curr_point, stop_point); + velocity_factor_.status = status; + velocity_factor_.detail = detail; +} + +} // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_run_out_module/CMakeLists.txt b/planning/behavior_velocity_run_out_module/CMakeLists.txt index 89745a18065a3..21758c4d0198c 100644 --- a/planning/behavior_velocity_run_out_module/CMakeLists.txt +++ b/planning/behavior_velocity_run_out_module/CMakeLists.txt @@ -12,6 +12,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED src/scene.cpp src/state_machine.cpp src/utils.cpp + src/path_utils.cpp ) ament_auto_package(INSTALL_TO_SHARE config) diff --git a/planning/behavior_velocity_run_out_module/README.md b/planning/behavior_velocity_run_out_module/README.md index ee762653245c6..a36cfdf6485c6 100644 --- a/planning/behavior_velocity_run_out_module/README.md +++ b/planning/behavior_velocity_run_out_module/README.md @@ -217,6 +217,11 @@ You can choose whether to use this feature by parameter of `slow_down_limit.enab | `max_jerk` | double | [m/s^3] minimum jerk deceleration for safe brake. | | `max_acc` | double | [m/s^2] minimum accel deceleration for safe brake. | +| Parameter /ignore_momentary_detection | Type | Description | +| ------------------------------------- | ------ | ----------------------------------------------------------------- | +| `enable` | bool | [-] whether to ignore momentary detection | +| `time_threshold` | double | [sec] ignores detections that persist for less than this duration | + ### Future extensions / Unimplemented parts - Calculate obstacle's min velocity and max velocity from covariance diff --git a/planning/behavior_velocity_run_out_module/config/run_out.param.yaml b/planning/behavior_velocity_run_out_module/config/run_out.param.yaml index f9668549f2fb2..2641214ac5ad4 100644 --- a/planning/behavior_velocity_run_out_module/config/run_out.param.yaml +++ b/planning/behavior_velocity_run_out_module/config/run_out.param.yaml @@ -48,3 +48,8 @@ enable: true max_jerk: -0.7 # [m/s^3] minimum jerk deceleration for safe brake. max_acc : -2.0 # [m/s^2] minimum accel deceleration for safe brake. + + # prevent abrupt stops caused by false positives in perception + ignore_momentary_detection: + enable: true + time_threshold: 0.15 # [sec] ignores detections that persist for less than this duration diff --git a/planning/behavior_velocity_run_out_module/src/debug.cpp b/planning/behavior_velocity_run_out_module/src/debug.cpp index 7fa17b0cea52a..c0d026b5ffaf4 100644 --- a/planning/behavior_velocity_run_out_module/src/debug.cpp +++ b/planning/behavior_velocity_run_out_module/src/debug.cpp @@ -16,7 +16,9 @@ #include "scene.hpp" -#include +#include +#include +#include using tier4_autoware_utils::appendMarkerArray; using tier4_autoware_utils::calcOffsetPose; @@ -74,7 +76,6 @@ RunOutDebug::RunOutDebug(rclcpp::Node & node) : node_(node) pub_debug_values_ = node.create_publisher("~/debug/run_out/debug_values", 1); pub_accel_reason_ = node.create_publisher("~/debug/run_out/accel_reason", 1); - pub_debug_trajectory_ = node.create_publisher("~/debug/run_out/trajectory", 1); pub_debug_pointcloud_ = node.create_publisher( "~/debug/run_out/filtered_pointcloud", rclcpp::SensorDataQoS().keep_last(1)); } @@ -304,11 +305,6 @@ void RunOutDebug::publishDebugValue() pub_accel_reason_->publish(accel_reason); } -void RunOutDebug::publishDebugTrajectory(const Trajectory & trajectory) -{ - pub_debug_trajectory_->publish(trajectory); -} - void RunOutDebug::publishFilteredPointCloud( const pcl::PointCloud & pointcloud, const std_msgs::msg::Header header) { diff --git a/planning/behavior_velocity_run_out_module/src/debug.hpp b/planning/behavior_velocity_run_out_module/src/debug.hpp index 273abb10c780a..91656d542ea8e 100644 --- a/planning/behavior_velocity_run_out_module/src/debug.hpp +++ b/planning/behavior_velocity_run_out_module/src/debug.hpp @@ -18,12 +18,14 @@ #include +#include +#include + #include #include #include namespace behavior_velocity_planner { -using autoware_auto_planning_msgs::msg::Trajectory; using sensor_msgs::msg::PointCloud2; using tier4_debug_msgs::msg::Float32MultiArrayStamped; using tier4_debug_msgs::msg::Int32Stamped; @@ -110,7 +112,6 @@ class RunOutDebug const double travel_time, const geometry_msgs::msg::Pose pose, const float lateral_offset); void setAccelReason(const AccelReason & accel_reason); void publishDebugValue(); - void publishDebugTrajectory(const Trajectory & trajectory); void publishFilteredPointCloud(const PointCloud2 & pointcloud); void publishFilteredPointCloud( const pcl::PointCloud & pointcloud, const std_msgs::msg::Header header); @@ -127,7 +128,6 @@ class RunOutDebug rclcpp::Node & node_; rclcpp::Publisher::SharedPtr pub_debug_values_; rclcpp::Publisher::SharedPtr pub_accel_reason_; - rclcpp::Publisher::SharedPtr pub_debug_trajectory_; rclcpp::Publisher::SharedPtr pub_debug_pointcloud_; std::vector collision_points_; std::vector nearest_collision_point_; diff --git a/planning/behavior_velocity_run_out_module/src/dynamic_obstacle.cpp b/planning/behavior_velocity_run_out_module/src/dynamic_obstacle.cpp index e2f9880435d5e..16013efc5fd97 100644 --- a/planning/behavior_velocity_run_out_module/src/dynamic_obstacle.cpp +++ b/planning/behavior_velocity_run_out_module/src/dynamic_obstacle.cpp @@ -14,6 +14,15 @@ #include "dynamic_obstacle.hpp" +#include +#include +#include +#include +#include + +#include +#include + #include #include diff --git a/planning/behavior_velocity_run_out_module/src/dynamic_obstacle.hpp b/planning/behavior_velocity_run_out_module/src/dynamic_obstacle.hpp index f9b2b12298d2d..5217193c9673e 100644 --- a/planning/behavior_velocity_run_out_module/src/dynamic_obstacle.hpp +++ b/planning/behavior_velocity_run_out_module/src/dynamic_obstacle.hpp @@ -21,7 +21,6 @@ #include #include #include -#include #include @@ -37,6 +36,8 @@ #include #endif +#include + #include #include diff --git a/planning/behavior_velocity_run_out_module/src/manager.cpp b/planning/behavior_velocity_run_out_module/src/manager.cpp index 8887f0749a2d5..a5253f59b60f9 100644 --- a/planning/behavior_velocity_run_out_module/src/manager.cpp +++ b/planning/behavior_velocity_run_out_module/src/manager.cpp @@ -14,14 +14,14 @@ #include "manager.hpp" +#include + #include #include namespace behavior_velocity_planner { -namespace -{ -} // namespace +using tier4_autoware_utils::getOrDeclareParameter; RunOutModuleManager::RunOutModuleManager(rclcpp::Node & node) : SceneModuleManagerInterface(node, getModuleName()) @@ -48,82 +48,82 @@ RunOutModuleManager::RunOutModuleManager(rclcpp::Node & node) { auto & p = planner_param_.common; - p.normal_min_jerk = node.has_parameter("normal.min_jerk") - ? node.get_parameter("normal.min_jerk").get_value() - : node.declare_parameter("normal.min_jerk"); - p.normal_min_acc = node.has_parameter("normal.min_acc") - ? node.get_parameter("normal.min_acc").get_value() - : node.declare_parameter("normal.min_acc"); - p.limit_min_jerk = node.has_parameter("limit.min_jerk") - ? node.get_parameter("limit.min_jerk").get_value() - : node.declare_parameter("limit.min_jerk"); - p.limit_min_acc = node.has_parameter("limit.min_acc") - ? node.get_parameter("limit.min_acc").get_value() - : node.declare_parameter("limit.min_acc"); + p.normal_min_jerk = getOrDeclareParameter(node, "normal.min_jerk"); + p.normal_min_acc = getOrDeclareParameter(node, "normal.min_acc"); + p.limit_min_jerk = getOrDeclareParameter(node, "limit.min_jerk"); + p.limit_min_acc = getOrDeclareParameter(node, "limit.min_acc"); } { auto & p = planner_param_.run_out; - p.detection_method = node.declare_parameter(ns + ".detection_method"); - p.use_partition_lanelet = node.declare_parameter(ns + ".use_partition_lanelet"); - p.specify_decel_jerk = node.declare_parameter(ns + ".specify_decel_jerk"); - p.stop_margin = node.declare_parameter(ns + ".stop_margin"); - p.passing_margin = node.declare_parameter(ns + ".passing_margin"); - p.deceleration_jerk = node.declare_parameter(ns + ".deceleration_jerk"); - p.detection_distance = node.declare_parameter(ns + ".detection_distance"); - p.detection_span = node.declare_parameter(ns + ".detection_span"); - p.min_vel_ego_kmph = node.declare_parameter(ns + ".min_vel_ego_kmph"); + p.detection_method = getOrDeclareParameter(node, ns + ".detection_method"); + p.use_partition_lanelet = getOrDeclareParameter(node, ns + ".use_partition_lanelet"); + p.specify_decel_jerk = getOrDeclareParameter(node, ns + ".specify_decel_jerk"); + p.stop_margin = getOrDeclareParameter(node, ns + ".stop_margin"); + p.passing_margin = getOrDeclareParameter(node, ns + ".passing_margin"); + p.deceleration_jerk = getOrDeclareParameter(node, ns + ".deceleration_jerk"); + p.detection_distance = getOrDeclareParameter(node, ns + ".detection_distance"); + p.detection_span = getOrDeclareParameter(node, ns + ".detection_span"); + p.min_vel_ego_kmph = getOrDeclareParameter(node, ns + ".min_vel_ego_kmph"); } { auto & p = planner_param_.detection_area; const std::string ns_da = ns + ".detection_area"; - p.margin_ahead = node.declare_parameter(ns_da + ".margin_ahead"); - p.margin_behind = node.declare_parameter(ns_da + ".margin_behind"); + p.margin_ahead = getOrDeclareParameter(node, ns_da + ".margin_ahead"); + p.margin_behind = getOrDeclareParameter(node, ns_da + ".margin_behind"); } { auto & p = planner_param_.mandatory_area; const std::string ns_da = ns + ".mandatory_area"; - p.decel_jerk = node.declare_parameter(ns_da + ".decel_jerk"); + p.decel_jerk = getOrDeclareParameter(node, ns_da + ".decel_jerk"); } { auto & p = planner_param_.dynamic_obstacle; const std::string ns_do = ns + ".dynamic_obstacle"; - p.use_mandatory_area = node.declare_parameter(ns_do + ".use_mandatory_area"); - p.min_vel_kmph = node.declare_parameter(ns_do + ".min_vel_kmph"); - p.max_vel_kmph = node.declare_parameter(ns_do + ".max_vel_kmph"); - p.diameter = node.declare_parameter(ns_do + ".diameter"); - p.height = node.declare_parameter(ns_do + ".height"); - p.max_prediction_time = node.declare_parameter(ns_do + ".max_prediction_time"); - p.time_step = node.declare_parameter(ns_do + ".time_step"); - p.points_interval = node.declare_parameter(ns_do + ".points_interval"); + p.use_mandatory_area = getOrDeclareParameter(node, ns_do + ".use_mandatory_area"); + p.min_vel_kmph = getOrDeclareParameter(node, ns_do + ".min_vel_kmph"); + p.max_vel_kmph = getOrDeclareParameter(node, ns_do + ".max_vel_kmph"); + p.diameter = getOrDeclareParameter(node, ns_do + ".diameter"); + p.height = getOrDeclareParameter(node, ns_do + ".height"); + p.max_prediction_time = getOrDeclareParameter(node, ns_do + ".max_prediction_time"); + p.time_step = getOrDeclareParameter(node, ns_do + ".time_step"); + p.points_interval = getOrDeclareParameter(node, ns_do + ".points_interval"); } { auto & p = planner_param_.approaching; const std::string ns_a = ns + ".approaching"; - p.enable = node.declare_parameter(ns_a + ".enable"); - p.margin = node.declare_parameter(ns_a + ".margin"); - p.limit_vel_kmph = node.declare_parameter(ns_a + ".limit_vel_kmph"); + p.enable = getOrDeclareParameter(node, ns_a + ".enable"); + p.margin = getOrDeclareParameter(node, ns_a + ".margin"); + p.limit_vel_kmph = getOrDeclareParameter(node, ns_a + ".limit_vel_kmph"); } { auto & p = planner_param_.state_param; const std::string ns_s = ns + ".state"; - p.stop_thresh = node.declare_parameter(ns_s + ".stop_thresh"); - p.stop_time_thresh = node.declare_parameter(ns_s + ".stop_time_thresh"); - p.disable_approach_dist = node.declare_parameter(ns_s + ".disable_approach_dist"); - p.keep_approach_duration = node.declare_parameter(ns_s + ".keep_approach_duration"); + p.stop_thresh = getOrDeclareParameter(node, ns_s + ".stop_thresh"); + p.stop_time_thresh = getOrDeclareParameter(node, ns_s + ".stop_time_thresh"); + p.disable_approach_dist = getOrDeclareParameter(node, ns_s + ".disable_approach_dist"); + p.keep_approach_duration = + getOrDeclareParameter(node, ns_s + ".keep_approach_duration"); } { auto & p = planner_param_.slow_down_limit; const std::string ns_m = ns + ".slow_down_limit"; - p.enable = node.declare_parameter(ns_m + ".enable"); - p.max_jerk = node.declare_parameter(ns_m + ".max_jerk"); - p.max_acc = node.declare_parameter(ns_m + ".max_acc"); + p.enable = getOrDeclareParameter(node, ns_m + ".enable"); + p.max_jerk = getOrDeclareParameter(node, ns_m + ".max_jerk"); + p.max_acc = getOrDeclareParameter(node, ns_m + ".max_acc"); + } + + { + auto & p = planner_param_.ignore_momentary_detection; + const std::string ns_param = ns + ".ignore_momentary_detection"; + p.enable = getOrDeclareParameter(node, ns_param + ".enable"); + p.time_threshold = getOrDeclareParameter(node, ns_param + ".time_threshold"); } debug_ptr_ = std::make_shared(node); diff --git a/planning/behavior_velocity_run_out_module/src/path_utils.cpp b/planning/behavior_velocity_run_out_module/src/path_utils.cpp new file mode 100644 index 0000000000000..1cec20297683c --- /dev/null +++ b/planning/behavior_velocity_run_out_module/src/path_utils.cpp @@ -0,0 +1,39 @@ +// Copyright 2023 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 "path_utils.hpp" + +#include +namespace behavior_velocity_planner::run_out_utils +{ +geometry_msgs::msg::Point findLongitudinalNearestPoint( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, + const std::vector & target_points) +{ + float min_dist = std::numeric_limits::max(); + geometry_msgs::msg::Point min_dist_point{}; + + for (const auto & p : target_points) { + const float dist = motion_utils::calcSignedArcLength(points, src_point, p); + if (dist < min_dist) { + min_dist = dist; + min_dist_point = p; + } + } + + return min_dist_point; +} + +} // namespace behavior_velocity_planner::run_out_utils diff --git a/planning/behavior_velocity_run_out_module/src/path_utils.hpp b/planning/behavior_velocity_run_out_module/src/path_utils.hpp index b01ee70f8bdfa..92aca946c13ef 100644 --- a/planning/behavior_velocity_run_out_module/src/path_utils.hpp +++ b/planning/behavior_velocity_run_out_module/src/path_utils.hpp @@ -15,7 +15,8 @@ #ifndef PATH_UTILS_HPP_ #define PATH_UTILS_HPP_ -#include +#include +#include #include #include @@ -29,24 +30,10 @@ namespace behavior_velocity_planner namespace run_out_utils { -template geometry_msgs::msg::Point findLongitudinalNearestPoint( - const T & points, const geometry_msgs::msg::Point & src_point, - const std::vector & target_points) -{ - float min_dist = std::numeric_limits::max(); - geometry_msgs::msg::Point min_dist_point{}; - - for (const auto & p : target_points) { - const float dist = motion_utils::calcSignedArcLength(points, src_point, p); - if (dist < min_dist) { - min_dist = dist; - min_dist_point = p; - } - } - - return min_dist_point; -} + const std::vector & points, + const geometry_msgs::msg::Point & src_point, + const std::vector & target_points); } // namespace run_out_utils } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_run_out_module/src/scene.cpp b/planning/behavior_velocity_run_out_module/src/scene.cpp index 3b2ed6a78a96c..54902f52c47da 100644 --- a/planning/behavior_velocity_run_out_module/src/scene.cpp +++ b/planning/behavior_velocity_run_out_module/src/scene.cpp @@ -18,6 +18,11 @@ #include #include +#include +#include +#include + +#include #include #include @@ -132,7 +137,7 @@ bool RunOutModule::modifyPathVelocity( } boost::optional RunOutModule::detectCollision( - const std::vector & dynamic_obstacles, const PathWithLaneId & path) const + const std::vector & dynamic_obstacles, const PathWithLaneId & path) { if (path.points.size() < 2) { RCLCPP_WARN_STREAM(logger_, "path doesn't have enough points."); @@ -176,6 +181,11 @@ boost::optional RunOutModule::detectCollision( continue; } + // ignore momentary obstacle detection to avoid sudden stopping by false positive + if (isMomentaryDetection()) { + return {}; + } + debug_ptr_->pushCollisionPoints(obstacle_selected->collision_points); debug_ptr_->pushNearestCollisionPoint(obstacle_selected->nearest_collision_point); @@ -183,6 +193,7 @@ boost::optional RunOutModule::detectCollision( } // no collision detected + first_detected_time_.reset(); return {}; } @@ -807,4 +818,21 @@ void RunOutModule::publishDebugValue( debug_ptr_->publishDebugValue(); } +bool RunOutModule::isMomentaryDetection() +{ + if (!planner_param_.ignore_momentary_detection.enable) { + return false; + } + + if (!first_detected_time_) { + first_detected_time_ = std::make_shared(clock_->now()); + return true; + } + + const auto now = clock_->now(); + const double elapsed_time_since_detection = (now - *first_detected_time_).seconds(); + RCLCPP_DEBUG_STREAM(logger_, "elapsed_time_since_detection: " << elapsed_time_since_detection); + + return elapsed_time_since_detection < planner_param_.ignore_momentary_detection.time_threshold; +} } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_run_out_module/src/scene.hpp b/planning/behavior_velocity_run_out_module/src/scene.hpp index 20bd8ffc799ff..925d8ea8b234c 100644 --- a/planning/behavior_velocity_run_out_module/src/scene.hpp +++ b/planning/behavior_velocity_run_out_module/src/scene.hpp @@ -62,13 +62,13 @@ class RunOutModule : public SceneModuleInterface std::unique_ptr dynamic_obstacle_creator_; std::shared_ptr debug_ptr_; std::unique_ptr state_machine_; + std::shared_ptr first_detected_time_; // Function Polygons2d createDetectionAreaPolygon(const PathWithLaneId & smoothed_path) const; boost::optional detectCollision( - const std::vector & dynamic_obstacles, - const PathWithLaneId & path_points) const; + const std::vector & dynamic_obstacles, const PathWithLaneId & path_points); float calcCollisionPositionOfVehicleSide( const geometry_msgs::msg::Point & point, const geometry_msgs::msg::Pose & base_pose) const; @@ -141,6 +141,8 @@ class RunOutModule : public SceneModuleInterface const PathWithLaneId & path, const std::vector extracted_obstacles, const boost::optional & dynamic_obstacle, const geometry_msgs::msg::Pose & current_pose) const; + + bool isMomentaryDetection(); }; } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_run_out_module/src/utils.cpp b/planning/behavior_velocity_run_out_module/src/utils.cpp index 3def2ade46731..61396da250c8d 100644 --- a/planning/behavior_velocity_run_out_module/src/utils.cpp +++ b/planning/behavior_velocity_run_out_module/src/utils.cpp @@ -14,9 +14,25 @@ #include "utils.hpp" +#include +#include + +#include +#include +#include + +#include + #include #include +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif +#include +#include namespace behavior_velocity_planner { namespace run_out_utils diff --git a/planning/behavior_velocity_run_out_module/src/utils.hpp b/planning/behavior_velocity_run_out_module/src/utils.hpp index 1f725291a38b7..b25a8fc94bff3 100644 --- a/planning/behavior_velocity_run_out_module/src/utils.hpp +++ b/planning/behavior_velocity_run_out_module/src/utils.hpp @@ -17,9 +17,12 @@ #include #include -#include #include +#include +#include +#include + #include #include @@ -30,6 +33,8 @@ namespace run_out_utils namespace bg = boost::geometry; using autoware_auto_perception_msgs::msg::ObjectClassification; using autoware_auto_perception_msgs::msg::PredictedObjects; +using autoware_auto_perception_msgs::msg::Shape; +using autoware_auto_planning_msgs::msg::PathPoint; using autoware_auto_planning_msgs::msg::PathWithLaneId; using tier4_autoware_utils::Box2d; using tier4_autoware_utils::LineString2d; @@ -121,6 +126,12 @@ struct DynamicObstacleParam float points_interval; // [m] }; +struct IgnoreMomentaryDetection +{ + bool enable; + double time_threshold; +}; + struct PlannerParam { CommonParam common; @@ -133,6 +144,7 @@ struct PlannerParam DynamicObstacleParam dynamic_obstacle; SlowDownLimit slow_down_limit; Smoother smoother; + IgnoreMomentaryDetection ignore_momentary_detection; }; enum class DetectionMethod { diff --git a/planning/behavior_velocity_speed_bump_module/src/debug.cpp b/planning/behavior_velocity_speed_bump_module/src/debug.cpp index 62e8cd31ec062..7409c24e7e8bf 100644 --- a/planning/behavior_velocity_speed_bump_module/src/debug.cpp +++ b/planning/behavior_velocity_speed_bump_module/src/debug.cpp @@ -15,8 +15,11 @@ #include "scene.hpp" #include -#include -#include +#include +#include +#include +#include +#include #include diff --git a/planning/behavior_velocity_speed_bump_module/src/manager.cpp b/planning/behavior_velocity_speed_bump_module/src/manager.cpp index e502963e60b47..5949357c90ff3 100644 --- a/planning/behavior_velocity_speed_bump_module/src/manager.cpp +++ b/planning/behavior_velocity_speed_bump_module/src/manager.cpp @@ -14,7 +14,10 @@ #include "manager.hpp" +#include +#include #include +#include #include @@ -28,25 +31,26 @@ namespace behavior_velocity_planner { using lanelet::autoware::SpeedBump; +using tier4_autoware_utils::getOrDeclareParameter; SpeedBumpModuleManager::SpeedBumpModuleManager(rclcpp::Node & node) : SceneModuleManagerInterface(node, getModuleName()) { std::string ns(getModuleName()); - planner_param_.slow_start_margin = node.declare_parameter(ns + ".slow_start_margin"); - planner_param_.slow_end_margin = node.declare_parameter(ns + ".slow_end_margin"); - planner_param_.print_debug_info = node.declare_parameter(ns + ".print_debug_info"); + planner_param_.slow_start_margin = getOrDeclareParameter(node, ns + ".slow_start_margin"); + planner_param_.slow_end_margin = getOrDeclareParameter(node, ns + ".slow_end_margin"); + planner_param_.print_debug_info = getOrDeclareParameter(node, ns + ".print_debug_info"); // limits for speed bump height and slow down speed ns += ".speed_calculation"; planner_param_.speed_calculation_min_height = - static_cast(node.declare_parameter(ns + ".min_height")); + static_cast(getOrDeclareParameter(node, ns + ".min_height")); planner_param_.speed_calculation_max_height = - static_cast(node.declare_parameter(ns + ".max_height")); + static_cast(getOrDeclareParameter(node, ns + ".max_height")); planner_param_.speed_calculation_min_speed = - static_cast(node.declare_parameter(ns + ".min_speed")); + static_cast(getOrDeclareParameter(node, ns + ".min_speed")); planner_param_.speed_calculation_max_speed = - static_cast(node.declare_parameter(ns + ".max_speed")); + static_cast(getOrDeclareParameter(node, ns + ".max_speed")); } void SpeedBumpModuleManager::launchNewModules( diff --git a/planning/behavior_velocity_speed_bump_module/src/scene.cpp b/planning/behavior_velocity_speed_bump_module/src/scene.cpp index fda92f037cf7a..dc944cc0c292b 100644 --- a/planning/behavior_velocity_speed_bump_module/src/scene.cpp +++ b/planning/behavior_velocity_speed_bump_module/src/scene.cpp @@ -14,8 +14,8 @@ #include "scene.hpp" -#include "motion_utils/motion_utils.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" +#include "motion_utils/trajectory/trajectory.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" #include "util.hpp" #include diff --git a/planning/behavior_velocity_speed_bump_module/src/scene.hpp b/planning/behavior_velocity_speed_bump_module/src/scene.hpp index 1a797adf2cd61..34de722055d3d 100644 --- a/planning/behavior_velocity_speed_bump_module/src/scene.hpp +++ b/planning/behavior_velocity_speed_bump_module/src/scene.hpp @@ -18,6 +18,7 @@ #include "util.hpp" #include +#include #include #include diff --git a/planning/behavior_velocity_speed_bump_module/src/util.cpp b/planning/behavior_velocity_speed_bump_module/src/util.cpp index 89845a4b5e271..21b283a397ead 100644 --- a/planning/behavior_velocity_speed_bump_module/src/util.cpp +++ b/planning/behavior_velocity_speed_bump_module/src/util.cpp @@ -14,15 +14,18 @@ #include "util.hpp" -#include "motion_utils/motion_utils.hpp" +#include "motion_utils/trajectory/path_with_lane_id.hpp" +#include "motion_utils/trajectory/trajectory.hpp" #include -#include +#include #include #include #include +#include + #include #include #include diff --git a/planning/behavior_velocity_stop_line_module/src/debug.cpp b/planning/behavior_velocity_stop_line_module/src/debug.cpp index 09949751f4e93..f63bdca5068a2 100644 --- a/planning/behavior_velocity_stop_line_module/src/debug.cpp +++ b/planning/behavior_velocity_stop_line_module/src/debug.cpp @@ -15,8 +15,9 @@ #include "scene.hpp" #include -#include - +#include +#include +#include #ifdef ROS_DISTRO_GALACTIC #include #else diff --git a/planning/behavior_velocity_stop_line_module/src/manager.cpp b/planning/behavior_velocity_stop_line_module/src/manager.cpp index 390550fc9e563..5c9f0fd39c644 100644 --- a/planning/behavior_velocity_stop_line_module/src/manager.cpp +++ b/planning/behavior_velocity_stop_line_module/src/manager.cpp @@ -14,6 +14,8 @@ #include "manager.hpp" +#include "tier4_autoware_utils/ros/parameter.hpp" + #include #include #include @@ -22,20 +24,22 @@ namespace behavior_velocity_planner { using lanelet::TrafficSign; +using tier4_autoware_utils::getOrDeclareParameter; StopLineModuleManager::StopLineModuleManager(rclcpp::Node & node) : SceneModuleManagerInterface(node, getModuleName()) { const std::string ns(getModuleName()); auto & p = planner_param_; - p.stop_margin = node.declare_parameter(ns + ".stop_margin"); - p.hold_stop_margin_distance = node.declare_parameter(ns + ".hold_stop_margin_distance"); - p.stop_duration_sec = node.declare_parameter(ns + ".stop_duration_sec"); + p.stop_margin = getOrDeclareParameter(node, ns + ".stop_margin"); + p.hold_stop_margin_distance = + getOrDeclareParameter(node, ns + ".hold_stop_margin_distance"); + p.stop_duration_sec = getOrDeclareParameter(node, ns + ".stop_duration_sec"); p.use_initialization_stop_line_state = - node.declare_parameter(ns + ".use_initialization_stop_line_state"); + getOrDeclareParameter(node, ns + ".use_initialization_stop_line_state"); // debug p.show_stop_line_collision_check = - node.declare_parameter(ns + ".debug.show_stop_line_collision_check"); + getOrDeclareParameter(node, ns + ".debug.show_stop_line_collision_check"); } std::vector StopLineModuleManager::getStopLinesWithLaneIdOnPath( diff --git a/planning/behavior_velocity_template_module/CMakeLists.txt b/planning/behavior_velocity_template_module/CMakeLists.txt new file mode 100644 index 0000000000000..e0beb42daf50d --- /dev/null +++ b/planning/behavior_velocity_template_module/CMakeLists.txt @@ -0,0 +1,13 @@ +cmake_minimum_required(VERSION 3.14) +project(behavior_velocity_template_module) + +find_package(autoware_cmake REQUIRED) +autoware_package() +pluginlib_export_plugin_description_file(behavior_velocity_planner plugins.xml) + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/manager.cpp + src/scene.cpp +) + +ament_auto_package(INSTALL_TO_SHARE config) diff --git a/planning/behavior_velocity_template_module/README.md b/planning/behavior_velocity_template_module/README.md new file mode 100644 index 0000000000000..1995b3e040061 --- /dev/null +++ b/planning/behavior_velocity_template_module/README.md @@ -0,0 +1,85 @@ +## Template + +A template for behavior velocity modules based on the behavior_velocity_speed_bump_module. + +# Autoware Behavior Velocity Module Template + +## `Scene` + +### `TemplateModule` Class + +The `TemplateModule` class serves as a foundation for creating a scene module within the Autoware behavior velocity planner. It defines the core methods and functionality needed for the module's behavior. You should replace the placeholder code with actual implementations tailored to your specific behavior velocity module. + +#### Constructor + +- The constructor for `TemplateModule` takes the essential parameters to create a module: `const int64_t module_id`, `const rclcpp::Logger & logger`, and `const rclcpp::Clock::SharedPtr clock`. These parameters are supplied by the `TemplateModuleManager` when registering a new module. Other parameters can be added to the constructor, if required by your specific module implementation. + +#### `modifyPathVelocity` Method + +- This method, defined in the `TemplateModule` class, is expected to modify the velocity of the input path based on certain conditions. In the provided code, it logs an informational message once when the template module is executing. +- The specific logic for velocity modification should be implemented in this method based on the module's requirements. + +#### `createDebugMarkerArray` Method + +- This method, also defined in the `TemplateModule` class, is responsible for creating a visualization of debug markers and returning them as a `visualization_msgs::msg::MarkerArray`. In the provided code, it returns an empty `MarkerArray`. +- You should implement the logic to generate debug markers specific to your module's functionality. + +#### `createVirtualWalls` Method + +- The `createVirtualWalls` method creates virtual walls for the scene and returns them as `motion_utils::VirtualWalls`. In the provided code, it returns an empty `VirtualWalls` object. +- You should implement the logic to create virtual walls based on your module's requirements. + +## `Manager` + +The managing of your modules is defined in manager.hpp and manager.cpp. The managing is handled by two classes: + +- The `TemplateModuleManager` class defines the core logic for managing and launching the behavior_velocity_template scenes (defined in behavior_velocity_template_module/src/scene.cpp/hpp). It inherits essential manager attributes from its parent class `SceneModuleManagerInterface`. +- The `TemplateModulePlugin` class provides a way to integrate the `TemplateModuleManager` into the logic of the Behavior Velocity Planner. + +### `TemplateModuleManager` Class + +#### Constructor `TemplateModuleManager` + +- This is the constructor of the `TemplateModuleManager` class, and it takes an `rclcpp::Node` reference as a parameter. +- It initializes a member variable `dummy_parameter` to 0.0. + +#### `getModuleName()` Method + +- This method is an override of a virtual method from the `SceneModuleManagerInterface` class. +- It returns a pointer to a constant character string, which is the name of the module. In this case, it returns "template" as the module name. + +#### `launchNewModules()` Method + +- This is a private method that takes an argument of type `autoware_auto_planning_msgs::msg::PathWithLaneId`. +- It is responsible for launching new modules based on the provided path information (PathWithLaneId). The implementation of this method involves initializing and configuring modules specific to your behavior velocity planner by using the `TemplateModule` class. +- In the provided source code, it initializes a `module_id` to 0 and checks if a module with the same ID is already registered. If not, it registers a new `TemplateModule` with the module ID. Note that each module managed by the `TemplateModuleManager` should have a unique ID. The template code registers a single module, so the `module_id` is set as 0 for simplicity. + +#### `getModuleExpiredFunction()` Method + +- This is a private method that takes an argument of type `autoware_auto_planning_msgs::msg::PathWithLaneId`. +- It returns a `std::function&)>`. This function is used by the behavior velocity planner to determine whether a particular module has expired or not based on the given path. +- The implementation of this method is expected to return a function that can be used to check the expiration status of modules. + +Please note that the specific functionality of the methods `launchNewModules()` and `getModuleExpiredFunction()` would depend on the details of your behavior velocity modules and how they are intended to be managed within the Autoware system. You would need to implement these methods according to your module's requirements. + +### `TemplateModulePlugin` Class + +#### `TemplateModulePlugin` Class + +- This class inherits from `PluginWrapper`. It essentially wraps your `TemplateModuleManager` class within a plugin, which can be loaded and managed dynamically. + +## `Example Usage` + +In the following example, we take each point of the path, and multiply it by 2. Essentially duplicating the speed. Note that the velocity smoother will further modify the path speed after all the behavior velocity modules are executed. + +```cpp +bool TemplateModule::modifyPathVelocity( + [[maybe_unused]] PathWithLaneId * path, [[maybe_unused]] StopReason * stop_reason) +{ + for (auto & p : path->points) { + p.point.longitudinal_velocity_mps *= 2.0; + } + + return false; +} +``` diff --git a/planning/behavior_velocity_template_module/config/template.param.yaml b/planning/behavior_velocity_template_module/config/template.param.yaml new file mode 100644 index 0000000000000..7045eb28e98a2 --- /dev/null +++ b/planning/behavior_velocity_template_module/config/template.param.yaml @@ -0,0 +1,4 @@ +/**: + ros__parameters: + template: + dummy: 1.2 diff --git a/planning/behavior_velocity_template_module/package.xml b/planning/behavior_velocity_template_module/package.xml new file mode 100644 index 0000000000000..93a1cb4042e93 --- /dev/null +++ b/planning/behavior_velocity_template_module/package.xml @@ -0,0 +1,37 @@ + + + + behavior_velocity_template_module + 0.1.0 + The behavior_velocity_template_module package + + Daniel Sanchez + + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + eigen3_cmake_module + + autoware_auto_planning_msgs + behavior_velocity_planner_common + eigen + geometry_msgs + lanelet2_extension + libboost-dev + motion_utils + pluginlib + rclcpp + route_handler + sensor_msgs + tf2 + tier4_autoware_utils + visualization_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/behavior_velocity_template_module/plugins.xml b/planning/behavior_velocity_template_module/plugins.xml new file mode 100644 index 0000000000000..3560c84e6a080 --- /dev/null +++ b/planning/behavior_velocity_template_module/plugins.xml @@ -0,0 +1,3 @@ + + + diff --git a/planning/behavior_velocity_template_module/src/manager.cpp b/planning/behavior_velocity_template_module/src/manager.cpp new file mode 100644 index 0000000000000..22763f44af7bf --- /dev/null +++ b/planning/behavior_velocity_template_module/src/manager.cpp @@ -0,0 +1,63 @@ +// Copyright 2023 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 "manager.hpp" + +#include +#include + +#include + +#include +#include +#include +#include +#include +#include + +namespace behavior_velocity_planner +{ +using tier4_autoware_utils::getOrDeclareParameter; + +TemplateModuleManager::TemplateModuleManager(rclcpp::Node & node) +: SceneModuleManagerInterface(node, getModuleName()) +{ + std::string ns(getModuleName()); + dummy_parameter = getOrDeclareParameter(node, ns + ".dummy"); +} + +void TemplateModuleManager::launchNewModules( + [[maybe_unused]] const autoware_auto_planning_msgs::msg::PathWithLaneId & path) +{ + int64_t module_id = 0; + if (!isModuleRegistered(module_id)) { + registerModule( + std::make_shared(module_id, logger_.get_child(getModuleName()), clock_)); + } +} + +std::function &)> +TemplateModuleManager::getModuleExpiredFunction( + [[maybe_unused]] const autoware_auto_planning_msgs::msg::PathWithLaneId & path) +{ + return []([[maybe_unused]] const std::shared_ptr & scene_module) -> bool { + return false; + }; +} + +} // namespace behavior_velocity_planner + +#include +PLUGINLIB_EXPORT_CLASS( + behavior_velocity_planner::TemplateModulePlugin, behavior_velocity_planner::PluginInterface) diff --git a/planning/behavior_velocity_template_module/src/manager.hpp b/planning/behavior_velocity_template_module/src/manager.hpp new file mode 100644 index 0000000000000..ef4e1f00cae92 --- /dev/null +++ b/planning/behavior_velocity_template_module/src/manager.hpp @@ -0,0 +1,93 @@ +// Copyright 2023 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 MANAGER_HPP_ +#define MANAGER_HPP_ + +#include "scene.hpp" + +#include +#include +#include +#include + +#include + +#include +#include + +namespace behavior_velocity_planner +{ +/** + * @brief Constructor for the TemplateModuleManager class. + * + * Initializes a TemplateModuleManager instance with the provided ROS node, setting up essential + * parameters. + * + * @param node A reference to the ROS node. + */ +class TemplateModuleManager : public SceneModuleManagerInterface +{ +public: + explicit TemplateModuleManager(rclcpp::Node & node); + + /** + * @brief Get the name of the module. + * + * This method returns a constant character string representing the name of the module, which in + * this case is "template." + * + * @return A pointer to a constant character string containing the module name. + */ + const char * getModuleName() override { return "template"; } + +private: + double dummy_parameter{0.0}; + + /** + * @brief Launch new modules based on the provided path. + * + * This method is responsible for launching new modules based on the information provided in the + * given path. + * + * @param path The path with lane ID information to determine module launch. + */ + void launchNewModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + + /** + * @brief Get a function to check module expiration. + * + * This method returns a function that can be used to determine whether a specific module has + * expired based on the given path information. + * + * @param path The path with lane ID information for module expiration check. + * @return A function for checking module expiration. + */ + std::function &)> getModuleExpiredFunction( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; +}; + +/** + * @brief Plugin class for TemplateModuleManager. + * + * The TemplateModulePlugin class is used to integrate the TemplateModuleManager into the Behavior + * Velocity Planner. + */ +class TemplateModulePlugin : public PluginWrapper +{ +}; + +} // namespace behavior_velocity_planner + +#endif // MANAGER_HPP_ diff --git a/planning/behavior_velocity_template_module/src/scene.cpp b/planning/behavior_velocity_template_module/src/scene.cpp new file mode 100644 index 0000000000000..558f67b49817c --- /dev/null +++ b/planning/behavior_velocity_template_module/src/scene.cpp @@ -0,0 +1,52 @@ +// Copyright 2023 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 "scene.hpp" + +#include "motion_utils/trajectory/trajectory.hpp" +// #include "tier4_autoware_utils/tier4_autoware_utils.hpp" + +#include + +#include + +namespace behavior_velocity_planner +{ + +TemplateModule::TemplateModule( + const int64_t module_id, const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock) +: SceneModuleInterface(module_id, logger, clock) +{ +} + +visualization_msgs::msg::MarkerArray TemplateModule::createDebugMarkerArray() +{ + visualization_msgs::msg::MarkerArray ma; + return ma; +}; + +motion_utils::VirtualWalls TemplateModule::createVirtualWalls() +{ + motion_utils::VirtualWalls vw; + return vw; +} + +bool TemplateModule::modifyPathVelocity( + [[maybe_unused]] PathWithLaneId * path, [[maybe_unused]] StopReason * stop_reason) +{ + RCLCPP_INFO_ONCE(logger_, "Template Module is executing!"); + return false; +} + +} // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_template_module/src/scene.hpp b/planning/behavior_velocity_template_module/src/scene.hpp new file mode 100644 index 0000000000000..fab0b23efb25f --- /dev/null +++ b/planning/behavior_velocity_template_module/src/scene.hpp @@ -0,0 +1,69 @@ +// Copyright 2023 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 SCENE_HPP_ +#define SCENE_HPP_ + +#include +#include + +#include +#include + +namespace behavior_velocity_planner +{ +using autoware_auto_planning_msgs::msg::PathWithLaneId; + +class TemplateModule : public SceneModuleInterface +{ +public: + TemplateModule( + const int64_t module_id, const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock); + + /** + * @brief Modify the velocity of path points. + * + * This method is responsible for adjusting the velocity of each point in the input path based on + * specific criteria. + * + * @param path A pointer to the path containing points to be modified. + * @param stop_reason A pointer to the stop reason data. + * @return [bool] wether the path velocity was modified or not. + */ + bool modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) override; + + /** + * @brief Create a visualization of debug markers. + * + * This method is responsible for generating a visualization of debug markers and returning them + * as a `visualization_msgs::msg::MarkerArray`. + * + * @return A `visualization_msgs::msg::MarkerArray` containing debug markers. + */ + visualization_msgs::msg::MarkerArray createDebugMarkerArray() override; + + /** + * @brief Create virtual walls for the scene. + * + * This method is responsible for generating virtual walls for the scene and returning them as a + * `motion_utils::VirtualWalls` object. + * + * @return A `motion_utils::VirtualWalls` object representing virtual walls in the scene. + */ + motion_utils::VirtualWalls createVirtualWalls() override; +}; + +} // namespace behavior_velocity_planner + +#endif // SCENE_HPP_ diff --git a/planning/behavior_velocity_traffic_light_module/config/traffic_light.param.yaml b/planning/behavior_velocity_traffic_light_module/config/traffic_light.param.yaml index e8e0357daa4a1..a837ae1b46b9b 100644 --- a/planning/behavior_velocity_traffic_light_module/config/traffic_light.param.yaml +++ b/planning/behavior_velocity_traffic_light_module/config/traffic_light.param.yaml @@ -5,4 +5,4 @@ tl_state_timeout: 1.0 yellow_lamp_period: 2.75 enable_pass_judge: true - enable_rtc: true # If set to true, the scene modules require approval from the rtc (request to cooperate) function. If set to false, the modules can be executed without requiring rtc approval + enable_rtc: false # If set to true, the scene modules require approval from the rtc (request to cooperate) function. If set to false, the modules can be executed without requiring rtc approval diff --git a/planning/behavior_velocity_traffic_light_module/package.xml b/planning/behavior_velocity_traffic_light_module/package.xml index a8733c5dcf5be..c658f0890b986 100644 --- a/planning/behavior_velocity_traffic_light_module/package.xml +++ b/planning/behavior_velocity_traffic_light_module/package.xml @@ -18,8 +18,8 @@ eigen3_cmake_module autoware_adapi_v1_msgs - autoware_auto_perception_msgs autoware_auto_planning_msgs + autoware_perception_msgs behavior_velocity_planner_common eigen geometry_msgs diff --git a/planning/behavior_velocity_traffic_light_module/src/debug.cpp b/planning/behavior_velocity_traffic_light_module/src/debug.cpp index 9e475441a3f3c..9a0ba4f37c3c0 100644 --- a/planning/behavior_velocity_traffic_light_module/src/debug.cpp +++ b/planning/behavior_velocity_traffic_light_module/src/debug.cpp @@ -15,8 +15,9 @@ #include "scene.hpp" #include -#include - +#include +#include +#include #ifdef ROS_DISTRO_GALACTIC #include #else diff --git a/planning/behavior_velocity_traffic_light_module/src/manager.cpp b/planning/behavior_velocity_traffic_light_module/src/manager.cpp index 5516710ff3d5d..b943feddc103e 100644 --- a/planning/behavior_velocity_traffic_light_module/src/manager.cpp +++ b/planning/behavior_velocity_traffic_light_module/src/manager.cpp @@ -14,6 +14,9 @@ #include "manager.hpp" +#include +#include + #include #include @@ -22,22 +25,23 @@ #include #include #include - namespace behavior_velocity_planner { using lanelet::TrafficLight; +using tier4_autoware_utils::getOrDeclareParameter; TrafficLightModuleManager::TrafficLightModuleManager(rclcpp::Node & node) : SceneModuleManagerInterfaceWithRTC( node, getModuleName(), - node.declare_parameter(std::string(getModuleName()) + ".enable_rtc")) + getOrDeclareParameter(node, std::string(getModuleName()) + ".enable_rtc")) { const std::string ns(getModuleName()); - planner_param_.stop_margin = node.declare_parameter(ns + ".stop_margin"); - planner_param_.tl_state_timeout = node.declare_parameter(ns + ".tl_state_timeout"); - planner_param_.enable_pass_judge = node.declare_parameter(ns + ".enable_pass_judge"); - planner_param_.yellow_lamp_period = node.declare_parameter(ns + ".yellow_lamp_period"); - pub_tl_state_ = node.create_publisher( + planner_param_.stop_margin = getOrDeclareParameter(node, ns + ".stop_margin"); + planner_param_.tl_state_timeout = getOrDeclareParameter(node, ns + ".tl_state_timeout"); + planner_param_.enable_pass_judge = getOrDeclareParameter(node, ns + ".enable_pass_judge"); + planner_param_.yellow_lamp_period = + getOrDeclareParameter(node, ns + ".yellow_lamp_period"); + pub_tl_state_ = node.create_publisher( "~/output/traffic_signal", 1); } @@ -47,9 +51,7 @@ void TrafficLightModuleManager::modifyPathVelocity( visualization_msgs::msg::MarkerArray debug_marker_array; visualization_msgs::msg::MarkerArray virtual_wall_marker_array; - autoware_auto_perception_msgs::msg::LookingTrafficSignal tl_state; - tl_state.header.stamp = path->header.stamp; - tl_state.is_module_running = false; + autoware_perception_msgs::msg::TrafficSignal tl_state; autoware_adapi_v1_msgs::msg::VelocityFactorArray velocity_factor_array; velocity_factor_array.header.frame_id = "map"; diff --git a/planning/behavior_velocity_traffic_light_module/src/manager.hpp b/planning/behavior_velocity_traffic_light_module/src/manager.hpp index 450cba09e1e08..c36c6af1128ce 100644 --- a/planning/behavior_velocity_traffic_light_module/src/manager.hpp +++ b/planning/behavior_velocity_traffic_light_module/src/manager.hpp @@ -55,8 +55,7 @@ class TrafficLightModuleManager : public SceneModuleManagerInterfaceWithRTC const lanelet::TrafficLightConstPtr registered_element) const; // Debug - rclcpp::Publisher::SharedPtr - pub_tl_state_; + rclcpp::Publisher::SharedPtr pub_tl_state_; boost::optional first_ref_stop_path_point_index_; }; diff --git a/planning/behavior_velocity_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_traffic_light_module/src/scene.cpp index 679a2f0cb9320..7c4e947a42e7b 100644 --- a/planning/behavior_velocity_traffic_light_module/src/scene.cpp +++ b/planning/behavior_velocity_traffic_light_module/src/scene.cpp @@ -17,6 +17,8 @@ #include #include +#include +#include #include // To be replaced by std::optional in C++17 #include @@ -159,32 +161,6 @@ bool calcStopPointAndInsertIndex( } return false; } - -geometry_msgs::msg::Point getTrafficLightPosition( - const lanelet::ConstLineStringOrPolygon3d & traffic_light) -{ - if (!traffic_light.lineString()) { - throw std::invalid_argument{"Traffic light is not LineString"}; - } - geometry_msgs::msg::Point tl_center; - auto traffic_light_ls = traffic_light.lineString().value(); - for (const auto & tl_point : traffic_light_ls) { - tl_center.x += tl_point.x() / traffic_light_ls.size(); - tl_center.y += tl_point.y() / traffic_light_ls.size(); - tl_center.z += tl_point.z() / traffic_light_ls.size(); - } - return tl_center; -} -autoware_auto_perception_msgs::msg::LookingTrafficSignal initializeTrafficSignal( - const rclcpp::Time stamp) -{ - autoware_auto_perception_msgs::msg::LookingTrafficSignal state; - state.header.stamp = stamp; - state.is_module_running = true; - state.perception.has_state = false; - state.result.has_state = false; - return state; -} } // namespace TrafficLightModule::TrafficLightModule( @@ -197,7 +173,6 @@ TrafficLightModule::TrafficLightModule( traffic_light_reg_elem_(traffic_light_reg_elem), lane_(lane), state_(State::APPROACH), - input_(Input::PERCEPTION), is_prev_state_stop_(false) { velocity_factor_.init(VelocityFactor::TRAFFIC_SIGNAL); @@ -206,7 +181,6 @@ TrafficLightModule::TrafficLightModule( bool TrafficLightModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) { - looking_tl_state_ = initializeTrafficSignal(path->header.stamp); debug_data_ = DebugData(); debug_data_.base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; first_stop_path_point_index_ = static_cast(path->points.size()) - 1; @@ -217,9 +191,8 @@ bool TrafficLightModule::modifyPathVelocity(PathWithLaneId * path, StopReason * const auto & self_pose = planner_data_->current_odometry; - // Get lanelet2 traffic lights and stop lines. + // Get lanelet2 stop lines. lanelet::ConstLineString3d lanelet_stop_lines = *(traffic_light_reg_elem_.stopLine()); - lanelet::ConstLineStringsOrPolygons3d traffic_lights = traffic_light_reg_elem_.trafficLights(); // Calculate stop pose and insert index Eigen::Vector2d stop_line_point{}; @@ -255,7 +228,7 @@ bool TrafficLightModule::modifyPathVelocity(PathWithLaneId * path, StopReason * first_ref_stop_path_point_index_ = stop_line_point_idx; // Check if stop is coming. - setSafe(!isStopSignal(traffic_lights)); + setSafe(!isStopSignal()); if (isActivated()) { is_prev_state_stop_ = false; return true; @@ -283,33 +256,27 @@ bool TrafficLightModule::modifyPathVelocity(PathWithLaneId * path, StopReason * return false; } -bool TrafficLightModule::isStopSignal(const lanelet::ConstLineStringsOrPolygons3d & traffic_lights) +bool TrafficLightModule::isStopSignal() { - if (!updateTrafficSignal(traffic_lights)) { + if (!updateTrafficSignal()) { return false; } - return looking_tl_state_.result.judge == - autoware_auto_perception_msgs::msg::TrafficSignalWithJudge::STOP; + return isTrafficSignalStop(looking_tl_state_); } -bool TrafficLightModule::updateTrafficSignal( - const lanelet::ConstLineStringsOrPolygons3d & traffic_lights) +bool TrafficLightModule::updateTrafficSignal() { - autoware_auto_perception_msgs::msg::TrafficSignalStamped tl_state_perception; - bool found_perception = getHighestConfidenceTrafficSignal(traffic_lights, tl_state_perception); + TrafficSignal signal; + bool found_signal = findValidTrafficSignal(signal); - if (!found_perception) { + if (!found_signal) { // Don't stop when UNKNOWN or TIMEOUT as discussed at #508 - input_ = Input::NONE; return false; } - if (found_perception) { - looking_tl_state_.perception = generateTlStateWithJudgeFromTlState(tl_state_perception.signal); - looking_tl_state_.result = looking_tl_state_.perception; - input_ = Input::PERCEPTION; - } + // Found signal associated with the lanelet + looking_tl_state_ = signal; return true; } @@ -337,7 +304,7 @@ bool TrafficLightModule::isPassthrough(const double & signed_arc_length) const const auto & enable_pass_judge = planner_param_.enable_pass_judge; - if (enable_pass_judge && !stoppable && !is_prev_state_stop_ && input_ == Input::PERCEPTION) { + if (enable_pass_judge && !stoppable && !is_prev_state_stop_) { // Cannot stop under acceleration and jerk limits. // However, ego vehicle can't enter the intersection while the light is yellow. // It is called dilemma zone. Make a stop decision to be safe. @@ -359,10 +326,9 @@ bool TrafficLightModule::isPassthrough(const double & signed_arc_length) const } bool TrafficLightModule::isTrafficSignalStop( - const autoware_auto_perception_msgs::msg::TrafficSignal & tl_state) const + const autoware_perception_msgs::msg::TrafficSignal & tl_state) const { - if (hasTrafficLightCircleColor( - tl_state, autoware_auto_perception_msgs::msg::TrafficLight::GREEN)) { + if (hasTrafficLightCircleColor(tl_state, TrafficSignalElement::GREEN)) { return false; } @@ -373,86 +339,46 @@ bool TrafficLightModule::isTrafficSignalStop( } if ( turn_direction == "right" && - hasTrafficLightShape(tl_state, autoware_auto_perception_msgs::msg::TrafficLight::RIGHT_ARROW)) { + hasTrafficLightShape(tl_state, TrafficSignalElement::RIGHT_ARROW)) { return false; } if ( - turn_direction == "left" && - hasTrafficLightShape(tl_state, autoware_auto_perception_msgs::msg::TrafficLight::LEFT_ARROW)) { + turn_direction == "left" && hasTrafficLightShape(tl_state, TrafficSignalElement::LEFT_ARROW)) { return false; } if ( turn_direction == "straight" && - hasTrafficLightShape(tl_state, autoware_auto_perception_msgs::msg::TrafficLight::UP_ARROW)) { + hasTrafficLightShape(tl_state, TrafficSignalElement::UP_ARROW)) { return false; } return true; } -bool TrafficLightModule::getHighestConfidenceTrafficSignal( - const lanelet::ConstLineStringsOrPolygons3d & traffic_lights, - autoware_auto_perception_msgs::msg::TrafficSignalStamped & highest_confidence_tl_state) +bool TrafficLightModule::findValidTrafficSignal(TrafficSignal & valid_traffic_signal) { - // search traffic light state - bool found = false; - double highest_confidence = 0.0; - std::string reason; - for (const auto & traffic_light : traffic_lights) { - // traffic light must be linestrings - if (!traffic_light.isLineString()) { - reason = "NotLineString"; - continue; - } - - const int id = static_cast(traffic_light).id(); - RCLCPP_DEBUG(logger_, "traffic light id: %d (on route)", id); - const auto tl_state_stamped = planner_data_->getTrafficSignal(id); - if (!tl_state_stamped) { - reason = "TrafficSignalNotFound"; - continue; - } - - const auto header = tl_state_stamped->header; - const auto tl_state = tl_state_stamped->signal; - if (!((clock_->now() - header.stamp).seconds() < planner_param_.tl_state_timeout)) { - reason = "TimeOut"; - continue; - } - - if ( - tl_state.lights.empty() || - tl_state.lights.front().color == autoware_auto_perception_msgs::msg::TrafficLight::UNKNOWN) { - reason = "TrafficLightUnknown"; - continue; - } - - if (highest_confidence < tl_state.lights.front().confidence) { - highest_confidence = tl_state.lights.front().confidence; - highest_confidence_tl_state = *tl_state_stamped; - try { - auto tl_position = getTrafficLightPosition(traffic_light); - debug_data_.traffic_light_points.push_back(tl_position); - debug_data_.highest_confidence_traffic_light_point = std::make_optional(tl_position); - } catch (const std::invalid_argument & ex) { - RCLCPP_WARN_STREAM(logger_, ex.what()); - continue; - } - found = true; - } + // get traffic signal associated with the regulatory element id + const auto traffic_signal_stamped = planner_data_->getTrafficSignal(traffic_light_reg_elem_.id()); + if (!traffic_signal_stamped) { + RCLCPP_WARN_THROTTLE( + logger_, *clock_, 5000 /* ms */, + "the traffic signal data associated with regulatory element id is not received"); + return false; } - if (!found) { - std::string not_found_traffic_ids{""}; - for (size_t i = 0; i < traffic_lights.size(); ++i) { - const int id = static_cast(traffic_lights.at(i)).id(); - not_found_traffic_ids += (i != 0 ? "," : "") + std::to_string(id); - } + // check if the traffic signal data is outdated + const auto is_traffic_signal_timeout = + (clock_->now() - traffic_signal_stamped->stamp).seconds() > planner_param_.tl_state_timeout; + if (is_traffic_signal_timeout) { RCLCPP_WARN_THROTTLE( - logger_, *clock_, 5000 /* ms */, "cannot find traffic light lamp state (%s) due to %s.", - not_found_traffic_ids.c_str(), reason.c_str()); + logger_, *clock_, 5000 /* ms */, "the received traffic signal data is outdated"); + RCLCPP_WARN_STREAM_THROTTLE( + logger_, *clock_, 5000 /* ms */, + "time diff: " << (clock_->now() - traffic_signal_stamped->stamp).seconds()); return false; } + + valid_traffic_signal = traffic_signal_stamped->signal; return true; } @@ -496,40 +422,24 @@ autoware_auto_planning_msgs::msg::PathWithLaneId TrafficLightModule::insertStopP } bool TrafficLightModule::hasTrafficLightCircleColor( - const autoware_auto_perception_msgs::msg::TrafficSignal & tl_state, - const uint8_t & lamp_color) const + const autoware_perception_msgs::msg::TrafficSignal & tl_state, const uint8_t & lamp_color) const { - using autoware_auto_perception_msgs::msg::TrafficLight; - const auto it_lamp = - std::find_if(tl_state.lights.begin(), tl_state.lights.end(), [&lamp_color](const auto & x) { - return x.shape == TrafficLight::CIRCLE && x.color == lamp_color; + std::find_if(tl_state.elements.begin(), tl_state.elements.end(), [&lamp_color](const auto & x) { + return x.shape == TrafficSignalElement::CIRCLE && x.color == lamp_color; }); - return it_lamp != tl_state.lights.end(); + return it_lamp != tl_state.elements.end(); } bool TrafficLightModule::hasTrafficLightShape( - const autoware_auto_perception_msgs::msg::TrafficSignal & tl_state, - const uint8_t & lamp_shape) const + const autoware_perception_msgs::msg::TrafficSignal & tl_state, const uint8_t & lamp_shape) const { const auto it_lamp = std::find_if( - tl_state.lights.begin(), tl_state.lights.end(), + tl_state.elements.begin(), tl_state.elements.end(), [&lamp_shape](const auto & x) { return x.shape == lamp_shape; }); - return it_lamp != tl_state.lights.end(); + return it_lamp != tl_state.elements.end(); } -autoware_auto_perception_msgs::msg::TrafficSignalWithJudge -TrafficLightModule::generateTlStateWithJudgeFromTlState( - const autoware_auto_perception_msgs::msg::TrafficSignal tl_state) const -{ - autoware_auto_perception_msgs::msg::TrafficSignalWithJudge tl_state_with_judge; - tl_state_with_judge.signal = tl_state; - tl_state_with_judge.has_state = true; - tl_state_with_judge.judge = isTrafficSignalStop(tl_state) - ? autoware_auto_perception_msgs::msg::TrafficSignalWithJudge::STOP - : autoware_auto_perception_msgs::msg::TrafficSignalWithJudge::GO; - return tl_state_with_judge; -} } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_traffic_light_module/src/scene.hpp b/planning/behavior_velocity_traffic_light_module/src/scene.hpp index 99ece6cbca09d..a24db2c440e91 100644 --- a/planning/behavior_velocity_traffic_light_module/src/scene.hpp +++ b/planning/behavior_velocity_traffic_light_module/src/scene.hpp @@ -29,8 +29,6 @@ #include #include -#include - #include #include @@ -39,15 +37,15 @@ namespace behavior_velocity_planner class TrafficLightModule : public SceneModuleInterface { public: + using TrafficSignal = autoware_perception_msgs::msg::TrafficSignal; + using TrafficSignalElement = autoware_perception_msgs::msg::TrafficSignalElement; enum class State { APPROACH, GO_OUT }; - enum class Input { PERCEPTION, NONE }; // EXTERNAL: FOA, V2X, etc. struct DebugData { double base_link2front; std::vector, - autoware_auto_perception_msgs::msg::TrafficSignal>> + std::shared_ptr, autoware_perception_msgs::msg::TrafficSignal>> tl_state; std::vector stop_poses; geometry_msgs::msg::Pose first_stop_pose; @@ -77,10 +75,7 @@ class TrafficLightModule : public SceneModuleInterface visualization_msgs::msg::MarkerArray createDebugMarkerArray() override; motion_utils::VirtualWalls createVirtualWalls() override; - inline autoware_auto_perception_msgs::msg::LookingTrafficSignal getTrafficSignal() const - { - return looking_tl_state_; - } + inline TrafficSignal getTrafficSignal() const { return looking_tl_state_; } inline State getTrafficLightModuleState() const { return state_; } @@ -90,10 +85,9 @@ class TrafficLightModule : public SceneModuleInterface } private: - bool isStopSignal(const lanelet::ConstLineStringsOrPolygons3d & traffic_lights); + bool isStopSignal(); - bool isTrafficSignalStop( - const autoware_auto_perception_msgs::msg::TrafficSignal & tl_state) const; + bool isTrafficSignalStop(const TrafficSignal & tl_state) const; autoware_auto_planning_msgs::msg::PathWithLaneId insertStopPose( const autoware_auto_planning_msgs::msg::PathWithLaneId & input, @@ -102,22 +96,13 @@ class TrafficLightModule : public SceneModuleInterface bool isPassthrough(const double & signed_arc_length) const; - bool hasTrafficLightCircleColor( - const autoware_auto_perception_msgs::msg::TrafficSignal & tl_state, - const uint8_t & lamp_color) const; + bool hasTrafficLightCircleColor(const TrafficSignal & tl_state, const uint8_t & lamp_color) const; - bool hasTrafficLightShape( - const autoware_auto_perception_msgs::msg::TrafficSignal & tl_state, - const uint8_t & lamp_shape) const; + bool hasTrafficLightShape(const TrafficSignal & tl_state, const uint8_t & lamp_shape) const; - bool getHighestConfidenceTrafficSignal( - const lanelet::ConstLineStringsOrPolygons3d & traffic_lights, - autoware_auto_perception_msgs::msg::TrafficSignalStamped & highest_confidence_tl_state); + bool findValidTrafficSignal(TrafficSignal & valid_traffic_signal); - bool updateTrafficSignal(const lanelet::ConstLineStringsOrPolygons3d & traffic_lights); - - autoware_auto_perception_msgs::msg::TrafficSignalWithJudge generateTlStateWithJudgeFromTlState( - const autoware_auto_perception_msgs::msg::TrafficSignal tl_state) const; + bool updateTrafficSignal(); // Lane id const int64_t lane_id_; @@ -129,9 +114,6 @@ class TrafficLightModule : public SceneModuleInterface // State State state_; - // Input - Input input_; - // Parameter PlannerParam planner_param_; @@ -144,7 +126,7 @@ class TrafficLightModule : public SceneModuleInterface boost::optional first_ref_stop_path_point_index_; // Traffic Light State - autoware_auto_perception_msgs::msg::LookingTrafficSignal looking_tl_state_; + TrafficSignal looking_tl_state_; }; } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_virtual_traffic_light_module/src/debug.cpp b/planning/behavior_velocity_virtual_traffic_light_module/src/debug.cpp index 1324525ebc6e1..83b5e6317aeb0 100644 --- a/planning/behavior_velocity_virtual_traffic_light_module/src/debug.cpp +++ b/planning/behavior_velocity_virtual_traffic_light_module/src/debug.cpp @@ -14,9 +14,10 @@ #include "scene.hpp" -#include -#include - +#include +#include +#include +#include using motion_utils::createStopVirtualWallMarker; using tier4_autoware_utils::appendMarkerArray; using tier4_autoware_utils::createDefaultMarker; diff --git a/planning/behavior_velocity_virtual_traffic_light_module/src/manager.cpp b/planning/behavior_velocity_virtual_traffic_light_module/src/manager.cpp index e5a2b0e43def2..5310fae78c294 100644 --- a/planning/behavior_velocity_virtual_traffic_light_module/src/manager.cpp +++ b/planning/behavior_velocity_virtual_traffic_light_module/src/manager.cpp @@ -14,7 +14,9 @@ #include "manager.hpp" +#include #include +#include #include #include @@ -25,6 +27,7 @@ namespace behavior_velocity_planner { using lanelet::autoware::VirtualTrafficLight; +using tier4_autoware_utils::getOrDeclareParameter; VirtualTrafficLightModuleManager::VirtualTrafficLightModuleManager(rclcpp::Node & node) : SceneModuleManagerInterface(node, getModuleName()) @@ -33,14 +36,15 @@ VirtualTrafficLightModuleManager::VirtualTrafficLightModuleManager(rclcpp::Node { auto & p = planner_param_; - p.max_delay_sec = node.declare_parameter(ns + ".max_delay_sec"); - p.near_line_distance = node.declare_parameter(ns + ".near_line_distance"); - p.dead_line_margin = node.declare_parameter(ns + ".dead_line_margin"); - p.hold_stop_margin_distance = node.declare_parameter(ns + ".hold_stop_margin_distance"); - p.max_yaw_deviation_rad = - tier4_autoware_utils::deg2rad(node.declare_parameter(ns + ".max_yaw_deviation_deg")); + p.max_delay_sec = getOrDeclareParameter(node, ns + ".max_delay_sec"); + p.near_line_distance = getOrDeclareParameter(node, ns + ".near_line_distance"); + p.dead_line_margin = getOrDeclareParameter(node, ns + ".dead_line_margin"); + p.hold_stop_margin_distance = + getOrDeclareParameter(node, ns + ".hold_stop_margin_distance"); + p.max_yaw_deviation_rad = tier4_autoware_utils::deg2rad( + getOrDeclareParameter(node, ns + ".max_yaw_deviation_deg")); p.check_timeout_after_stop_line = - node.declare_parameter(ns + ".check_timeout_after_stop_line"); + getOrDeclareParameter(node, ns + ".check_timeout_after_stop_line"); } } diff --git a/planning/behavior_velocity_walkway_module/src/debug.cpp b/planning/behavior_velocity_walkway_module/src/debug.cpp index 51db3c204f2a4..8814fa8271866 100644 --- a/planning/behavior_velocity_walkway_module/src/debug.cpp +++ b/planning/behavior_velocity_walkway_module/src/debug.cpp @@ -15,8 +15,10 @@ #include "scene_walkway.hpp" #include -#include -#include +#include +#include +#include +#include #include diff --git a/planning/behavior_velocity_walkway_module/src/manager.cpp b/planning/behavior_velocity_walkway_module/src/manager.cpp index d0c1a47408929..8995c2362a8cd 100644 --- a/planning/behavior_velocity_walkway_module/src/manager.cpp +++ b/planning/behavior_velocity_walkway_module/src/manager.cpp @@ -15,6 +15,7 @@ #include "manager.hpp" #include +#include #include #include @@ -26,6 +27,7 @@ namespace behavior_velocity_planner { using lanelet::autoware::Crosswalk; +using tier4_autoware_utils::getOrDeclareParameter; WalkwayModuleManager::WalkwayModuleManager(rclcpp::Node & node) : SceneModuleManagerInterface(node, getModuleName()) @@ -35,18 +37,15 @@ WalkwayModuleManager::WalkwayModuleManager(rclcpp::Node & node) // for walkway parameters auto & wp = walkway_planner_param_; wp.stop_distance_from_crosswalk = - node.declare_parameter(ns + ".stop_distance_from_crosswalk"); - wp.stop_duration = node.declare_parameter(ns + ".stop_duration"); + getOrDeclareParameter(node, ns + ".stop_distance_from_crosswalk"); + wp.stop_duration = getOrDeclareParameter(node, ns + ".stop_duration"); } void WalkwayModuleManager::launchNewModules(const PathWithLaneId & path) { const auto rh = planner_data_->route_handler_; - if (!opt_use_regulatory_element_) { - opt_use_regulatory_element_ = checkRegulatoryElementExistence(rh->getLaneletMapPtr()); - } - const auto launch = [this, &path](const auto & lanelet) { + const auto launch = [this, &path](const auto & lanelet, const auto & use_regulatory_element) { const auto attribute = lanelet.attributeOr(lanelet::AttributeNamesString::Subtype, std::string("")); if (attribute != lanelet::AttributeValueString::Walkway) { @@ -62,24 +61,21 @@ void WalkwayModuleManager::launchNewModules(const PathWithLaneId & path) const auto lanelet_map_ptr = planner_data_->route_handler_->getLaneletMapPtr(); registerModule(std::make_shared( - lanelet.id(), lanelet_map_ptr, p, *opt_use_regulatory_element_, logger, clock_)); + lanelet.id(), lanelet_map_ptr, p, use_regulatory_element, logger, clock_)); }; - if (*opt_use_regulatory_element_) { - const auto crosswalk_leg_elem_map = planning_utils::getRegElemMapOnPath( - path, rh->getLaneletMapPtr(), planner_data_->current_odometry->pose); + const auto crosswalk_leg_elem_map = planning_utils::getRegElemMapOnPath( + path, rh->getLaneletMapPtr(), planner_data_->current_odometry->pose); - for (const auto & crosswalk : crosswalk_leg_elem_map) { - launch(crosswalk.first->crosswalkLanelet()); - } - } else { - const auto crosswalk_lanelets = getCrosswalksOnPath( - planner_data_->current_odometry->pose, path, rh->getLaneletMapPtr(), - rh->getOverallGraphPtr()); + for (const auto & crosswalk : crosswalk_leg_elem_map) { + launch(crosswalk.first->crosswalkLanelet(), true); + } - for (const auto & crosswalk : crosswalk_lanelets) { - launch(crosswalk); - } + const auto crosswalk_lanelets = getCrosswalksOnPath( + planner_data_->current_odometry->pose, path, rh->getLaneletMapPtr(), rh->getOverallGraphPtr()); + + for (const auto & crosswalk : crosswalk_lanelets) { + launch(crosswalk, false); } } @@ -90,17 +86,14 @@ WalkwayModuleManager::getModuleExpiredFunction(const PathWithLaneId & path) std::set walkway_id_set; - if (*opt_use_regulatory_element_) { - const auto crosswalk_leg_elem_map = planning_utils::getRegElemMapOnPath( - path, rh->getLaneletMapPtr(), planner_data_->current_odometry->pose); + walkway_id_set = getCrosswalkIdSetOnPath( + planner_data_->current_odometry->pose, path, rh->getLaneletMapPtr(), rh->getOverallGraphPtr()); - for (const auto & crosswalk : crosswalk_leg_elem_map) { - walkway_id_set.insert(crosswalk.first->id()); - } - } else { - walkway_id_set = getCrosswalkIdSetOnPath( - planner_data_->current_odometry->pose, path, rh->getLaneletMapPtr(), - rh->getOverallGraphPtr()); + const auto crosswalk_leg_elem_map = planning_utils::getRegElemMapOnPath( + path, rh->getLaneletMapPtr(), planner_data_->current_odometry->pose); + + for (const auto & crosswalk : crosswalk_leg_elem_map) { + walkway_id_set.insert(crosswalk.first->id()); } return [walkway_id_set](const std::shared_ptr & scene_module) { diff --git a/planning/behavior_velocity_walkway_module/src/manager.hpp b/planning/behavior_velocity_walkway_module/src/manager.hpp index 555e3a73db103..453fcdb40f0db 100644 --- a/planning/behavior_velocity_walkway_module/src/manager.hpp +++ b/planning/behavior_velocity_walkway_module/src/manager.hpp @@ -50,8 +50,6 @@ class WalkwayModuleManager : public SceneModuleManagerInterface std::function &)> getModuleExpiredFunction( const PathWithLaneId & path) override; - - std::optional opt_use_regulatory_element_{std::nullopt}; }; class WalkwayModulePlugin : public PluginWrapper diff --git a/planning/freespace_planner/include/freespace_planner/freespace_planner_node.hpp b/planning/freespace_planner/include/freespace_planner/freespace_planner_node.hpp index fde0767006e33..9c13f14d180d0 100644 --- a/planning/freespace_planner/include/freespace_planner/freespace_planner_node.hpp +++ b/planning/freespace_planner/include/freespace_planner/freespace_planner_node.hpp @@ -31,6 +31,8 @@ #ifndef FREESPACE_PLANNER__FREESPACE_PLANNER_NODE_HPP_ #define FREESPACE_PLANNER__FREESPACE_PLANNER_NODE_HPP_ +#include "tier4_autoware_utils/ros/logger_level_configure.hpp" + #include #include #include @@ -158,6 +160,8 @@ class FreespacePlannerNode : public rclcpp::Node void initializePlanningAlgorithm(); TransformStamped getTransform(const std::string & from, const std::string & to); + + std::unique_ptr logger_configure_; }; } // namespace freespace_planner diff --git a/planning/freespace_planner/package.xml b/planning/freespace_planner/package.xml index a748b7f80c9ad..f92c0a449970d 100644 --- a/planning/freespace_planner/package.xml +++ b/planning/freespace_planner/package.xml @@ -19,6 +19,7 @@ autoware_auto_planning_msgs freespace_planning_algorithms geometry_msgs + motion_utils nav_msgs planning_test_utils rclcpp diff --git a/planning/freespace_planner/src/freespace_planner/freespace_planner_node.cpp b/planning/freespace_planner/src/freespace_planner/freespace_planner_node.cpp index e0d7f86e45245..6400ae6d135ba 100644 --- a/planning/freespace_planner/src/freespace_planner/freespace_planner_node.cpp +++ b/planning/freespace_planner/src/freespace_planner/freespace_planner_node.cpp @@ -32,7 +32,8 @@ #include "freespace_planning_algorithms/abstract_algorithm.hpp" -#include +#include +#include #include #include @@ -284,6 +285,8 @@ FreespacePlannerNode::FreespacePlannerNode(const rclcpp::NodeOptions & node_opti timer_ = rclcpp::create_timer( this, get_clock(), period_ns, std::bind(&FreespacePlannerNode::onTimer, this)); } + + logger_configure_ = std::make_unique(this); } PlannerCommonParam FreespacePlannerNode::getPlannerCommonParam() diff --git a/planning/freespace_planning_algorithms/src/abstract_algorithm.cpp b/planning/freespace_planning_algorithms/src/abstract_algorithm.cpp index 159c0df82fcc3..2c7340072eb53 100644 --- a/planning/freespace_planning_algorithms/src/abstract_algorithm.cpp +++ b/planning/freespace_planning_algorithms/src/abstract_algorithm.cpp @@ -14,7 +14,8 @@ #include "freespace_planning_algorithms/abstract_algorithm.hpp" -#include +#include +#include #include diff --git a/planning/freespace_planning_algorithms/src/astar_search.cpp b/planning/freespace_planning_algorithms/src/astar_search.cpp index b2b2321adacd0..1ab5e9d5df487 100644 --- a/planning/freespace_planning_algorithms/src/astar_search.cpp +++ b/planning/freespace_planning_algorithms/src/astar_search.cpp @@ -14,7 +14,8 @@ #include "freespace_planning_algorithms/astar_search.hpp" -#include +#include +#include #include diff --git a/planning/mission_planner/config/mission_planner.param.yaml b/planning/mission_planner/config/mission_planner.param.yaml index 98c28344ea25c..9b7dcffbc687b 100644 --- a/planning/mission_planner/config/mission_planner.param.yaml +++ b/planning/mission_planner/config/mission_planner.param.yaml @@ -9,3 +9,4 @@ reroute_time_threshold: 10.0 minimum_reroute_length: 30.0 consider_no_drivable_lanes: false # This flag is for considering no_drivable_lanes in planning or not. + check_footprint_inside_lanes: true diff --git a/planning/mission_planner/schema/mission_planner_nodes.schema.json b/planning/mission_planner/schema/mission_planner_nodes.schema.json new file mode 100644 index 0000000000000..3d0bdc41c5c34 --- /dev/null +++ b/planning/mission_planner/schema/mission_planner_nodes.schema.json @@ -0,0 +1,83 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Mission Planner nodes", + "type": "object", + "definitions": { + "mission_planner_nodes": { + "type": "object", + "properties": { + "map_frame": { + "type": "string", + "description": "The frame name for map ", + "default": "map" + }, + "arrival_check_angle_deg": { + "type": "number", + "description": "Angle threshold for goal check ", + "default": "45.0" + }, + "arrival_check_distance": { + "type": "number", + "description": "Distance threshold for goal check", + "default": "1.0", + "minimum": 0.0 + }, + "arrival_check_duration": { + "type": "number", + "description": "Duration threshold for goal check", + "default": "1.0", + "minimum": 0.0 + }, + "goal_angle_threshold_deg": { + "type": "number", + "description": "Max goal pose angle for goal approve", + "default": "45.0" + }, + "enable_correct_goal_pose": { + "type": "boolean", + "description": "Enabling correction of goal pose according to the closest lanelet orientation", + "default": "false" + }, + "reroute_time_threshold": { + "type": "number", + "description": "If the time to the rerouting point at the current velocity is greater than this threshold, rerouting is possible", + "default": "10.0", + "minimum": 0.0 + }, + "minimum_reroute_length": { + "type": "number", + "description": "Minimum Length for publishing a new route", + "default": "30.0" + }, + "consider_no_drivable_lanes": { + "type": "boolean", + "description": "This flag is for considering no_drivable_lanes in planning or not", + "default": "false" + } + }, + "required": [ + "map_frame", + "arrival_check_angle_deg", + "arrival_check_distance", + "arrival_check_duration", + "goal_angle_threshold_deg", + "enable_correct_goal_pose", + "reroute_time_threshold", + "minimum_reroute_length", + "consider_no_drivable_lanes" + ] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/mission_planner_nodes" + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] +} diff --git a/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp b/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp index d8289824e517c..673519b6f7a0e 100644 --- a/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp +++ b/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp @@ -20,7 +20,11 @@ #include #include #include -#include +#include +#include +#include +#include +#include #include #include @@ -148,6 +152,8 @@ void DefaultPlanner::initialize_common(rclcpp::Node * node) param_.goal_angle_threshold_deg = node_->declare_parameter("goal_angle_threshold_deg"); param_.enable_correct_goal_pose = node_->declare_parameter("enable_correct_goal_pose"); param_.consider_no_drivable_lanes = node_->declare_parameter("consider_no_drivable_lanes"); + param_.check_footprint_inside_lanes = + node_->declare_parameter("check_footprint_inside_lanes"); } void DefaultPlanner::initialize(rclcpp::Node * node) @@ -345,6 +351,7 @@ bool DefaultPlanner::is_goal_valid( // check if goal footprint exceeds lane when the goal isn't in parking_lot if ( + param_.check_footprint_inside_lanes && !check_goal_footprint( closest_lanelet, combined_prev_lanelet, polygon_footprint, next_lane_length) && !is_in_parking_lot( diff --git a/planning/mission_planner/src/lanelet2_plugins/default_planner.hpp b/planning/mission_planner/src/lanelet2_plugins/default_planner.hpp index cf5a19443dd82..251d0db533182 100644 --- a/planning/mission_planner/src/lanelet2_plugins/default_planner.hpp +++ b/planning/mission_planner/src/lanelet2_plugins/default_planner.hpp @@ -38,6 +38,7 @@ struct DefaultPlannerParameters double goal_angle_threshold_deg; bool enable_correct_goal_pose; bool consider_no_drivable_lanes; + bool check_footprint_inside_lanes; }; class DefaultPlanner : public mission_planner::PlannerPlugin diff --git a/planning/mission_planner/src/lanelet2_plugins/utility_functions.hpp b/planning/mission_planner/src/lanelet2_plugins/utility_functions.hpp index 0bfb0553e75ef..3ea6237f38501 100644 --- a/planning/mission_planner/src/lanelet2_plugins/utility_functions.hpp +++ b/planning/mission_planner/src/lanelet2_plugins/utility_functions.hpp @@ -15,8 +15,6 @@ #ifndef LANELET2_PLUGINS__UTILITY_FUNCTIONS_HPP_ #define LANELET2_PLUGINS__UTILITY_FUNCTIONS_HPP_ -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" - #include #include #include diff --git a/planning/mission_planner/src/mission_planner/arrival_checker.cpp b/planning/mission_planner/src/mission_planner/arrival_checker.cpp index ac7a37acd4d5b..1e9f02ff0338d 100644 --- a/planning/mission_planner/src/mission_planner/arrival_checker.cpp +++ b/planning/mission_planner/src/mission_planner/arrival_checker.cpp @@ -14,7 +14,9 @@ #include "arrival_checker.hpp" -#include +#include +#include +#include #include diff --git a/planning/mission_planner/src/mission_planner/mission_planner.cpp b/planning/mission_planner/src/mission_planner/mission_planner.cpp index 08da1260b000c..513a5b4c25a2e 100644 --- a/planning/mission_planner/src/mission_planner/mission_planner.cpp +++ b/planning/mission_planner/src/mission_planner/mission_planner.cpp @@ -24,6 +24,8 @@ #include #include +#include + #include #include #include @@ -97,12 +99,14 @@ MissionPlanner::MissionPlanner(const rclcpp::NodeOptions & options) rclcpp::QoS(1), std::bind(&MissionPlanner::on_reroute_availability, this, std::placeholders::_1)); - auto qos_transient_local = rclcpp::QoS{1}.transient_local(); + const auto durable_qos = rclcpp::QoS(1).transient_local(); sub_vector_map_ = create_subscription( - "input/vector_map", qos_transient_local, + "input/vector_map", durable_qos, std::bind(&MissionPlanner::on_map, this, std::placeholders::_1)); + sub_modified_goal_ = create_subscription( + "input/modified_goal", durable_qos, + std::bind(&MissionPlanner::on_modified_goal, this, std::placeholders::_1)); - const auto durable_qos = rclcpp::QoS(1).transient_local(); pub_marker_ = create_publisher("debug/route_marker", durable_qos); const auto adaptor = component_interface_utils::NodeAdaptor(this); @@ -117,9 +121,32 @@ MissionPlanner::MissionPlanner(const rclcpp::NodeOptions & options) adaptor.init_srv(srv_change_route_points_, this, &MissionPlanner::on_change_route_points); adaptor.init_srv(srv_set_mrm_route_, this, &MissionPlanner::on_set_mrm_route); adaptor.init_srv(srv_clear_mrm_route_, this, &MissionPlanner::on_clear_mrm_route); - adaptor.init_sub(sub_modified_goal_, this, &MissionPlanner::on_modified_goal); + // Route state will be published when the node gets ready for route api after initialization, + // otherwise the mission planner rejects the request for the API. + data_check_timer_ = create_wall_timer( + std::chrono::milliseconds(100), std::bind(&MissionPlanner::checkInitialization, this)); + + logger_configure_ = std::make_unique(this); +} + +void MissionPlanner::checkInitialization() +{ + if (!planner_->ready()) { + RCLCPP_INFO_THROTTLE( + get_logger(), *get_clock(), 5000, "waiting lanelet map... Route API is not ready."); + return; + } + if (!odometry_) { + RCLCPP_INFO_THROTTLE( + get_logger(), *get_clock(), 5000, "waiting odometry... Route API is not ready."); + return; + } + + // All data is ready. Now API is available. + RCLCPP_INFO(get_logger(), "Route API is ready."); change_state(RouteState::Message::UNSET); + data_check_timer_->cancel(); // stop timer callback } void MissionPlanner::on_odometry(const Odometry::ConstSharedPtr msg) diff --git a/planning/mission_planner/src/mission_planner/mission_planner.hpp b/planning/mission_planner/src/mission_planner/mission_planner.hpp index fd1b317970749..ea44d2643e186 100644 --- a/planning/mission_planner/src/mission_planner/mission_planner.hpp +++ b/planning/mission_planner/src/mission_planner/mission_planner.hpp @@ -16,6 +16,7 @@ #define MISSION_PLANNER__MISSION_PLANNER_HPP_ #include "arrival_checker.hpp" +#include "tier4_autoware_utils/ros/logger_level_configure.hpp" #include #include @@ -75,12 +76,18 @@ class MissionPlanner : public rclcpp::Node rclcpp::Subscription::SharedPtr sub_odometry_; rclcpp::Subscription::SharedPtr sub_vector_map_; rclcpp::Subscription::SharedPtr sub_reroute_availability_; + rclcpp::Subscription::SharedPtr sub_modified_goal_; + Odometry::ConstSharedPtr odometry_; HADMapBin::ConstSharedPtr map_ptr_; RerouteAvailability::ConstSharedPtr reroute_availability_; void on_odometry(const Odometry::ConstSharedPtr msg); void on_map(const HADMapBin::ConstSharedPtr msg); void on_reroute_availability(const RerouteAvailability::ConstSharedPtr msg); + void on_modified_goal(const PoseWithUuidStamped::ConstSharedPtr msg); + + rclcpp::TimerBase::SharedPtr data_check_timer_; + void checkInitialization(); rclcpp::Publisher::SharedPtr pub_marker_; void clear_route(); @@ -128,8 +135,6 @@ class MissionPlanner : public rclcpp::Node const ClearMrmRoute::Service::Request::SharedPtr req, const ClearMrmRoute::Service::Response::SharedPtr res); - component_interface_utils::Subscription::SharedPtr sub_modified_goal_; - void on_modified_goal(const ModifiedGoal::Message::ConstSharedPtr msg); void on_change_route( const SetRoute::Service::Request::SharedPtr req, const SetRoute::Service::Response::SharedPtr res); @@ -143,6 +148,8 @@ class MissionPlanner : public rclcpp::Node std::shared_ptr normal_route_{nullptr}; std::shared_ptr mrm_route_{nullptr}; + + std::unique_ptr logger_configure_; }; } // namespace mission_planner diff --git a/planning/motion_velocity_smoother/README.md b/planning/motion_velocity_smoother/README.md index d52a6431c48d8..1ff7f5982b4eb 100644 --- a/planning/motion_velocity_smoother/README.md +++ b/planning/motion_velocity_smoother/README.md @@ -140,6 +140,7 @@ After the optimization, a resampling called `post resampling` is performed befor | Name | Type | Description | Default value | | :------------------------------------- | :------- | :----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | +| `enable_lateral_acc_limit` | `bool` | To toggle the lateral acceleration filter on and off. You can switch it dynamically at runtime. | true | | `max_lateral_accel` | `double` | Max lateral acceleration limit [m/ss] | 0.5 | | `min_curve_velocity` | `double` | Min velocity at lateral acceleration limit [m/ss] | 2.74 | | `decel_distance_before_curve` | `double` | Distance to slowdown before a curve for lateral acceleration limit [m] | 3.5 | @@ -197,12 +198,13 @@ After the optimization, a resampling called `post resampling` is performed befor ### Limit steering angle rate parameters -| Name | Type | Description | Default value | -| :------------------------------- | :------- | :----------------------------------------------------------------------- | :------------ | -| `max_steering_angle_rate` | `double` | Maximum steering angle rate [degree/s] | 40.0 | -| `resample_ds` | `double` | Distance between trajectory points [m] | 0.1 | -| `curvature_threshold` | `double` | If curvature > curvature_threshold, steeringRateLimit is triggered [1/m] | 0.02 | -| `curvature_calculation_distance` | `double` | Distance of points while curvature is calculating [m] | 1.0 | +| Name | Type | Description | Default value | +| :------------------------------- | :------- | :------------------------------------------------------------------------------------ | :------------ | +| `enable_steering_rate_limit` | `bool` | To toggle the steer rate filter on and off. You can switch it dynamically at runtime. | true | +| `max_steering_angle_rate` | `double` | Maximum steering angle rate [degree/s] | 40.0 | +| `resample_ds` | `double` | Distance between trajectory points [m] | 0.1 | +| `curvature_threshold` | `double` | If curvature > curvature_threshold, steeringRateLimit is triggered [1/m] | 0.02 | +| `curvature_calculation_distance` | `double` | Distance of points while curvature is calculating [m] | 1.0 | ### Weights for optimization diff --git a/planning/motion_velocity_smoother/config/default_motion_velocity_smoother.param.yaml b/planning/motion_velocity_smoother/config/default_motion_velocity_smoother.param.yaml index bcbf0a5bb6c06..21ad85137ef36 100644 --- a/planning/motion_velocity_smoother/config/default_motion_velocity_smoother.param.yaml +++ b/planning/motion_velocity_smoother/config/default_motion_velocity_smoother.param.yaml @@ -7,12 +7,21 @@ # external velocity limit parameter margin_to_insert_external_velocity_limit: 0.3 # margin distance to insert external velocity limit [m] - # curve parameters + # ---- curve parameters ---- + # - common parameters - + curvature_calculation_distance: 5.0 # distance of points while curvature is calculating for the steer rate and lateral acceleration limit [m] + # - lateral acceleration limit parameters - + enable_lateral_acc_limit: true # To toggle the lateral acc filter on and off. You can switch it dynamically at runtime. max_lateral_accel: 0.8 # max lateral acceleration limit [m/ss] - min_curve_velocity: 2.74 # min velocity at lateral acceleration limit [m/ss] + min_curve_velocity: 2.74 # min velocity at lateral acceleration limit and steering angle rate limit [m/s] decel_distance_before_curve: 3.5 # slow speed distance before a curve for lateral acceleration limit decel_distance_after_curve: 2.0 # slow speed distance after a curve for lateral acceleration limit min_decel_for_lateral_acc_lim_filter: -2.5 # deceleration limit applied in the lateral acceleration filter to avoid sudden braking [m/ss] + # - steering angle rate limit parameters - + enable_steering_rate_limit: true # To toggle the steer rate filter on and off. You can switch it dynamically at runtime. + max_steering_angle_rate: 40.0 # maximum steering angle rate [degree/s] + resample_ds: 0.1 # distance between trajectory points [m] + curvature_threshold: 0.02 # if curvature > curvature_threshold, steeringRateLimit is triggered [1/m] # engage & replan parameters replan_vel_deviation: 5.53 # velocity deviation to replan initial velocity [m/s] @@ -48,12 +57,6 @@ post_sparse_resample_dt: 0.1 # resample time interval for sparse sampling [s] post_sparse_min_interval_distance: 1.0 # minimum points-interval length for sparse sampling [m] - # steering angle rate limit parameters - max_steering_angle_rate: 40.0 # maximum steering angle rate [degree/s] - resample_ds: 0.1 # distance between trajectory points [m] - curvature_threshold: 0.02 # if curvature > curvature_threshold, steeringRateLimit is triggered [1/m] - curvature_calculation_distance: 1.0 # distance of points while curvature is calculating [m] - # system over_stop_velocity_warn_thr: 1.389 # used to check if the optimization exceeds the input velocity on the stop point diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp b/planning/motion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp index 503fd4450cdbd..347d19f631588 100644 --- a/planning/motion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp +++ b/planning/motion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp @@ -30,6 +30,7 @@ #include "tf2_ros/transform_listener.h" #include "tier4_autoware_utils/geometry/geometry.hpp" #include "tier4_autoware_utils/math/unit_conversion.hpp" +#include "tier4_autoware_utils/ros/logger_level_configure.hpp" #include "tier4_autoware_utils/ros/self_pose_listener.hpp" #include "tier4_autoware_utils/system/stop_watch.hpp" @@ -123,6 +124,9 @@ class MotionVelocitySmootherNode : public rclcpp::Node struct Param { + bool enable_lateral_acc_limit; + bool enable_steering_rate_limit; + double max_velocity; // max velocity [m/s] double margin_to_insert_external_velocity_limit; // for external velocity limit [m] double replan_vel_deviation; // if speed error exceeds this [m/s], @@ -260,6 +264,8 @@ class MotionVelocitySmootherNode : public rclcpp::Node bool isReverse(const TrajectoryPoints & points) const; void flipVelocity(TrajectoryPoints & points) const; void publishStopWatchTime(); + + std::unique_ptr logger_configure_; }; } // namespace motion_velocity_smoother diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/resample.hpp b/planning/motion_velocity_smoother/include/motion_velocity_smoother/resample.hpp index 24632c06bad6f..80f691b5f7b40 100644 --- a/planning/motion_velocity_smoother/include/motion_velocity_smoother/resample.hpp +++ b/planning/motion_velocity_smoother/include/motion_velocity_smoother/resample.hpp @@ -15,14 +15,9 @@ #ifndef MOTION_VELOCITY_SMOOTHER__RESAMPLE_HPP_ #define MOTION_VELOCITY_SMOOTHER__RESAMPLE_HPP_ -#include "motion_utils/trajectory/trajectory.hpp" -#include "motion_velocity_smoother/trajectory_utils.hpp" - #include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" #include -#include "boost/optional.hpp" - #include namespace motion_velocity_smoother diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/smoother_base.hpp b/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/smoother_base.hpp index a7416faa3c652..beb571635f70c 100644 --- a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/smoother_base.hpp +++ b/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/smoother_base.hpp @@ -15,17 +15,11 @@ #ifndef MOTION_VELOCITY_SMOOTHER__SMOOTHER__SMOOTHER_BASE_HPP_ #define MOTION_VELOCITY_SMOOTHER__SMOOTHER__SMOOTHER_BASE_HPP_ -#include "motion_utils/trajectory/trajectory.hpp" #include "motion_velocity_smoother/resample.hpp" -#include "motion_velocity_smoother/trajectory_utils.hpp" #include "rclcpp/rclcpp.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" -#include "vehicle_info_util/vehicle_info_util.hpp" #include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" -#include "boost/optional.hpp" - #include #include @@ -33,7 +27,6 @@ namespace motion_velocity_smoother { using autoware_auto_planning_msgs::msg::TrajectoryPoint; using TrajectoryPoints = std::vector; -using vehicle_info_util::VehicleInfoUtil; class SmootherBase { diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/trajectory_utils.hpp b/planning/motion_velocity_smoother/include/motion_velocity_smoother/trajectory_utils.hpp index 58322fda322bb..ec8a89c5a3e9c 100644 --- a/planning/motion_velocity_smoother/include/motion_velocity_smoother/trajectory_utils.hpp +++ b/planning/motion_velocity_smoother/include/motion_velocity_smoother/trajectory_utils.hpp @@ -15,9 +15,6 @@ #ifndef MOTION_VELOCITY_SMOOTHER__TRAJECTORY_UTILS_HPP_ #define MOTION_VELOCITY_SMOOTHER__TRAJECTORY_UTILS_HPP_ -#include "motion_utils/trajectory/trajectory.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" - #include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" #include "geometry_msgs/msg/pose.hpp" diff --git a/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp b/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp index d3c77aa0faa84..6b5da01b9cb09 100644 --- a/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp +++ b/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp @@ -39,7 +39,7 @@ MotionVelocitySmootherNode::MotionVelocitySmootherNode(const rclcpp::NodeOptions using std::placeholders::_1; // set common params - const auto vehicle_info = VehicleInfoUtil(*this).getVehicleInfo(); + const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); wheelbase_ = vehicle_info.wheel_base_m; initCommonParam(); over_stop_velocity_warn_thr_ = declare_parameter("over_stop_velocity_warn_thr"); @@ -79,7 +79,7 @@ MotionVelocitySmootherNode::MotionVelocitySmootherNode(const rclcpp::NodeOptions debug_closest_acc_ = create_publisher("~/closest_acceleration", 1); debug_closest_jerk_ = create_publisher("~/closest_jerk", 1); debug_closest_max_velocity_ = create_publisher("~/closest_max_velocity", 1); - debug_calculation_time_ = create_publisher("~/calculation_time", 1); + debug_calculation_time_ = create_publisher("~/debug/processing_time_ms", 1); pub_trajectory_raw_ = create_publisher("~/debug/trajectory_raw", 1); pub_trajectory_vel_lim_ = create_publisher("~/debug/trajectory_external_velocity_limited", 1); @@ -99,6 +99,8 @@ MotionVelocitySmootherNode::MotionVelocitySmootherNode(const rclcpp::NodeOptions pub_velocity_limit_->publish(max_vel_msg); clock_ = get_clock(); + + logger_configure_ = std::make_unique(this); } void MotionVelocitySmootherNode::setupSmoother(const double wheelbase) @@ -165,6 +167,9 @@ rcl_interfaces::msg::SetParametersResult MotionVelocitySmootherNode::onParameter { auto & p = node_param_; + update_param_bool("enable_lateral_acc_limit", p.enable_lateral_acc_limit); + update_param_bool("enable_steering_rate_limit", p.enable_steering_rate_limit); + update_param("max_velocity", p.max_velocity); update_param( "margin_to_insert_external_velocity_limit", p.margin_to_insert_external_velocity_limit); @@ -266,6 +271,9 @@ rcl_interfaces::msg::SetParametersResult MotionVelocitySmootherNode::onParameter void MotionVelocitySmootherNode::initCommonParam() { auto & p = node_param_; + p.enable_lateral_acc_limit = declare_parameter("enable_lateral_acc_limit"); + p.enable_steering_rate_limit = declare_parameter("enable_steering_rate_limit"); + p.max_velocity = declare_parameter("max_velocity"); // 72.0 kmph p.margin_to_insert_external_velocity_limit = declare_parameter("margin_to_insert_external_velocity_limit"); @@ -572,12 +580,17 @@ bool MotionVelocitySmootherNode::smoothVelocity( // Lateral acceleration limit constexpr bool enable_smooth_limit = true; constexpr bool use_resampling = true; - const auto traj_lateral_acc_filtered = smoother_->applyLateralAccelerationFilter( - input, initial_motion.vel, initial_motion.acc, enable_smooth_limit, use_resampling); + const auto traj_lateral_acc_filtered = + node_param_.enable_lateral_acc_limit + ? smoother_->applyLateralAccelerationFilter( + input, initial_motion.vel, initial_motion.acc, enable_smooth_limit, use_resampling) + : input; // Steering angle rate limit (Note: set use_resample = false since it is resampled above) const auto traj_steering_rate_limited = - smoother_->applySteeringRateLimit(traj_lateral_acc_filtered, false); + node_param_.enable_steering_rate_limit + ? smoother_->applySteeringRateLimit(traj_lateral_acc_filtered, false) + : traj_lateral_acc_filtered; // Resample trajectory with ego-velocity based interval distance auto traj_resampled = smoother_->resampleTrajectory( diff --git a/planning/motion_velocity_smoother/src/resample.cpp b/planning/motion_velocity_smoother/src/resample.cpp index fbf129f105c7f..37f3ce953bd48 100644 --- a/planning/motion_velocity_smoother/src/resample.cpp +++ b/planning/motion_velocity_smoother/src/resample.cpp @@ -16,6 +16,8 @@ #include "motion_utils/resample/resample.hpp" #include "motion_utils/trajectory/tmp_conversion.hpp" +#include "motion_utils/trajectory/trajectory.hpp" +#include "motion_velocity_smoother/trajectory_utils.hpp" #include #include diff --git a/planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp b/planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp index a02e432e1e584..d1694d7420ca2 100644 --- a/planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp +++ b/planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp @@ -16,6 +16,7 @@ #include "motion_utils/resample/resample.hpp" #include "motion_utils/trajectory/tmp_conversion.hpp" +#include "motion_velocity_smoother/trajectory_utils.hpp" #include #include diff --git a/planning/motion_velocity_smoother/src/smoother/smoother_base.cpp b/planning/motion_velocity_smoother/src/smoother/smoother_base.cpp index 2e58961d54c00..4e5efdbfc32e4 100644 --- a/planning/motion_velocity_smoother/src/smoother/smoother_base.cpp +++ b/planning/motion_velocity_smoother/src/smoother/smoother_base.cpp @@ -16,8 +16,10 @@ #include "motion_utils/resample/resample.hpp" #include "motion_utils/trajectory/tmp_conversion.hpp" +#include "motion_utils/trajectory/trajectory.hpp" #include "motion_velocity_smoother/resample.hpp" #include "motion_velocity_smoother/trajectory_utils.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" #include "tier4_autoware_utils/math/unit_conversion.hpp" #include @@ -26,7 +28,6 @@ namespace motion_velocity_smoother { -using vehicle_info_util::VehicleInfoUtil; SmootherBase::SmootherBase(rclcpp::Node & node) { @@ -102,8 +103,6 @@ TrajectoryPoints SmootherBase::applyLateralAccelerationFilter( return input; // cannot calculate lateral acc. do nothing. } - constexpr double curvature_calc_dist = 5.0; // [m] calc curvature with 5m away points - // Interpolate with constant interval distance for lateral acceleration calculation. TrajectoryPoints output; const double points_interval = @@ -123,8 +122,8 @@ TrajectoryPoints SmootherBase::applyLateralAccelerationFilter( output = input; } - const size_t idx_dist = - static_cast(std::max(static_cast((curvature_calc_dist) / points_interval), 1)); + const size_t idx_dist = static_cast( + std::max(static_cast((base_param_.curvature_calculation_distance) / points_interval), 1)); // Calculate curvature assuming the trajectory points interval is constant const auto curvature_v = trajectory_utils::calcTrajectoryCurvatureFrom3Points(output, idx_dist); diff --git a/planning/motion_velocity_smoother/src/trajectory_utils.cpp b/planning/motion_velocity_smoother/src/trajectory_utils.cpp index ab5db29ad2fc0..dd361696a5dad 100644 --- a/planning/motion_velocity_smoother/src/trajectory_utils.cpp +++ b/planning/motion_velocity_smoother/src/trajectory_utils.cpp @@ -16,6 +16,8 @@ #include "interpolation/linear_interpolation.hpp" #include "interpolation/spline_interpolation.hpp" +#include "motion_utils/trajectory/trajectory.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" #include #include diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/common_structs.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/common_structs.hpp index 1b8a40ef8ef02..4f4f01bf1ec9f 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/common_structs.hpp +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/common_structs.hpp @@ -17,7 +17,8 @@ #include "obstacle_avoidance_planner/type_alias.hpp" #include "rclcpp/rclcpp.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" +#include "tier4_autoware_utils/ros/update_param.hpp" +#include "tier4_autoware_utils/system/stop_watch.hpp" #include #include @@ -44,7 +45,11 @@ struct PlannerData struct TimeKeeper { - void init() { accumulated_msg = "\n"; } + void init() + { + accumulated_msg = "\n"; + accumulated_time = 0.0; + } template TimeKeeper & operator<<(const T & msg) @@ -70,6 +75,7 @@ struct TimeKeeper { const double elapsed_time = stop_watch_.toc(func_name); *this << white_spaces << func_name << ":= " << elapsed_time << " [ms]"; + accumulated_time = elapsed_time; endLine(); } @@ -79,6 +85,10 @@ struct TimeKeeper std::string accumulated_msg = "\n"; std::stringstream latest_stream; + double getAccumulatedTime() const { return accumulated_time; } + + double accumulated_time{0.0}; + tier4_autoware_utils::StopWatch< std::chrono::milliseconds, std::chrono::microseconds, std::chrono::steady_clock> stop_watch_; diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/mpt_optimizer.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/mpt_optimizer.hpp index ea2e18b099f1d..b87cfc7a1e743 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/mpt_optimizer.hpp +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/mpt_optimizer.hpp @@ -22,7 +22,7 @@ #include "obstacle_avoidance_planner/state_equation_generator.hpp" #include "obstacle_avoidance_planner/type_alias.hpp" #include "osqp_interface/osqp_interface.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" #include "vehicle_info_util/vehicle_info_util.hpp" #include diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp index f716f497b90da..9207277d0bc98 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp @@ -15,13 +15,13 @@ #ifndef OBSTACLE_AVOIDANCE_PLANNER__NODE_HPP_ #define OBSTACLE_AVOIDANCE_PLANNER__NODE_HPP_ -#include "motion_utils/motion_utils.hpp" +#include "motion_utils/trajectory/trajectory.hpp" #include "obstacle_avoidance_planner/common_structs.hpp" #include "obstacle_avoidance_planner/mpt_optimizer.hpp" #include "obstacle_avoidance_planner/replan_checker.hpp" #include "obstacle_avoidance_planner/type_alias.hpp" #include "rclcpp/rclcpp.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" +#include "tier4_autoware_utils/ros/logger_level_configure.hpp" #include "vehicle_info_util/vehicle_info_util.hpp" #include @@ -90,7 +90,8 @@ class ObstacleAvoidancePlanner : public rclcpp::Node // debug publisher rclcpp::Publisher::SharedPtr debug_extended_traj_pub_; rclcpp::Publisher::SharedPtr debug_markers_pub_; - rclcpp::Publisher::SharedPtr debug_calculation_time_pub_; + rclcpp::Publisher::SharedPtr debug_calculation_time_str_pub_; + rclcpp::Publisher::SharedPtr debug_calculation_time_float_pub_; // parameter callback rcl_interfaces::msg::SetParametersResult onParam( @@ -128,6 +129,8 @@ class ObstacleAvoidancePlanner : public rclcpp::Node private: double vehicle_stop_margin_outside_drivable_area_; + + std::unique_ptr logger_configure_; }; } // namespace obstacle_avoidance_planner diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/type_alias.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/type_alias.hpp index 0f45e1223551f..b02d2232a4aa1 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/type_alias.hpp +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/type_alias.hpp @@ -24,6 +24,7 @@ #include "geometry_msgs/msg/twist.hpp" #include "nav_msgs/msg/odometry.hpp" #include "std_msgs/msg/header.hpp" +#include "tier4_debug_msgs/msg/float64_stamped.hpp" #include "tier4_debug_msgs/msg/string_stamped.hpp" #include "visualization_msgs/msg/marker_array.hpp" @@ -42,6 +43,7 @@ using nav_msgs::msg::Odometry; using visualization_msgs::msg::Marker; using visualization_msgs::msg::MarkerArray; // debug +using tier4_debug_msgs::msg::Float64Stamped; using tier4_debug_msgs::msg::StringStamped; } // namespace obstacle_avoidance_planner diff --git a/planning/obstacle_avoidance_planner/rviz/obstacle_avoidance_planner.rviz b/planning/obstacle_avoidance_planner/rviz/obstacle_avoidance_planner.rviz index 8e62886d255ea..c3cd89bbce9b9 100644 --- a/planning/obstacle_avoidance_planner/rviz/obstacle_avoidance_planner.rviz +++ b/planning/obstacle_avoidance_planner/rviz/obstacle_avoidance_planner.rviz @@ -24,8 +24,6 @@ Panels: - /Current View1 Name: Views Splitter Ratio: 0.5 - - Class: tier4_localization_rviz_plugin/InitialPoseButtonPanel - Name: InitialPoseButtonPanel - Class: AutowareDateTimePanel Name: AutowareDateTimePanel - Class: rviz_plugins::AutowareStatePanel @@ -1451,8 +1449,6 @@ Window Geometry: Height: 2032 Hide Left Dock: false Hide Right Dock: false - InitialPoseButtonPanel: - collapsed: false QMainWindow State: 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 RecognitionResultOnImage: collapsed: false diff --git a/planning/obstacle_avoidance_planner/src/debug_marker.cpp b/planning/obstacle_avoidance_planner/src/debug_marker.cpp index 79f3dfd2fe2e4..2f8babf103877 100644 --- a/planning/obstacle_avoidance_planner/src/debug_marker.cpp +++ b/planning/obstacle_avoidance_planner/src/debug_marker.cpp @@ -14,7 +14,6 @@ #include "obstacle_avoidance_planner/debug_marker.hpp" -#include "motion_utils/motion_utils.hpp" #include "obstacle_avoidance_planner/mpt_optimizer.hpp" #include "visualization_msgs/msg/marker_array.hpp" @@ -48,21 +47,22 @@ MarkerArray getFootprintsMarkerArray( const auto & traj_point = mpt_traj.at(i); - const double half_width = vehicle_info.vehicle_width_m / 2.0; + const double base_to_right = (vehicle_info.wheel_tread_m / 2.0) + vehicle_info.right_overhang_m; + const double base_to_left = (vehicle_info.wheel_tread_m / 2.0) + vehicle_info.left_overhang_m; const double base_to_front = vehicle_info.vehicle_length_m - vehicle_info.rear_overhang_m; const double base_to_rear = vehicle_info.rear_overhang_m; marker.points.push_back( - tier4_autoware_utils::calcOffsetPose(traj_point.pose, base_to_front, -half_width, 0.0) + tier4_autoware_utils::calcOffsetPose(traj_point.pose, base_to_front, base_to_left, 0.0) .position); marker.points.push_back( - tier4_autoware_utils::calcOffsetPose(traj_point.pose, base_to_front, half_width, 0.0) + tier4_autoware_utils::calcOffsetPose(traj_point.pose, base_to_front, -base_to_right, 0.0) .position); marker.points.push_back( - tier4_autoware_utils::calcOffsetPose(traj_point.pose, -base_to_rear, half_width, 0.0) + tier4_autoware_utils::calcOffsetPose(traj_point.pose, -base_to_rear, -base_to_right, 0.0) .position); marker.points.push_back( - tier4_autoware_utils::calcOffsetPose(traj_point.pose, -base_to_rear, -half_width, 0.0) + tier4_autoware_utils::calcOffsetPose(traj_point.pose, -base_to_rear, base_to_left, 0.0) .position); marker.points.push_back(marker.points.front()); @@ -72,8 +72,8 @@ MarkerArray getFootprintsMarkerArray( } MarkerArray getBoundsWidthMarkerArray( - const std::vector & ref_points, const double vehicle_width, - const size_t sampling_num) + const std::vector & ref_points, + const vehicle_info_util::VehicleInfo & vehicle_info, const size_t sampling_num) { const auto current_time = rclcpp::Clock().now(); MarkerArray marker_array; @@ -106,8 +106,10 @@ MarkerArray getBoundsWidthMarkerArray( } const geometry_msgs::msg::Pose & pose = ref_points.at(i).pose_on_constraints.at(bound_idx); + const double base_to_right = + (vehicle_info.wheel_tread_m / 2.0) + vehicle_info.right_overhang_m; const double lb_y = - ref_points.at(i).bounds_on_constraints.at(bound_idx).lower_bound - vehicle_width / 2.0; + ref_points.at(i).bounds_on_constraints.at(bound_idx).lower_bound - base_to_right; const auto lb = tier4_autoware_utils::calcOffsetPose(pose, 0.0, lb_y, 0.0).position; lb_marker.points.push_back(pose.position); @@ -126,8 +128,10 @@ MarkerArray getBoundsWidthMarkerArray( } const geometry_msgs::msg::Pose & pose = ref_points.at(i).pose_on_constraints.at(bound_idx); + const double base_to_left = + (vehicle_info.wheel_tread_m / 2.0) + vehicle_info.left_overhang_m; const double ub_y = - ref_points.at(i).bounds_on_constraints.at(bound_idx).upper_bound + vehicle_width / 2.0; + ref_points.at(i).bounds_on_constraints.at(bound_idx).upper_bound + base_to_left; const auto ub = tier4_autoware_utils::calcOffsetPose(pose, 0.0, ub_y, 0.0).position; ub_marker.points.push_back(pose.position); @@ -141,7 +145,8 @@ MarkerArray getBoundsWidthMarkerArray( } MarkerArray getBoundsLineMarkerArray( - const std::vector & ref_points, const double vehicle_width) + const std::vector & ref_points, + const vehicle_info_util::VehicleInfo & vehicle_info) { MarkerArray marker_array; @@ -159,12 +164,13 @@ MarkerArray getBoundsLineMarkerArray( for (size_t i = 0; i < ref_points.size(); i++) { const geometry_msgs::msg::Pose & pose = ref_points.at(i).pose; - - const double ub_y = ref_points.at(i).bounds.upper_bound + vehicle_width / 2.0; + const double base_to_right = (vehicle_info.wheel_tread_m / 2.0) + vehicle_info.right_overhang_m; + const double base_to_left = (vehicle_info.wheel_tread_m / 2.0) + vehicle_info.left_overhang_m; + const double ub_y = ref_points.at(i).bounds.upper_bound + base_to_left; const auto ub = tier4_autoware_utils::calcOffsetPose(pose, 0.0, ub_y, 0.0).position; ub_marker.points.push_back(ub); - const double lb_y = ref_points.at(i).bounds.lower_bound - vehicle_width / 2.0; + const double lb_y = ref_points.at(i).bounds.lower_bound - base_to_right; const auto lb = tier4_autoware_utils::calcOffsetPose(pose, 0.0, lb_y, 0.0).position; lb_marker.points.push_back(lb); } @@ -176,8 +182,9 @@ MarkerArray getBoundsLineMarkerArray( MarkerArray getVehicleCircleLinesMarkerArray( const std::vector & ref_points, - const std::vector & vehicle_circle_longitudinal_offsets, const double vehicle_width, - const size_t sampling_num, const std::string & ns) + const std::vector & vehicle_circle_longitudinal_offsets, + const vehicle_info_util::VehicleInfo & vehicle_info, const size_t sampling_num, + const std::string & ns) { const auto current_time = rclcpp::Clock().now(); MarkerArray msg; @@ -207,11 +214,13 @@ MarkerArray getVehicleCircleLinesMarkerArray( auto base_pose = tier4_autoware_utils::calcOffsetPose(pose_with_deviation, d, 0.0, 0.0); base_pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(ref_point.getYaw() + ref_point.alpha); - + const double base_to_right = + (vehicle_info.wheel_tread_m / 2.0) + vehicle_info.right_overhang_m; + const double base_to_left = (vehicle_info.wheel_tread_m / 2.0) + vehicle_info.left_overhang_m; const auto ub = - tier4_autoware_utils::calcOffsetPose(base_pose, 0.0, vehicle_width / 2.0, 0.0).position; + tier4_autoware_utils::calcOffsetPose(base_pose, 0.0, base_to_left, 0.0).position; const auto lb = - tier4_autoware_utils::calcOffsetPose(base_pose, 0.0, -vehicle_width / 2.0, 0.0).position; + tier4_autoware_utils::calcOffsetPose(base_pose, 0.0, -base_to_right, 0.0).position; marker.points.push_back(ub); marker.points.push_back(lb); @@ -369,13 +378,12 @@ MarkerArray getDebugMarker( MarkerArray marker_array; // bounds line - appendMarkerArray( - getBoundsLineMarkerArray(debug_data.ref_points, vehicle_info.vehicle_width_m), &marker_array); + appendMarkerArray(getBoundsLineMarkerArray(debug_data.ref_points, vehicle_info), &marker_array); // bounds width appendMarkerArray( getBoundsWidthMarkerArray( - debug_data.ref_points, vehicle_info.vehicle_width_m, debug_data.mpt_visualize_sampling_num), + debug_data.ref_points, vehicle_info, debug_data.mpt_visualize_sampling_num), &marker_array); // current vehicle circles @@ -405,9 +413,8 @@ MarkerArray getDebugMarker( // vehicle circle line appendMarkerArray( getVehicleCircleLinesMarkerArray( - debug_data.ref_points, debug_data.vehicle_circle_longitudinal_offsets, - vehicle_info.vehicle_width_m, debug_data.mpt_visualize_sampling_num, - "vehicle_circle_lines"), + debug_data.ref_points, debug_data.vehicle_circle_longitudinal_offsets, vehicle_info, + debug_data.mpt_visualize_sampling_num, "vehicle_circle_lines"), &marker_array); // footprint by drivable area diff --git a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp index 0a55cc8d91f8e..765136b3cf6f6 100644 --- a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp +++ b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp @@ -15,11 +15,12 @@ #include "obstacle_avoidance_planner/mpt_optimizer.hpp" #include "interpolation/spline_interpolation_points_2d.hpp" -#include "motion_utils/motion_utils.hpp" +#include "motion_utils/trajectory/trajectory.hpp" #include "obstacle_avoidance_planner/utils/geometry_utils.hpp" #include "obstacle_avoidance_planner/utils/trajectory_utils.hpp" #include "tf2/utils.h" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" +#include "tier4_autoware_utils/math/normalization.hpp" #include #include @@ -35,9 +36,11 @@ std::tuple, std::vector> calcVehicleCirclesByUniform const vehicle_info_util::VehicleInfo & vehicle_info, const size_t circle_num, const double radius_ratio) { + const double lateral_offset = + abs(vehicle_info.right_overhang_m - vehicle_info.left_overhang_m) / 2.0; const double radius = std::hypot( vehicle_info.vehicle_length_m / static_cast(circle_num) / 2.0, - vehicle_info.vehicle_width_m / 2.0) * + vehicle_info.vehicle_width_m / 2.0 + lateral_offset) * radius_ratio; const std::vector radiuses(circle_num, radius); @@ -58,16 +61,18 @@ std::tuple, std::vector> calcVehicleCirclesByBicycle if (circle_num < 2) { throw std::invalid_argument("circle_num is less than 2."); } - + const double lateral_offset = + abs(vehicle_info.right_overhang_m - vehicle_info.left_overhang_m) / 2.0; // 1st circle (rear wheel) - const double rear_radius = vehicle_info.vehicle_width_m / 2.0 * rear_radius_ratio; + const double rear_radius = + vehicle_info.vehicle_width_m / 2.0 + lateral_offset * rear_radius_ratio; const double rear_lon_offset = 0.0; // 2nd circle (front wheel) const double front_radius = std::hypot( vehicle_info.vehicle_length_m / static_cast(circle_num) / 2.0, - vehicle_info.vehicle_width_m / 2.0) * + vehicle_info.vehicle_width_m / 2.0 + lateral_offset) * front_radius_ratio; const double unit_lon_length = vehicle_info.vehicle_length_m / static_cast(circle_num); @@ -83,8 +88,9 @@ std::tuple, std::vector> calcVehicleCirclesByFitting if (circle_num < 2) { throw std::invalid_argument("circle_num is less than 2."); } - - const double radius = vehicle_info.vehicle_width_m / 2.0; + const double lateral_offset = + abs(vehicle_info.right_overhang_m - vehicle_info.left_overhang_m) / 2.0; + const double radius = vehicle_info.vehicle_width_m / 2.0 + lateral_offset; std::vector radiuses(circle_num, radius); const double unit_lon_length = diff --git a/planning/obstacle_avoidance_planner/src/node.cpp b/planning/obstacle_avoidance_planner/src/node.cpp index 6c4730e9d86f9..da31c7f469555 100644 --- a/planning/obstacle_avoidance_planner/src/node.cpp +++ b/planning/obstacle_avoidance_planner/src/node.cpp @@ -15,6 +15,7 @@ #include "obstacle_avoidance_planner/node.hpp" #include "interpolation/spline_interpolation_points_2d.hpp" +#include "motion_utils/marker/marker_helper.hpp" #include "obstacle_avoidance_planner/debug_marker.hpp" #include "obstacle_avoidance_planner/utils/geometry_utils.hpp" #include "obstacle_avoidance_planner/utils/trajectory_utils.hpp" @@ -44,6 +45,14 @@ StringStamped createStringStamped(const rclcpp::Time & now, const std::string & return msg; } +Float64Stamped createFloat64Stamped(const rclcpp::Time & now, const float & data) +{ + Float64Stamped msg; + msg.stamp = now; + msg.data = data; + return msg; +} + void setZeroVelocityAfterStopPoint(std::vector & traj_points) { const auto opt_zero_vel_idx = motion_utils::searchZeroVelocityIndex(traj_points); @@ -91,7 +100,9 @@ ObstacleAvoidancePlanner::ObstacleAvoidancePlanner(const rclcpp::NodeOptions & n // debug publisher debug_extended_traj_pub_ = create_publisher("~/debug/extended_traj", 1); debug_markers_pub_ = create_publisher("~/debug/marker", 1); - debug_calculation_time_pub_ = create_publisher("~/debug/calculation_time", 1); + debug_calculation_time_str_pub_ = create_publisher("~/debug/calculation_time", 1); + debug_calculation_time_float_pub_ = + create_publisher("~/debug/processing_time_ms", 1); { // parameters // parameter for option @@ -139,6 +150,8 @@ ObstacleAvoidancePlanner::ObstacleAvoidancePlanner(const rclcpp::NodeOptions & n // initialized. set_param_res_ = this->add_on_set_parameters_callback( std::bind(&ObstacleAvoidancePlanner::onParam, this, std::placeholders::_1)); + + logger_configure_ = std::make_unique(this); } rcl_interfaces::msg::SetParametersResult ObstacleAvoidancePlanner::onParam( @@ -250,7 +263,9 @@ void ObstacleAvoidancePlanner::onPath(const Path::ConstSharedPtr path_ptr) // publish calculation_time // NOTE: This function must be called after measuring onPath calculation time const auto calculation_time_msg = createStringStamped(now(), time_keeper_ptr_->getLog()); - debug_calculation_time_pub_->publish(calculation_time_msg); + debug_calculation_time_str_pub_->publish(calculation_time_msg); + debug_calculation_time_float_pub_->publish( + createFloat64Stamped(now(), time_keeper_ptr_->getAccumulatedTime())); const auto output_traj_msg = trajectory_utils::createTrajectory(path_ptr->header, full_traj_points); @@ -442,7 +457,24 @@ void ObstacleAvoidancePlanner::applyInputVelocity( ego_nearest_param_.yaw_threshold); // calculate and insert stop pose on output trajectory - if (stop_seg_idx) { + const bool is_stop_point_inside_trajectory = [&]() { + if (!stop_seg_idx) { + return false; + } + if (*stop_seg_idx == output_traj_points.size() - 2) { + const double signed_projected_length_to_segment = + motion_utils::calcLongitudinalOffsetToSegment( + output_traj_points, *stop_seg_idx, input_stop_pose.position); + const double segment_length = + motion_utils::calcSignedArcLength(output_traj_points, *stop_seg_idx, *stop_seg_idx + 1); + if (segment_length < signed_projected_length_to_segment) { + // NOTE: input_stop_pose is outside output_traj_points. + return false; + } + } + return true; + }(); + if (is_stop_point_inside_trajectory) { trajectory_utils::insertStopPoint(output_traj_points, input_stop_pose, *stop_seg_idx); } } @@ -572,25 +604,25 @@ std::vector ObstacleAvoidancePlanner::extendTrajectory( const auto joint_end_traj_point_idx = trajectory_utils::getPointIndexAfter( traj_points, joint_start_pose.position, joint_start_traj_seg_idx, joint_traj_max_length_for_smoothing, joint_traj_min_length_for_smoothing); + if (!joint_end_traj_point_idx) { + return trajectory_utils::resampleTrajectoryPoints( + optimized_traj_points, traj_param_.output_delta_arc_length); + } // calculate full trajectory points const auto full_traj_points = [&]() { - if (!joint_end_traj_point_idx) { - return optimized_traj_points; - } - - const auto extended_traj_points = std::vector{ + auto extended_traj_points = std::vector{ traj_points.begin() + *joint_end_traj_point_idx, traj_points.end()}; - // NOTE: if optimized_traj_points's back is non zero velocity and extended_traj_points' front is - // zero velocity, the zero velocity will be inserted in the whole joint trajectory. - auto modified_optimized_traj_points = optimized_traj_points; - if (!extended_traj_points.empty() && !modified_optimized_traj_points.empty()) { - modified_optimized_traj_points.back().longitudinal_velocity_mps = - extended_traj_points.front().longitudinal_velocity_mps; + if (!extended_traj_points.empty() && !optimized_traj_points.empty()) { + // NOTE: Without this code, if optimized_traj_points's back is non zero velocity and + // extended_traj_points' front + // is zero velocity, the zero velocity will be inserted in the whole joint trajectory. + // The input stop point will be inserted explicitly in the latter part. + extended_traj_points.front().longitudinal_velocity_mps = + optimized_traj_points.back().longitudinal_velocity_mps; } - - return concatVectors(modified_optimized_traj_points, extended_traj_points); + return concatVectors(optimized_traj_points, extended_traj_points); }(); // resample trajectory points @@ -598,7 +630,7 @@ std::vector ObstacleAvoidancePlanner::extendTrajectory( full_traj_points, traj_param_.output_delta_arc_length); // update stop velocity on joint - for (size_t i = joint_start_traj_seg_idx + 1; i <= joint_end_traj_point_idx; ++i) { + for (size_t i = joint_start_traj_seg_idx + 1; i <= *joint_end_traj_point_idx; ++i) { if (hasZeroVelocity(traj_points.at(i))) { if (i != 0 && !hasZeroVelocity(traj_points.at(i - 1))) { // Here is when current point is 0 velocity, but previous point is not 0 velocity. diff --git a/planning/obstacle_avoidance_planner/src/replan_checker.cpp b/planning/obstacle_avoidance_planner/src/replan_checker.cpp index 8d281ced9a5ac..6d7c6e702471a 100644 --- a/planning/obstacle_avoidance_planner/src/replan_checker.cpp +++ b/planning/obstacle_avoidance_planner/src/replan_checker.cpp @@ -14,9 +14,10 @@ #include "obstacle_avoidance_planner/replan_checker.hpp" -#include "motion_utils/motion_utils.hpp" +#include "motion_utils/trajectory/trajectory.hpp" #include "obstacle_avoidance_planner/utils/trajectory_utils.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" +#include "tier4_autoware_utils/ros/update_param.hpp" #include diff --git a/planning/obstacle_avoidance_planner/src/utils/geometry_utils.cpp b/planning/obstacle_avoidance_planner/src/utils/geometry_utils.cpp index 67b500571fa06..3f35de9147e6a 100644 --- a/planning/obstacle_avoidance_planner/src/utils/geometry_utils.cpp +++ b/planning/obstacle_avoidance_planner/src/utils/geometry_utils.cpp @@ -14,15 +14,21 @@ #include "obstacle_avoidance_planner/utils/geometry_utils.hpp" -#include "motion_utils/motion_utils.hpp" +#include "motion_utils/trajectory/trajectory.hpp" #include "obstacle_avoidance_planner/mpt_optimizer.hpp" #include "tf2/utils.h" +#include + #include "autoware_auto_planning_msgs/msg/path_point.hpp" #include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" #include "geometry_msgs/msg/point32.hpp" #include "geometry_msgs/msg/pose.hpp" +#include +#include +#include + #include #include #include diff --git a/planning/obstacle_avoidance_planner/src/utils/trajectory_utils.cpp b/planning/obstacle_avoidance_planner/src/utils/trajectory_utils.cpp index 8390cf379586b..e86782c9cb0cf 100644 --- a/planning/obstacle_avoidance_planner/src/utils/trajectory_utils.cpp +++ b/planning/obstacle_avoidance_planner/src/utils/trajectory_utils.cpp @@ -14,7 +14,9 @@ #include "obstacle_avoidance_planner/utils/trajectory_utils.hpp" -#include "motion_utils/motion_utils.hpp" +#include "motion_utils/resample/resample.hpp" +#include "motion_utils/trajectory/tmp_conversion.hpp" +#include "motion_utils/trajectory/trajectory.hpp" #include "obstacle_avoidance_planner/mpt_optimizer.hpp" #include "obstacle_avoidance_planner/utils/geometry_utils.hpp" #include "tf2/utils.h" diff --git a/planning/obstacle_cruise_planner/README.md b/planning/obstacle_cruise_planner/README.md index 314f9dcc29393..260f302791079 100644 --- a/planning/obstacle_cruise_planner/README.md +++ b/planning/obstacle_cruise_planner/README.md @@ -230,14 +230,19 @@ $$ ### Slow down planning -| Parameter | Type | Description | -| ---------------------------- | ------ | ------------------------------------------------------------------- | -| `slow_down.min_lat_velocity` | double | minimum velocity to linearly calculate slow down velocity [m] | -| `slow_down.max_lat_velocity` | double | maximum velocity to linearly calculate slow down velocity [m] | -| `slow_down.min_lat_margin` | double | minimum lateral margin to linearly calculate slow down velocity [m] | -| `slow_down.max_lat_margin` | double | maximum lateral margin to linearly calculate slow down velocity [m] | - -The role of the slow down planning is inserting slow down velocity in the trajectory where the trajectory points are close to the obstacles. +| Parameter | Type | Description | +| ----------------------------------------------- | -------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------ | +| `slow_down.labels` | vector(string) | A vector of labels for customizing obstacle-label-based slow down behavior. Each label represents an obstacle type that will be treated differently when applying slow down. The possible labels are ("default" (Mandatory), "unknown","car","truck","bus","trailer","motorcycle","bicycle" or "pedestrian") | +| `slow_down.default.min_lat_velocity` | double | minimum velocity to linearly calculate slow down velocity [m]. Note: This default value will be used when the detected obstacle label does not match any of the slow_down.labels | +| `slow_down.default.max_lat_velocity` | double | maximum velocity to linearly calculate slow down velocity [m]. Note: This default value will be used when the detected obstacle label does not match any of the slow_down.labels | +| `slow_down.default.min_lat_margin` | double | minimum lateral margin to linearly calculate slow down velocity [m]. Note: This default value will be used when the detected obstacle label does not match any of the slow_down.labels | +| `slow_down.default.max_lat_margin` | double | maximum lateral margin to linearly calculate slow down velocity [m]. Note: This default value will be used when the detected obstacle label does not match any of the slow_down.labels | +| `(optional) slow_down."label".min_lat_velocity` | double | minimum velocity to linearly calculate slow down velocity [m]. Note: only for obstacles specified in `slow_down.labels` | +| `(optional) slow_down."label".max_lat_velocity` | double | maximum velocity to linearly calculate slow down velocity [m]. Note: only for obstacles specified in `slow_down.labels` | +| `(optional) slow_down."label".min_lat_margin` | double | minimum lateral margin to linearly calculate slow down velocity [m]. Note: only for obstacles specified in `slow_down.labels` | +| `(optional) slow_down."label".max_lat_margin` | double | maximum lateral margin to linearly calculate slow down velocity [m]. Note: only for obstacles specified in `slow_down.labels` | + +The role of the slow down planning is inserting slow down velocity in the trajectory where the trajectory points are close to the obstacles. The parameters can be customized depending on the obstacle type (see `slow_down.labels`), making it possible to adjust the slow down behavior depending if the obstacle is a pedestrian, bicycle, car, etc. The closest point on the obstacle to the ego's trajectory is calculated. Then, the slow down velocity is calculated by linear interpolation with the distance between the point and trajectory as follows. 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 e2012fab43ba4..123d0dda93b7a 100644 --- a/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml +++ b/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml @@ -20,6 +20,11 @@ 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] + stop_on_curve: + enable_approaching: true # false + additional_safe_distance_margin: 0.0 # [m] + min_safe_distance_margin: 3.0 # [m] + suppress_sudden_obstacle_stop: true stop_obstacle_type: unknown: true @@ -53,7 +58,7 @@ pedestrian: false slow_down_obstacle_type: - unknown: true + unknown: false car: true truck: true bus: true @@ -101,6 +106,13 @@ successive_num_to_entry_slow_down_condition: 5 successive_num_to_exit_slow_down_condition: 5 + # consider the current ego pose (it is not the nearest pose on the reference trajectory) + # Both the lateral error and the yaw error are assumed to decrease to zero by the time duration "time_to_convergence" + # The both errors decrease with constant rates against the time. + consider_current_pose: + enable_to_consider_current_pose: false + time_to_convergence: 1.5 #[s] + cruise: pid_based_planner: use_velocity_limit_based_planner: true @@ -167,10 +179,19 @@ slow_down: # parameters to calculate slow down velocity by linear interpolation - min_lat_margin: 0.2 - max_lat_margin: 1.0 - min_ego_velocity: 2.0 - max_ego_velocity: 8.0 + labels: + - "default" + - "pedestrian" + default: + min_lat_margin: 0.2 + max_lat_margin: 1.0 + min_ego_velocity: 2.0 + max_ego_velocity: 8.0 + pedestrian: + min_lat_margin: 0.5 + max_lat_margin: 1.5 + min_ego_velocity: 1.0 + max_ego_velocity: 2.0 time_margin_on_target_velocity: 1.5 # [s] diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp index 12ebadf770996..c6cb73a680e5a 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp @@ -15,9 +15,13 @@ #ifndef OBSTACLE_CRUISE_PLANNER__COMMON_STRUCTS_HPP_ #define OBSTACLE_CRUISE_PLANNER__COMMON_STRUCTS_HPP_ -#include "motion_utils/motion_utils.hpp" +#include "motion_utils/trajectory/interpolation.hpp" +#include "motion_utils/trajectory/tmp_conversion.hpp" +#include "motion_utils/trajectory/trajectory.hpp" #include "obstacle_cruise_planner/type_alias.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" +#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" +#include "tier4_autoware_utils/ros/update_param.hpp" +#include "tier4_autoware_utils/ros/uuid_helper.hpp" #include @@ -104,12 +108,15 @@ struct StopObstacle : public TargetObstacleInterface { StopObstacle( const std::string & arg_uuid, const rclcpp::Time & arg_stamp, - const geometry_msgs::msg::Pose & arg_pose, const double arg_lon_velocity, - const double arg_lat_velocity, const geometry_msgs::msg::Point arg_collision_point) + const geometry_msgs::msg::Pose & arg_pose, const Shape & arg_shape, + const double arg_lon_velocity, const double arg_lat_velocity, + const geometry_msgs::msg::Point arg_collision_point) : TargetObstacleInterface(arg_uuid, arg_stamp, arg_pose, arg_lon_velocity, arg_lat_velocity), + shape(arg_shape), collision_point(arg_collision_point) { } + Shape shape; geometry_msgs::msg::Point collision_point; }; @@ -130,19 +137,21 @@ struct SlowDownObstacle : public TargetObstacleInterface { SlowDownObstacle( const std::string & arg_uuid, const rclcpp::Time & arg_stamp, - const geometry_msgs::msg::Pose & arg_pose, const double arg_lon_velocity, - const double arg_lat_velocity, const double arg_precise_lat_dist, + const ObjectClassification & object_classification, const geometry_msgs::msg::Pose & arg_pose, + const double arg_lon_velocity, const double arg_lat_velocity, const double arg_precise_lat_dist, const geometry_msgs::msg::Point & arg_front_collision_point, const geometry_msgs::msg::Point & arg_back_collision_point) : TargetObstacleInterface(arg_uuid, arg_stamp, arg_pose, arg_lon_velocity, arg_lat_velocity), precise_lat_dist(arg_precise_lat_dist), front_collision_point(arg_front_collision_point), - back_collision_point(arg_back_collision_point) + back_collision_point(arg_back_collision_point), + classification(object_classification) { } double precise_lat_dist; // for efficient calculation geometry_msgs::msg::Point front_collision_point; geometry_msgs::msg::Point back_collision_point; + ObjectClassification classification; }; struct LongitudinalInfo 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 260a4c6400588..9b2de4c8f2de0 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp @@ -20,6 +20,7 @@ #include "obstacle_cruise_planner/pid_based_planner/pid_based_planner.hpp" #include "obstacle_cruise_planner/type_alias.hpp" #include "signal_processing/lowpass_filter_1d.hpp" +#include "tier4_autoware_utils/ros/logger_level_configure.hpp" #include "tier4_autoware_utils/system/stop_watch.hpp" #include @@ -48,6 +49,10 @@ class ObstacleCruisePlannerNode : public rclcpp::Node void onSmoothedTrajectory(const Trajectory::ConstSharedPtr msg); // main functions + std::vector createOneStepPolygons( + const std::vector & traj_points, + const vehicle_info_util::VehicleInfo & vehicle_info, + const geometry_msgs::msg::Pose & current_ego_pose, const double lat_margin = 0.0) const; std::vector convertToObstacles(const std::vector & traj_points) const; std::tuple, std::vector, std::vector> determineEgoBehaviorAgainstObstacles( @@ -100,6 +105,10 @@ class ObstacleCruisePlannerNode : public rclcpp::Node bool enable_debug_info_; bool enable_calculation_time_info_; double min_behavior_stop_margin_; + bool enable_approaching_on_curve_; + double additional_safe_distance_margin_on_curve_; + double min_safe_distance_margin_on_curve_; + bool suppress_sudden_obstacle_stop_; std::vector stop_obstacle_types_; std::vector inside_cruise_obstacle_types_; @@ -189,6 +198,9 @@ class ObstacleCruisePlannerNode : public rclcpp::Node double lat_hysteresis_margin_for_slow_down; int successive_num_to_entry_slow_down_condition; int successive_num_to_exit_slow_down_condition; + // consideration for the current ego pose + bool enable_to_consider_current_pose{false}; + double time_to_convergence{1.5}; }; BehaviorDeterminationParam behavior_determination_param_; @@ -248,6 +260,8 @@ class ObstacleCruisePlannerNode : public rclcpp::Node // previous closest obstacle std::shared_ptr prev_closest_stop_obstacle_ptr_{nullptr}; + + std::unique_ptr logger_configure_; }; } // namespace motion_planning diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/optimization_based_planner/optimization_based_planner.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/optimization_based_planner/optimization_based_planner.hpp index 55a97ccc2785d..44626206ec038 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/optimization_based_planner/optimization_based_planner.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/optimization_based_planner/optimization_based_planner.hpp @@ -19,7 +19,6 @@ #include "obstacle_cruise_planner/optimization_based_planner/velocity_optimizer.hpp" #include "obstacle_cruise_planner/planner_interface.hpp" #include "obstacle_cruise_planner/type_alias.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" #include "vehicle_info_util/vehicle_info_util.hpp" #include diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp index 9be6ec10e952e..d8c31fb35df9b 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp @@ -15,17 +15,19 @@ #ifndef OBSTACLE_CRUISE_PLANNER__PLANNER_INTERFACE_HPP_ #define OBSTACLE_CRUISE_PLANNER__PLANNER_INTERFACE_HPP_ -#include "motion_utils/motion_utils.hpp" +#include "motion_utils/trajectory/trajectory.hpp" #include "obstacle_cruise_planner/common_structs.hpp" #include "obstacle_cruise_planner/stop_planning_debug_info.hpp" #include "obstacle_cruise_planner/type_alias.hpp" #include "obstacle_cruise_planner/utils.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" +#include "tier4_autoware_utils/ros/update_param.hpp" +#include "tier4_autoware_utils/system/stop_watch.hpp" #include #include #include #include +#include #include #include @@ -53,11 +55,17 @@ class PlannerInterface void setParam( const bool enable_debug_info, const bool enable_calculation_time_info, - const double min_behavior_stop_margin) + const double min_behavior_stop_margin, const double enable_approaching_on_curve, + const double additional_safe_distance_margin_on_curve, + const double min_safe_distance_margin_on_curve, const bool suppress_sudden_obstacle_stop) { enable_debug_info_ = enable_debug_info; enable_calculation_time_info_ = enable_calculation_time_info; min_behavior_stop_margin_ = min_behavior_stop_margin; + enable_approaching_on_curve_ = enable_approaching_on_curve; + additional_safe_distance_margin_on_curve_ = additional_safe_distance_margin_on_curve; + min_safe_distance_margin_on_curve_ = min_safe_distance_margin_on_curve; + suppress_sudden_obstacle_stop_ = suppress_sudden_obstacle_stop; } std::vector generateStopTrajectory( @@ -101,6 +109,10 @@ class PlannerInterface bool enable_calculation_time_info_{false}; LongitudinalInfo longitudinal_info_; double min_behavior_stop_margin_; + bool enable_approaching_on_curve_; + double additional_safe_distance_margin_on_curve_; + double min_safe_distance_margin_on_curve_; + bool suppress_sudden_obstacle_stop_; // stop watch tier4_autoware_utils::StopWatch< @@ -192,6 +204,8 @@ class PlannerInterface std::optional start_point{std::nullopt}; std::optional end_point{std::nullopt}; }; + double calculateMarginFromObstacleOnCurve( + const PlannerData & planner_data, const StopObstacle & stop_obstacle) const; double calculateSlowDownVelocity( const SlowDownObstacle & obstacle, const std::optional & prev_output) const; std::optional> calculateDistanceToSlowDownWithConstraints( @@ -208,12 +222,42 @@ class PlannerInterface struct SlowDownParam { + std::vector obstacle_labels{"default"}; + std::unordered_map types_map; + struct ObstacleSpecificParams + { + double max_lat_margin; + double min_lat_margin; + double max_ego_velocity; + double min_ego_velocity; + }; explicit SlowDownParam(rclcpp::Node & node) { - max_lat_margin = node.declare_parameter("slow_down.max_lat_margin"); - min_lat_margin = node.declare_parameter("slow_down.min_lat_margin"); - max_ego_velocity = node.declare_parameter("slow_down.max_ego_velocity"); - min_ego_velocity = node.declare_parameter("slow_down.min_ego_velocity"); + types_map = {{ObjectClassification::UNKNOWN, "unknown"}, + {ObjectClassification::CAR, "car"}, + {ObjectClassification::TRUCK, "truck"}, + {ObjectClassification::BUS, "bus"}, + {ObjectClassification::TRAILER, "trailer"}, + {ObjectClassification::MOTORCYCLE, "motorcycle"}, + {ObjectClassification::BICYCLE, "bicycle"}, + {ObjectClassification::PEDESTRIAN, "pedestrian"}}; + obstacle_labels = + node.declare_parameter>("slow_down.labels", obstacle_labels); + // obstacle label dependant parameters + for (const auto & label : obstacle_labels) { + ObstacleSpecificParams params; + params.max_lat_margin = + node.declare_parameter("slow_down." + label + ".max_lat_margin"); + params.min_lat_margin = + node.declare_parameter("slow_down." + label + ".min_lat_margin"); + params.max_ego_velocity = + node.declare_parameter("slow_down." + label + ".max_ego_velocity"); + params.min_ego_velocity = + node.declare_parameter("slow_down." + label + ".min_ego_velocity"); + obstacle_to_param_struct_map.emplace(std::make_pair(label, params)); + } + + // common parameters time_margin_on_target_velocity = node.declare_parameter("slow_down.time_margin_on_target_velocity"); lpf_gain_slow_down_vel = node.declare_parameter("slow_down.lpf_gain_slow_down_vel"); @@ -222,16 +266,35 @@ class PlannerInterface node.declare_parameter("slow_down.lpf_gain_dist_to_slow_down"); } + ObstacleSpecificParams getObstacleParamByLabel(const ObjectClassification & label_id) const + { + const std::string label = types_map.at(label_id.label); + if (obstacle_to_param_struct_map.count(label) > 0) { + return obstacle_to_param_struct_map.at(label); + } + return obstacle_to_param_struct_map.at("default"); + } + void onParam(const std::vector & parameters) { - tier4_autoware_utils::updateParam( - parameters, "slow_down.max_lat_margin", max_lat_margin); - tier4_autoware_utils::updateParam( - parameters, "slow_down.min_lat_margin", min_lat_margin); - tier4_autoware_utils::updateParam( - parameters, "slow_down.max_ego_velocity", max_ego_velocity); - tier4_autoware_utils::updateParam( - parameters, "slow_down.min_ego_velocity", min_ego_velocity); + // obstacle type dependant parameters + for (const auto & label : obstacle_labels) { + auto & param_by_obstacle_label = obstacle_to_param_struct_map[label]; + tier4_autoware_utils::updateParam( + parameters, "slow_down." + label + ".max_lat_margin", + param_by_obstacle_label.max_lat_margin); + tier4_autoware_utils::updateParam( + parameters, "slow_down." + label + ".min_lat_margin", + param_by_obstacle_label.min_lat_margin); + tier4_autoware_utils::updateParam( + parameters, "slow_down." + label + ".max_ego_velocity", + param_by_obstacle_label.max_ego_velocity); + tier4_autoware_utils::updateParam( + parameters, "slow_down." + label + ".min_ego_velocity", + param_by_obstacle_label.min_ego_velocity); + } + + // common parameters tier4_autoware_utils::updateParam( parameters, "slow_down.time_margin_on_target_velocity", time_margin_on_target_velocity); tier4_autoware_utils::updateParam( @@ -242,10 +305,8 @@ class PlannerInterface parameters, "slow_down.lpf_gain_dist_to_slow_down", lpf_gain_dist_to_slow_down); } - double max_lat_margin; - double min_lat_margin; - double max_ego_velocity; - double min_ego_velocity; + std::unordered_map obstacle_to_param_struct_map; + double time_margin_on_target_velocity; double lpf_gain_slow_down_vel; double lpf_gain_lat_dist; 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 fdbf5b413d978..c2b14cc2e875f 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 @@ -17,7 +17,7 @@ #include "obstacle_cruise_planner/common_structs.hpp" #include "obstacle_cruise_planner/type_alias.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" +#include "tier4_autoware_utils/geometry/boost_geometry.hpp" #include "vehicle_info_util/vehicle_info_util.hpp" #include @@ -33,6 +33,11 @@ namespace bg = boost::geometry; using tier4_autoware_utils::Point2d; using tier4_autoware_utils::Polygon2d; +Polygon2d createOneStepPolygon( + const std::vector & last_poses, + const std::vector & current_poses, + const vehicle_info_util::VehicleInfo & vehicle_info, const double lat_margin); + std::optional getCollisionPoint( const std::vector & traj_points, const std::vector & traj_polygons, const Obstacle & obstacle, const bool is_driving_forward); @@ -45,9 +50,6 @@ std::vector getCollisionPoints( const double max_lat_dist = std::numeric_limits::max(), const double max_prediction_time_for_collision_check = std::numeric_limits::max()); -std::vector createOneStepPolygons( - const std::vector & traj_points, - const vehicle_info_util::VehicleInfo & vehicle_info, const double lat_margin = 0.0); } // namespace polygon_utils #endif // OBSTACLE_CRUISE_PLANNER__POLYGON_UTILS_HPP_ diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/utils.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/utils.hpp index ed6d0f52691e3..c635b400ed104 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/utils.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/utils.hpp @@ -16,9 +16,8 @@ #define OBSTACLE_CRUISE_PLANNER__UTILS_HPP_ #include "common_structs.hpp" -#include "motion_utils/motion_utils.hpp" #include "obstacle_cruise_planner/type_alias.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" #include diff --git a/planning/obstacle_cruise_planner/package.xml b/planning/obstacle_cruise_planner/package.xml index 6cfbf9225dd7e..f482be8ebafe7 100644 --- a/planning/obstacle_cruise_planner/package.xml +++ b/planning/obstacle_cruise_planner/package.xml @@ -6,6 +6,8 @@ Takayuki Murooka Yutaka Shimizu + Kosuke Takeuchi + Satoshi Ota Apache License 2.0 diff --git a/planning/obstacle_cruise_planner/src/node.cpp b/planning/obstacle_cruise_planner/src/node.cpp index 70c0c687b2514..c9fe9f2210423 100644 --- a/planning/obstacle_cruise_planner/src/node.cpp +++ b/planning/obstacle_cruise_planner/src/node.cpp @@ -20,6 +20,7 @@ #include "obstacle_cruise_planner/polygon_utils.hpp" #include "obstacle_cruise_planner/utils.hpp" #include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" +#include "tier4_autoware_utils/ros/marker_helper.hpp" #include "tier4_autoware_utils/ros/update_param.hpp" #include @@ -99,14 +100,15 @@ std::pair projectObstacleVelocityToTrajectory( const std::vector & traj_points, const Obstacle & obstacle) { const size_t object_idx = motion_utils::findNearestIndex(traj_points, obstacle.pose.position); - - const double object_vel_norm = std::hypot(obstacle.twist.linear.x, obstacle.twist.linear.y); - const double object_vel_yaw = std::atan2(obstacle.twist.linear.y, obstacle.twist.linear.x); const double traj_yaw = tf2::getYaw(traj_points.at(object_idx).pose.orientation); - return std::make_pair( - object_vel_norm * std::cos(object_vel_yaw - traj_yaw), - object_vel_norm * std::sin(object_vel_yaw - traj_yaw)); + const double obstacle_yaw = tf2::getYaw(obstacle.pose.orientation); + + const Eigen::Rotation2Dd R_ego_to_obstacle(obstacle_yaw - traj_yaw); + const Eigen::Vector2d obstacle_velocity(obstacle.twist.linear.x, obstacle.twist.linear.y); + const Eigen::Vector2d projected_velocity = R_ego_to_obstacle * obstacle_velocity; + + return std::make_pair(projected_velocity[0], projected_velocity[1]); } double calcObstacleMaxLength(const Shape & shape) @@ -130,11 +132,11 @@ double calcObstacleMaxLength(const Shape & shape) } TrajectoryPoint getExtendTrajectoryPoint( - const double extend_distance, const TrajectoryPoint & goal_point) + const double extend_distance, const TrajectoryPoint & goal_point, const bool is_driving_forward) { TrajectoryPoint extend_trajectory_point; - extend_trajectory_point.pose = - tier4_autoware_utils::calcOffsetPose(goal_point.pose, extend_distance, 0.0, 0.0); + extend_trajectory_point.pose = tier4_autoware_utils::calcOffsetPose( + goal_point.pose, extend_distance * (is_driving_forward ? 1.0 : -1.0), 0.0, 0.0); 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; @@ -146,6 +148,8 @@ std::vector extendTrajectoryPoints( const double step_length) { auto output_points = input_points; + const auto is_driving_forward_opt = motion_utils::isDrivingForwardWithTwist(input_points); + const bool is_driving_forward = is_driving_forward_opt ? *is_driving_forward_opt : true; if (extend_distance < std::numeric_limits::epsilon()) { return output_points; @@ -155,11 +159,13 @@ std::vector extendTrajectoryPoints( double extend_sum = 0.0; while (extend_sum <= (extend_distance - step_length)) { - const auto extend_trajectory_point = getExtendTrajectoryPoint(extend_sum, goal_point); + const auto extend_trajectory_point = + getExtendTrajectoryPoint(extend_sum, goal_point, is_driving_forward); output_points.push_back(extend_trajectory_point); extend_sum += step_length; } - const auto extend_trajectory_point = getExtendTrajectoryPoint(extend_distance, goal_point); + const auto extend_trajectory_point = + getExtendTrajectoryPoint(extend_distance, goal_point, is_driving_forward); output_points.push_back(extend_trajectory_point); return output_points; @@ -269,6 +275,10 @@ ObstacleCruisePlannerNode::BehaviorDeterminationParam::BehaviorDeterminationPara "behavior_determination.slow_down.successive_num_to_entry_slow_down_condition"); successive_num_to_exit_slow_down_condition = node.declare_parameter( "behavior_determination.slow_down.successive_num_to_exit_slow_down_condition"); + enable_to_consider_current_pose = node.declare_parameter( + "behavior_determination.consider_current_pose.enable_to_consider_current_pose"); + time_to_convergence = node.declare_parameter( + "behavior_determination.consider_current_pose.time_to_convergence"); } void ObstacleCruisePlannerNode::BehaviorDeterminationParam::onParam( @@ -327,6 +337,12 @@ void ObstacleCruisePlannerNode::BehaviorDeterminationParam::onParam( tier4_autoware_utils::updateParam( parameters, "behavior_determination.slow_down.successive_num_to_exit_slow_down_condition", successive_num_to_exit_slow_down_condition); + tier4_autoware_utils::updateParam( + parameters, "behavior_determination.consider_current_pose.enable_to_consider_current_pose", + enable_to_consider_current_pose); + tier4_autoware_utils::updateParam( + parameters, "behavior_determination.consider_current_pose.time_to_convergence", + time_to_convergence); } ObstacleCruisePlannerNode::ObstacleCruisePlannerNode(const rclcpp::NodeOptions & node_options) @@ -358,7 +374,7 @@ ObstacleCruisePlannerNode::ObstacleCruisePlannerNode(const rclcpp::NodeOptions & "~/output/clear_velocity_limit", rclcpp::QoS{1}.transient_local()); // debug publisher - debug_calculation_time_pub_ = create_publisher("~/debug/calculation_time", 1); + debug_calculation_time_pub_ = create_publisher("~/debug/processing_time_ms", 1); debug_cruise_wall_marker_pub_ = create_publisher("~/debug/cruise/virtual_wall", 1); debug_stop_wall_marker_pub_ = create_publisher("~/virtual_wall", 1); debug_slow_down_wall_marker_pub_ = @@ -397,8 +413,18 @@ ObstacleCruisePlannerNode::ObstacleCruisePlannerNode(const rclcpp::NodeOptions & } min_behavior_stop_margin_ = declare_parameter("common.min_behavior_stop_margin"); + additional_safe_distance_margin_on_curve_ = + declare_parameter("common.stop_on_curve.additional_safe_distance_margin"); + enable_approaching_on_curve_ = + declare_parameter("common.stop_on_curve.enable_approaching"); + min_safe_distance_margin_on_curve_ = + declare_parameter("common.stop_on_curve.min_safe_distance_margin"); + suppress_sudden_obstacle_stop_ = + declare_parameter("common.suppress_sudden_obstacle_stop"); planner_ptr_->setParam( - enable_debug_info_, enable_calculation_time_info_, min_behavior_stop_margin_); + enable_debug_info_, enable_calculation_time_info_, min_behavior_stop_margin_, + enable_approaching_on_curve_, additional_safe_distance_margin_on_curve_, + min_safe_distance_margin_on_curve_, suppress_sudden_obstacle_stop_); } { // stop/cruise/slow down obstacle type @@ -413,6 +439,8 @@ ObstacleCruisePlannerNode::ObstacleCruisePlannerNode(const rclcpp::NodeOptions & // set parameter callback set_param_res_ = this->add_on_set_parameters_callback( std::bind(&ObstacleCruisePlannerNode::onParam, this, std::placeholders::_1)); + + logger_configure_ = std::make_unique(this); } ObstacleCruisePlannerNode::PlanningAlgorithm ObstacleCruisePlannerNode::getPlanningAlgorithmType( @@ -435,8 +463,20 @@ rcl_interfaces::msg::SetParametersResult ObstacleCruisePlannerNode::onParam( parameters, "common.enable_debug_info", enable_debug_info_); tier4_autoware_utils::updateParam( parameters, "common.enable_calculation_time_info", enable_calculation_time_info_); + + tier4_autoware_utils::updateParam( + parameters, "common.stop_on_curve.enable_approaching", enable_approaching_on_curve_); + tier4_autoware_utils::updateParam( + parameters, "common.stop_on_curve.additional_safe_distance_margin", + additional_safe_distance_margin_on_curve_); + tier4_autoware_utils::updateParam( + parameters, "common.stop_on_curve.min_safe_distance_margin", + min_safe_distance_margin_on_curve_); + planner_ptr_->setParam( - enable_debug_info_, enable_calculation_time_info_, min_behavior_stop_margin_); + enable_debug_info_, enable_calculation_time_info_, min_behavior_stop_margin_, + enable_approaching_on_curve_, additional_safe_distance_margin_on_curve_, + min_safe_distance_margin_on_curve_, suppress_sudden_obstacle_stop_); tier4_autoware_utils::updateParam( parameters, "common.enable_slow_down_planning", enable_slow_down_planning_); @@ -507,6 +547,57 @@ void ObstacleCruisePlannerNode::onTrajectory(const Trajectory::ConstSharedPtr ms get_logger(), enable_calculation_time_info_, "%s := %f [ms]", __func__, calculation_time); } +std::vector ObstacleCruisePlannerNode::createOneStepPolygons( + const std::vector & traj_points, + const vehicle_info_util::VehicleInfo & vehicle_info, + const geometry_msgs::msg::Pose & current_ego_pose, const double lat_margin) const +{ + const auto & p = behavior_determination_param_; + const bool is_enable_current_pose_consideration = p.enable_to_consider_current_pose; + const double step_length = p.decimate_trajectory_step_length; + const double time_to_convergence = p.time_to_convergence; + + std::vector polygons; + const double current_ego_lat_error = + motion_utils::calcLateralOffset(traj_points, current_ego_pose.position); + const double current_ego_yaw_error = + motion_utils::calcYawDeviation(traj_points, current_ego_pose); + double time_elapsed = 0.0; + + std::vector last_poses = {traj_points.at(0).pose}; + if (is_enable_current_pose_consideration) { + last_poses.push_back(current_ego_pose); + } + + for (size_t i = 0; i < traj_points.size(); ++i) { + std::vector current_poses = {traj_points.at(i).pose}; + + // estimate the future ego pose with assuming that the pose error against the reference path + // will decrease to zero by the time_to_convergence + if (is_enable_current_pose_consideration && time_elapsed < time_to_convergence) { + const double rem_ratio = (time_to_convergence - time_elapsed) / time_to_convergence; + geometry_msgs::msg::Pose indexed_pose_err; + indexed_pose_err.set__orientation( + tier4_autoware_utils::createQuaternionFromYaw(current_ego_yaw_error * rem_ratio)); + indexed_pose_err.set__position( + tier4_autoware_utils::createPoint(0.0, current_ego_lat_error * rem_ratio, 0.0)); + + current_poses.push_back( + tier4_autoware_utils::transformPose(indexed_pose_err, traj_points.at(i).pose)); + + if (traj_points.at(i).longitudinal_velocity_mps != 0.0) { + time_elapsed += step_length / traj_points.at(i).longitudinal_velocity_mps; + } else { + time_elapsed = std::numeric_limits::max(); + } + } + polygons.push_back( + polygon_utils::createOneStepPolygon(last_poses, current_poses, vehicle_info, lat_margin)); + last_poses = current_poses; + } + return polygons; +} + std::vector ObstacleCruisePlannerNode::convertToObstacles( const std::vector & traj_points) const { @@ -626,7 +717,7 @@ ObstacleCruisePlannerNode::determineEgoBehaviorAgainstObstacles( // calculated decimated trajectory points and trajectory polygon const auto decimated_traj_points = decimateTrajectoryPoints(traj_points); const auto decimated_traj_polys = - polygon_utils::createOneStepPolygons(decimated_traj_points, vehicle_info_); + createOneStepPolygons(decimated_traj_points, vehicle_info_, ego_odom_ptr_->pose.pose); debug_data_ptr_->detection_polygons = decimated_traj_polys; // determine ego's behavior from stop, cruise and slow down @@ -907,7 +998,7 @@ std::optional ObstacleCruisePlannerNode::createStopObstacle( } const auto [tangent_vel, normal_vel] = projectObstacleVelocityToTrajectory(traj_points, obstacle); - return StopObstacle{obstacle.uuid, obstacle.stamp, obstacle.pose, + return StopObstacle{obstacle.uuid, obstacle.stamp, obstacle.pose, obstacle.shape, tangent_vel, normal_vel, *collision_point}; } @@ -958,8 +1049,8 @@ ObstacleCruisePlannerNode::createCollisionPointForStopObstacle( } // calculate collision points with trajectory with lateral stop margin - const auto traj_polys_with_lat_margin = - polygon_utils::createOneStepPolygons(traj_points, vehicle_info_, p.max_lat_margin_for_stop); + const auto traj_polys_with_lat_margin = createOneStepPolygons( + traj_points, vehicle_info_, ego_odom_ptr_->pose.pose, p.max_lat_margin_for_stop); const auto collision_point = polygon_utils::getCollisionPoint( traj_points, traj_polys_with_lat_margin, obstacle, is_driving_forward_); return collision_point; @@ -1020,8 +1111,8 @@ std::optional ObstacleCruisePlannerNode::createSlowDownObstacl // calculate collision points with trajectory with lateral stop margin // NOTE: For additional margin, hysteresis is not divided by two. - const auto traj_polys_with_lat_margin = polygon_utils::createOneStepPolygons( - traj_points, vehicle_info_, + const auto traj_polys_with_lat_margin = createOneStepPolygons( + traj_points, vehicle_info_, ego_odom_ptr_->pose.pose, p.max_lat_margin_for_slow_down + p.lat_hysteresis_margin_for_slow_down); std::vector front_collision_polygons; @@ -1084,9 +1175,9 @@ std::optional ObstacleCruisePlannerNode::createSlowDownObstacl } const auto [tangent_vel, normal_vel] = projectObstacleVelocityToTrajectory(traj_points, obstacle); - return SlowDownObstacle{obstacle.uuid, obstacle.stamp, obstacle.pose, - tangent_vel, normal_vel, precise_lat_dist, - front_collision_point, back_collision_point}; + return SlowDownObstacle{obstacle.uuid, obstacle.stamp, obstacle.classification, + obstacle.pose, tangent_vel, normal_vel, + precise_lat_dist, front_collision_point, back_collision_point}; } void ObstacleCruisePlannerNode::checkConsistency( diff --git a/planning/obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp b/planning/obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp index f420993c1adac..08f893a0a9a50 100644 --- a/planning/obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp +++ b/planning/obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp @@ -17,11 +17,13 @@ #include "interpolation/linear_interpolation.hpp" #include "interpolation/spline_interpolation.hpp" #include "interpolation/zero_order_hold.hpp" +#include "motion_utils/marker/marker_helper.hpp" #include "motion_utils/resample/resample.hpp" #include "motion_utils/trajectory/interpolation.hpp" #include "motion_utils/trajectory/trajectory.hpp" #include "obstacle_cruise_planner/utils.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" +#include "tier4_autoware_utils/ros/marker_helper.hpp" #ifdef ROS_DISTRO_GALACTIC #include diff --git a/planning/obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp b/planning/obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp index ffd2f97b1eb89..005a85d9b872d 100644 --- a/planning/obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp +++ b/planning/obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp @@ -15,8 +15,11 @@ #include "obstacle_cruise_planner/pid_based_planner/pid_based_planner.hpp" #include "interpolation/spline_interpolation.hpp" +#include "motion_utils/marker/marker_helper.hpp" #include "obstacle_cruise_planner/utils.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" +#include "tier4_autoware_utils/ros/marker_helper.hpp" +#include "tier4_autoware_utils/ros/update_param.hpp" #include "tier4_planning_msgs/msg/velocity_limit.hpp" diff --git a/planning/obstacle_cruise_planner/src/planner_interface.cpp b/planning/obstacle_cruise_planner/src/planner_interface.cpp index 4cf6bf9e23806..76469364cfb39 100644 --- a/planning/obstacle_cruise_planner/src/planner_interface.cpp +++ b/planning/obstacle_cruise_planner/src/planner_interface.cpp @@ -15,8 +15,15 @@ #include "obstacle_cruise_planner/planner_interface.hpp" #include "motion_utils/distance/distance.hpp" +#include "motion_utils/marker/marker_helper.hpp" +#include "motion_utils/resample/resample.hpp" +#include "motion_utils/trajectory/tmp_conversion.hpp" +#include "motion_utils/trajectory/trajectory.hpp" #include "signal_processing/lowpass_filter_1d.hpp" +#include "tier4_autoware_utils/ros/marker_helper.hpp" +#include +#include namespace { StopSpeedExceeded createStopSpeedExceededMsg( @@ -204,6 +211,19 @@ double calcDecelerationVelocityFromDistanceToTarget( } return current_velocity; } + +std::vector resampleTrajectoryPoints( + const std::vector & traj_points, const double interval) +{ + const auto traj = motion_utils::convertToTrajectory(traj_points); + const auto resampled_traj = motion_utils::resampleTrajectory(traj, interval); + return motion_utils::convertToTrajectoryPointArray(resampled_traj); +} + +tier4_autoware_utils::Point2d convertPoint(const geometry_msgs::msg::Point & p) +{ + return tier4_autoware_utils::Point2d{p.x, p.y}; +} } // namespace std::vector PlannerInterface::generateStopTrajectory( @@ -259,16 +279,19 @@ std::vector PlannerInterface::generateStopTrajectory( planner_data.traj_points, planner_data.ego_pose.position, ego_segment_idx, 0); const double dist_to_ego = -negative_dist_to_ego; + const double margin_from_obstacle = + calculateMarginFromObstacleOnCurve(planner_data, *closest_stop_obstacle); + // If behavior stop point is ahead of the closest_obstacle_stop point within a certain margin // we set closest_obstacle_stop_distance to closest_behavior_stop_distance - const double margin_from_obstacle = [&]() { + const double margin_from_obstacle_considering_behavior_module = [&]() { const size_t nearest_segment_idx = findEgoSegmentIndex(planner_data.traj_points, planner_data.ego_pose); const auto closest_behavior_stop_idx = motion_utils::searchZeroVelocityIndex(planner_data.traj_points, nearest_segment_idx + 1); if (!closest_behavior_stop_idx) { - return longitudinal_info_.safe_distance_margin; + return margin_from_obstacle; } const double closest_behavior_stop_dist_from_ego = motion_utils::calcSignedArcLength( @@ -282,44 +305,49 @@ std::vector PlannerInterface::generateStopTrajectory( abs_ego_offset; const double stop_dist_diff = closest_behavior_stop_dist_from_ego - closest_obstacle_stop_dist_from_ego; - if (stop_dist_diff < longitudinal_info_.safe_distance_margin) { + if (stop_dist_diff < margin_from_obstacle) { // Use terminal margin (terminal_safe_distance_margin) for obstacle stop return longitudinal_info_.terminal_safe_distance_margin; } } else { - const double closest_obstacle_stop_dist_from_ego = closest_obstacle_dist - dist_to_ego - - longitudinal_info_.safe_distance_margin - - abs_ego_offset; + const double closest_obstacle_stop_dist_from_ego = + closest_obstacle_dist - dist_to_ego - margin_from_obstacle - abs_ego_offset; const double stop_dist_diff = closest_behavior_stop_dist_from_ego - closest_obstacle_stop_dist_from_ego; - if (0.0 < stop_dist_diff && stop_dist_diff < longitudinal_info_.safe_distance_margin) { + if (0.0 < stop_dist_diff && stop_dist_diff < margin_from_obstacle) { // Use shorter margin (min_behavior_stop_margin) for obstacle stop return min_behavior_stop_margin_; } } - return longitudinal_info_.safe_distance_margin; + return margin_from_obstacle; }(); - // Calculate feasible stop margin (Check the feasibility) - const double feasible_stop_dist = calcMinimumDistanceToStop( - planner_data.ego_vel, longitudinal_info_.limit_max_accel, - longitudinal_info_.limit_min_accel) + - dist_to_ego; - const double closest_obstacle_stop_dist = - closest_obstacle_dist - margin_from_obstacle - abs_ego_offset; - - bool will_collide_with_obstacle = false; - double feasible_margin_from_obstacle = margin_from_obstacle; - if (closest_obstacle_stop_dist < feasible_stop_dist) { - feasible_margin_from_obstacle = - margin_from_obstacle - (feasible_stop_dist - closest_obstacle_stop_dist); - will_collide_with_obstacle = true; - } + const auto [stop_margin_from_obstacle, will_collide_with_obstacle] = [&]() { + // Check feasibility to stop + if (suppress_sudden_obstacle_stop_) { + const double closest_obstacle_stop_dist = + closest_obstacle_dist - margin_from_obstacle_considering_behavior_module - abs_ego_offset; + + // Calculate feasible stop margin (Check the feasibility) + const double feasible_stop_dist = calcMinimumDistanceToStop( + planner_data.ego_vel, longitudinal_info_.limit_max_accel, + longitudinal_info_.limit_min_accel) + + dist_to_ego; + + if (closest_obstacle_stop_dist < feasible_stop_dist) { + const auto feasible_margin_from_obstacle = + margin_from_obstacle_considering_behavior_module - + (feasible_stop_dist - closest_obstacle_stop_dist); + return std::make_pair(feasible_margin_from_obstacle, true); + } + } + return std::make_pair(margin_from_obstacle_considering_behavior_module, false); + }(); // Generate Output Trajectory const double zero_vel_dist = [&]() { const double current_zero_vel_dist = - std::max(0.0, closest_obstacle_dist - abs_ego_offset - feasible_margin_from_obstacle); + std::max(0.0, closest_obstacle_dist - abs_ego_offset - stop_margin_from_obstacle); // Hold previous stop distance if necessary if ( @@ -352,7 +380,7 @@ std::vector PlannerInterface::generateStopTrajectory( // virtual wall marker for stop obstacle const auto markers = motion_utils::createStopVirtualWallMarker( output_traj_points.at(*zero_vel_idx).pose, "obstacle stop", planner_data.current_time, 0, - abs_ego_offset); + abs_ego_offset, "", planner_data.is_driving_forward); tier4_autoware_utils::appendMarkerArray(markers, &debug_data_ptr_->stop_wall_marker); debug_data_ptr_->obstacles_to_stop.push_back(*closest_stop_obstacle); @@ -378,7 +406,7 @@ std::vector PlannerInterface::generateStopTrajectory( StopPlanningDebugInfo::TYPE::STOP_CURRENT_OBSTACLE_VELOCITY, closest_stop_obstacle->velocity); stop_planning_debug_info_.set( - StopPlanningDebugInfo::TYPE::STOP_TARGET_OBSTACLE_DISTANCE, feasible_margin_from_obstacle); + StopPlanningDebugInfo::TYPE::STOP_TARGET_OBSTACLE_DISTANCE, stop_margin_from_obstacle); stop_planning_debug_info_.set(StopPlanningDebugInfo::TYPE::STOP_TARGET_VELOCITY, 0.0); stop_planning_debug_info_.set(StopPlanningDebugInfo::TYPE::STOP_TARGET_ACCELERATION, 0.0); @@ -390,6 +418,91 @@ std::vector PlannerInterface::generateStopTrajectory( return output_traj_points; } +double PlannerInterface::calculateMarginFromObstacleOnCurve( + const PlannerData & planner_data, const StopObstacle & stop_obstacle) const +{ + if (!enable_approaching_on_curve_) { + return longitudinal_info_.safe_distance_margin; + } + + const double abs_ego_offset = planner_data.is_driving_forward + ? std::abs(vehicle_info_.max_longitudinal_offset_m) + : std::abs(vehicle_info_.min_longitudinal_offset_m); + + // calculate short trajectory points towards obstacle + const size_t obj_segment_idx = + motion_utils::findNearestSegmentIndex(planner_data.traj_points, stop_obstacle.collision_point); + std::vector short_traj_points{planner_data.traj_points.at(obj_segment_idx + 1)}; + double sum_short_traj_length{0.0}; + for (int i = obj_segment_idx; 0 <= i; --i) { + short_traj_points.push_back(planner_data.traj_points.at(i)); + + if ( + 1 < short_traj_points.size() && + longitudinal_info_.safe_distance_margin + abs_ego_offset < sum_short_traj_length) { + break; + } + sum_short_traj_length += tier4_autoware_utils::calcDistance2d( + planner_data.traj_points.at(i), planner_data.traj_points.at(i + 1)); + } + std::reverse(short_traj_points.begin(), short_traj_points.end()); + if (short_traj_points.size() < 2) { + return longitudinal_info_.safe_distance_margin; + } + + // calculate collision index between straight line from ego pose and object + const auto calculate_distance_from_straight_ego_path = + [&](const auto & ego_pose, const auto & object_polygon) { + const auto forward_ego_pose = tier4_autoware_utils::calcOffsetPose( + ego_pose, longitudinal_info_.safe_distance_margin + 3.0, 0.0, 0.0); + const auto ego_straight_segment = tier4_autoware_utils::Segment2d{ + convertPoint(ego_pose.position), convertPoint(forward_ego_pose.position)}; + return boost::geometry::distance(ego_straight_segment, object_polygon); + }; + const auto resampled_short_traj_points = resampleTrajectoryPoints(short_traj_points, 0.5); + const auto object_polygon = + tier4_autoware_utils::toPolygon2d(stop_obstacle.pose, stop_obstacle.shape); + const auto collision_idx = [&]() -> std::optional { + for (size_t i = 0; i < resampled_short_traj_points.size(); ++i) { + const double dist_to_obj = calculate_distance_from_straight_ego_path( + resampled_short_traj_points.at(i).pose, object_polygon); + if (dist_to_obj < vehicle_info_.vehicle_width_m / 2.0) { + return i; + } + } + return std::nullopt; + }(); + if (!collision_idx) { + return min_safe_distance_margin_on_curve_; + } + if (*collision_idx == 0) { + return longitudinal_info_.safe_distance_margin; + } + + // calculate margin from obstacle + const double partial_segment_length = [&]() { + const double collision_segment_length = tier4_autoware_utils::calcDistance2d( + resampled_short_traj_points.at(*collision_idx - 1), + resampled_short_traj_points.at(*collision_idx)); + const double prev_dist = calculate_distance_from_straight_ego_path( + resampled_short_traj_points.at(*collision_idx - 1).pose, object_polygon); + const double next_dist = calculate_distance_from_straight_ego_path( + resampled_short_traj_points.at(*collision_idx).pose, object_polygon); + return (next_dist - vehicle_info_.vehicle_width_m / 2.0) / (next_dist - prev_dist) * + collision_segment_length; + }(); + + const double short_margin_from_obstacle = + partial_segment_length + + motion_utils::calcSignedArcLength( + resampled_short_traj_points, *collision_idx, stop_obstacle.collision_point) - + abs_ego_offset + additional_safe_distance_margin_on_curve_; + + return std::min( + longitudinal_info_.safe_distance_margin, + std::max(min_safe_distance_margin_on_curve_, short_margin_from_obstacle)); +} + double PlannerInterface::calcDistanceToCollisionPoint( const PlannerData & planner_data, const geometry_msgs::msg::Point & collision_point) { @@ -510,7 +623,7 @@ std::vector PlannerInterface::generateSlowDownTrajectory( const auto markers = motion_utils::createSlowDownVirtualWallMarker( slow_down_traj_points.at(slow_down_wall_idx).pose, "obstacle slow down", - planner_data.current_time, i, abs_ego_offset); + planner_data.current_time, i, abs_ego_offset, "", planner_data.is_driving_forward); tier4_autoware_utils::appendMarkerArray(markers, &debug_data_ptr_->slow_down_wall_marker); } @@ -518,14 +631,14 @@ std::vector PlannerInterface::generateSlowDownTrajectory( if (slow_down_start_idx) { const auto markers = motion_utils::createSlowDownVirtualWallMarker( slow_down_traj_points.at(*slow_down_start_idx).pose, "obstacle slow down start", - planner_data.current_time, i * 2, abs_ego_offset); + planner_data.current_time, i * 2, abs_ego_offset, "", planner_data.is_driving_forward); tier4_autoware_utils::appendMarkerArray( markers, &debug_data_ptr_->slow_down_debug_wall_marker); } if (slow_down_end_idx) { const auto markers = motion_utils::createSlowDownVirtualWallMarker( slow_down_traj_points.at(*slow_down_end_idx).pose, "obstacle slow down end", - planner_data.current_time, i * 2 + 1, abs_ego_offset); + planner_data.current_time, i * 2 + 1, abs_ego_offset, "", planner_data.is_driving_forward); tier4_autoware_utils::appendMarkerArray( markers, &debug_data_ptr_->slow_down_debug_wall_marker); } @@ -552,7 +665,7 @@ std::vector PlannerInterface::generateSlowDownTrajectory( double PlannerInterface::calculateSlowDownVelocity( const SlowDownObstacle & obstacle, const std::optional & prev_output) const { - const auto & p = slow_down_param_; + const auto & p = slow_down_param_.getObstacleParamByLabel(obstacle.classification); const double stable_precise_lat_dist = [&]() { if (prev_output) { @@ -578,7 +691,6 @@ PlannerInterface::calculateDistanceToSlowDownWithConstraints( const SlowDownObstacle & obstacle, const std::optional & prev_output, const double dist_to_ego) const { - const auto & p = slow_down_param_; const double abs_ego_offset = planner_data.is_driving_forward ? std::abs(vehicle_info_.max_longitudinal_offset_m) : std::abs(vehicle_info_.min_longitudinal_offset_m); @@ -615,8 +727,8 @@ PlannerInterface::calculateDistanceToSlowDownWithConstraints( // calculate distance during deceleration, slow down preparation, and slow down const double min_slow_down_prepare_dist = 3.0; - const double slow_down_prepare_dist = - std::max(min_slow_down_prepare_dist, slow_down_vel * p.time_margin_on_target_velocity); + const double slow_down_prepare_dist = std::max( + min_slow_down_prepare_dist, slow_down_vel * slow_down_param_.time_margin_on_target_velocity); const double deceleration_dist = offset_dist_to_collision + dist_to_front_collision - abs_ego_offset - dist_to_ego - slow_down_prepare_dist; const double slow_down_dist = diff --git a/planning/obstacle_cruise_planner/src/polygon_utils.cpp b/planning/obstacle_cruise_planner/src/polygon_utils.cpp index 077b783f0e65e..e6b4ed89ca460 100644 --- a/planning/obstacle_cruise_planner/src/polygon_utils.cpp +++ b/planning/obstacle_cruise_planner/src/polygon_utils.cpp @@ -31,36 +31,6 @@ geometry_msgs::msg::Point calcOffsetPosition( return tier4_autoware_utils::calcOffsetPose(pose, offset_x, offset_y, 0.0).position; } -Polygon2d createOneStepPolygon( - const geometry_msgs::msg::Pose & base_step_pose, const geometry_msgs::msg::Pose & next_step_pose, - const vehicle_info_util::VehicleInfo & vehicle_info, const double lat_margin) -{ - Polygon2d polygon; - - const double base_to_front = vehicle_info.max_longitudinal_offset_m; - const double width = vehicle_info.vehicle_width_m / 2.0 + lat_margin; - const double base_to_rear = vehicle_info.rear_overhang_m; - - // base step - appendPointToPolygon(polygon, calcOffsetPosition(base_step_pose, base_to_front, width)); - appendPointToPolygon(polygon, calcOffsetPosition(base_step_pose, base_to_front, -width)); - appendPointToPolygon(polygon, calcOffsetPosition(base_step_pose, -base_to_rear, -width)); - appendPointToPolygon(polygon, calcOffsetPosition(base_step_pose, -base_to_rear, width)); - - // next step - appendPointToPolygon(polygon, calcOffsetPosition(next_step_pose, base_to_front, width)); - appendPointToPolygon(polygon, calcOffsetPosition(next_step_pose, base_to_front, -width)); - appendPointToPolygon(polygon, calcOffsetPosition(next_step_pose, -base_to_rear, -width)); - appendPointToPolygon(polygon, calcOffsetPosition(next_step_pose, -base_to_rear, width)); - - bg::correct(polygon); - - Polygon2d hull_polygon; - bg::convex_hull(polygon, hull_polygon); - - return hull_polygon; -} - PointWithStamp calcNearestCollisionPoint( const size_t first_within_idx, const std::vector & collision_points, const std::vector & decimated_traj_points, const bool is_driving_forward) @@ -132,6 +102,38 @@ std::optional>> getCollisionIndex( namespace polygon_utils { +Polygon2d createOneStepPolygon( + const std::vector & last_poses, + const std::vector & current_poses, + const vehicle_info_util::VehicleInfo & vehicle_info, const double lat_margin) +{ + Polygon2d polygon; + + const double base_to_front = vehicle_info.max_longitudinal_offset_m; + const double width = vehicle_info.vehicle_width_m / 2.0 + lat_margin; + const double base_to_rear = vehicle_info.rear_overhang_m; + + for (auto & pose : last_poses) { + appendPointToPolygon(polygon, calcOffsetPosition(pose, base_to_front, width)); + appendPointToPolygon(polygon, calcOffsetPosition(pose, base_to_front, -width)); + appendPointToPolygon(polygon, calcOffsetPosition(pose, -base_to_rear, -width)); + appendPointToPolygon(polygon, calcOffsetPosition(pose, -base_to_rear, width)); + } + for (auto & pose : current_poses) { + appendPointToPolygon(polygon, calcOffsetPosition(pose, base_to_front, width)); + appendPointToPolygon(polygon, calcOffsetPosition(pose, base_to_front, -width)); + appendPointToPolygon(polygon, calcOffsetPosition(pose, -base_to_rear, -width)); + appendPointToPolygon(polygon, calcOffsetPosition(pose, -base_to_rear, width)); + } + + bg::correct(polygon); + + Polygon2d hull_polygon; + bg::convex_hull(polygon, hull_polygon); + + return hull_polygon; +} + std::optional getCollisionPoint( const std::vector & traj_points, const std::vector & traj_polygons, const Obstacle & obstacle, const bool is_driving_forward) @@ -184,25 +186,4 @@ std::vector getCollisionPoints( return collision_points; } -std::vector createOneStepPolygons( - const std::vector & traj_points, - const vehicle_info_util::VehicleInfo & vehicle_info, const double lat_margin) -{ - std::vector polygons; - - for (size_t i = 0; i < traj_points.size(); ++i) { - const auto polygon = [&]() { - if (i == 0) { - return createOneStepPolygon( - traj_points.at(i).pose, traj_points.at(i).pose, vehicle_info, lat_margin); - } - return createOneStepPolygon( - traj_points.at(i - 1).pose, traj_points.at(i).pose, vehicle_info, lat_margin); - }(); - - polygons.push_back(polygon); - } - return polygons; -} - } // namespace polygon_utils diff --git a/planning/obstacle_cruise_planner/src/utils.cpp b/planning/obstacle_cruise_planner/src/utils.cpp index 091adfab586b4..42bac55e63dd8 100644 --- a/planning/obstacle_cruise_planner/src/utils.cpp +++ b/planning/obstacle_cruise_planner/src/utils.cpp @@ -15,6 +15,7 @@ #include "obstacle_cruise_planner/utils.hpp" #include "object_recognition_utils/predicted_path_utils.hpp" +#include "tier4_autoware_utils/ros/marker_helper.hpp" namespace obstacle_cruise_utils { diff --git a/planning/obstacle_stop_planner/include/obstacle_stop_planner/adaptive_cruise_control.hpp b/planning/obstacle_stop_planner/include/obstacle_stop_planner/adaptive_cruise_control.hpp index 40a3f36bff903..9d95c5a4796d4 100644 --- a/planning/obstacle_stop_planner/include/obstacle_stop_planner/adaptive_cruise_control.hpp +++ b/planning/obstacle_stop_planner/include/obstacle_stop_planner/adaptive_cruise_control.hpp @@ -16,7 +16,6 @@ #define OBSTACLE_STOP_PLANNER__ADAPTIVE_CRUISE_CONTROL_HPP_ #include -#include #include #include 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 c836b380e5cf2..368ad34b9c4e2 100644 --- a/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp +++ b/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp @@ -18,13 +18,14 @@ #include "obstacle_stop_planner/adaptive_cruise_control.hpp" #include "obstacle_stop_planner/debug_marker.hpp" #include "obstacle_stop_planner/planner_data.hpp" +#include "tier4_autoware_utils/ros/logger_level_configure.hpp" +#include "tier4_autoware_utils/system/stop_watch.hpp" #include #include #include #include -#include -#include +#include #include #include @@ -37,6 +38,7 @@ #include #include #include +#include #include #include #include @@ -78,9 +80,11 @@ using autoware_auto_planning_msgs::msg::Trajectory; using autoware_auto_planning_msgs::msg::TrajectoryPoint; using tier4_autoware_utils::Point2d; using tier4_autoware_utils::Polygon2d; +using tier4_autoware_utils::StopWatch; using tier4_debug_msgs::msg::BoolStamped; using tier4_debug_msgs::msg::Float32MultiArrayStamped; using tier4_debug_msgs::msg::Float32Stamped; +using tier4_debug_msgs::msg::Float64Stamped; using tier4_planning_msgs::msg::ExpandStopRange; using tier4_planning_msgs::msg::VelocityLimit; using tier4_planning_msgs::msg::VelocityLimitClearCommand; @@ -143,6 +147,8 @@ class ObstacleStopPlannerNode : public rclcpp::Node rclcpp::Publisher::SharedPtr pub_collision_pointcloud_debug_; + rclcpp::Publisher::SharedPtr pub_processing_time_ms_; + std::unique_ptr acc_controller_; std::shared_ptr debug_ptr_; boost::optional latest_slow_down_section_{boost::none}; @@ -166,6 +172,8 @@ class ObstacleStopPlannerNode : public rclcpp::Node StopParam stop_param_; SlowDownParam slow_down_param_; + StopWatch stop_watch_; + // 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_) @@ -275,6 +283,8 @@ class ObstacleStopPlannerNode : public rclcpp::Node return ret; } + + std::unique_ptr logger_configure_; }; } // namespace motion_planning 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 index 8023e86583885..7e7e659325abf 100644 --- a/planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_utils.hpp +++ b/planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_utils.hpp @@ -18,7 +18,7 @@ #include "obstacle_stop_planner/planner_data.hpp" -#include +#include #include #include @@ -46,7 +46,6 @@ using diagnostic_msgs::msg::KeyValue; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using geometry_msgs::msg::PoseArray; -using geometry_msgs::msg::TransformStamped; using std_msgs::msg::Header; using autoware_auto_planning_msgs::msg::TrajectoryPoint; diff --git a/planning/obstacle_stop_planner/src/adaptive_cruise_control.cpp b/planning/obstacle_stop_planner/src/adaptive_cruise_control.cpp index fb1e93185f738..c0f57489078d6 100644 --- a/planning/obstacle_stop_planner/src/adaptive_cruise_control.cpp +++ b/planning/obstacle_stop_planner/src/adaptive_cruise_control.cpp @@ -14,6 +14,8 @@ #include "obstacle_stop_planner/adaptive_cruise_control.hpp" +#include "motion_utils/trajectory/trajectory.hpp" + #include #include #include @@ -28,6 +30,8 @@ #include #endif +#include + #include #include #include @@ -369,9 +373,7 @@ void AdaptiveCruiseController::calcDistanceToNearestPointOnPath( /* get total distance to collision point */ double dist_to_point = 0; // get distance from self to next nearest point - dist_to_point += boost::geometry::distance( - convertPointRosToBoost(self_pose.position), - convertPointRosToBoost(trajectory.at(1).pose.position)); + dist_to_point += motion_utils::calcSignedArcLength(trajectory, self_pose.position, size_t(1)); // add distance from next self-nearest-point(=idx:0) to prev point of nearest_point_idx for (int i = 1; i < nearest_point_idx - 1; i++) { @@ -425,9 +427,12 @@ bool AdaptiveCruiseController::estimatePointVelocityFromObject( obj_vel_norm = std::hypot( obj.kinematics.initial_twist_with_covariance.twist.linear.x, obj.kinematics.initial_twist_with_covariance.twist.linear.y); - obj_vel_yaw = std::atan2( - obj.kinematics.initial_twist_with_covariance.twist.linear.y, - obj.kinematics.initial_twist_with_covariance.twist.linear.x); + + const double obj_yaw = + tf2::getYaw(obj.kinematics.initial_pose_with_covariance.pose.orientation); + obj_vel_yaw = obj_yaw + std::atan2( + obj.kinematics.initial_twist_with_covariance.twist.linear.y, + obj.kinematics.initial_twist_with_covariance.twist.linear.x); get_obj = true; break; } @@ -449,9 +454,13 @@ void AdaptiveCruiseController::calculateProjectedVelocityFromObject( double obj_vel_norm = std::hypot( object.kinematics.initial_twist_with_covariance.twist.linear.x, object.kinematics.initial_twist_with_covariance.twist.linear.y); - double obj_vel_yaw = std::atan2( - object.kinematics.initial_twist_with_covariance.twist.linear.y, - object.kinematics.initial_twist_with_covariance.twist.linear.x); + + const double obj_yaw = + tf2::getYaw(object.kinematics.initial_pose_with_covariance.pose.orientation); + const double obj_vel_yaw = + obj_yaw + std::atan2( + object.kinematics.initial_twist_with_covariance.twist.linear.y, + object.kinematics.initial_twist_with_covariance.twist.linear.x); *velocity = obj_vel_norm * std::cos(tier4_autoware_utils::normalizeRadian(obj_vel_yaw - traj_yaw)); @@ -676,15 +685,13 @@ void AdaptiveCruiseController::insertMaxVelocityToPath( const geometry_msgs::msg::Pose self_pose, const double current_vel, const double target_vel, const double dist_to_collision_point, TrajectoryPoints * output_trajectory) { - // plus distance from self to next nearest point - double dist = dist_to_collision_point; + // signed distance from self pose to the point of index 1 double dist_to_first_point = 0.0; + if (output_trajectory->size() > 1) { - dist_to_first_point = boost::geometry::distance( - convertPointRosToBoost(self_pose.position), - convertPointRosToBoost(output_trajectory->at(1).pose.position)); + dist_to_first_point = + motion_utils::calcSignedArcLength(*output_trajectory, self_pose.position, size_t(1)); } - dist += dist_to_first_point; double margin_to_insert = dist_to_collision_point * param_.margin_rate_to_change_vel; // accel = (v_after^2 - v_before^2 ) / 2x @@ -693,7 +700,7 @@ void AdaptiveCruiseController::insertMaxVelocityToPath( const double clipped_acc = boost::algorithm::clamp( target_acc, param_.min_standard_acceleration, param_.max_standard_acceleration); double pre_vel = current_vel; - double total_dist = 0.0; + double total_dist = dist_to_first_point; for (size_t i = 1; i < output_trajectory->size(); i++) { // calc velocity of each point by gradient deceleration const auto current_p = output_trajectory->at(i); @@ -706,7 +713,8 @@ void AdaptiveCruiseController::insertMaxVelocityToPath( next_pre_vel = pre_vel; } else { // v_after = sqrt (2x*accel + v_before^2) - next_pre_vel = std::sqrt(2 * p_dist * clipped_acc + std::pow(pre_vel, 2)); + next_pre_vel = + std::sqrt(2 * std::min(p_dist, total_dist) * clipped_acc + std::pow(pre_vel, 2)); } if (target_acc >= 0) { next_pre_vel = std::min(next_pre_vel, target_vel); diff --git a/planning/obstacle_stop_planner/src/debug_marker.cpp b/planning/obstacle_stop_planner/src/debug_marker.cpp index 15b0a721ed170..9277d373d498d 100644 --- a/planning/obstacle_stop_planner/src/debug_marker.cpp +++ b/planning/obstacle_stop_planner/src/debug_marker.cpp @@ -14,8 +14,9 @@ #include "obstacle_stop_planner/debug_marker.hpp" -#include -#include +#include +#include +#include #ifdef ROS_DISTRO_GALACTIC #include diff --git a/planning/obstacle_stop_planner/src/node.cpp b/planning/obstacle_stop_planner/src/node.cpp index bec94335c4739..9a9712c5a2503 100644 --- a/planning/obstacle_stop_planner/src/node.cpp +++ b/planning/obstacle_stop_planner/src/node.cpp @@ -209,6 +209,8 @@ ObstacleStopPlannerNode::ObstacleStopPlannerNode(const rclcpp::NodeOptions & nod pub_collision_pointcloud_debug_ = this->create_publisher("~/debug/collision_pointcloud", 1); + pub_processing_time_ms_ = this->create_publisher("~/debug/processing_time_ms", 1); + // Subscribers if (!node_param_.use_predicted_objects) { // No need to point cloud while using predicted objects @@ -242,6 +244,8 @@ ObstacleStopPlannerNode::ObstacleStopPlannerNode(const rclcpp::NodeOptions & nod "~/input/expand_stop_range", 1, std::bind(&ObstacleStopPlannerNode::onExpandStopRange, this, std::placeholders::_1), createSubscriptionOptions(this)); + + logger_configure_ = std::make_unique(this); } void ObstacleStopPlannerNode::onPointCloud(const PointCloud2::ConstSharedPtr input_msg) @@ -272,6 +276,8 @@ void ObstacleStopPlannerNode::onPointCloud(const PointCloud2::ConstSharedPtr inp void ObstacleStopPlannerNode::onTrigger(const Trajectory::ConstSharedPtr input_msg) { + stop_watch_.tic(__func__); + mutex_.lock(); // NOTE: these variables must not be referenced for multithreading const auto vehicle_info = vehicle_info_; @@ -374,6 +380,11 @@ void ObstacleStopPlannerNode::onTrigger(const Trajectory::ConstSharedPtr input_m trajectory.header = input_msg->header; pub_trajectory_->publish(trajectory); + + Float64Stamped processing_time_ms; + processing_time_ms.stamp = now(); + processing_time_ms.data = stop_watch_.toc(__func__); + pub_processing_time_ms_->publish(processing_time_ms); } void ObstacleStopPlannerNode::searchObstacle( diff --git a/planning/obstacle_stop_planner/src/planner_utils.cpp b/planning/obstacle_stop_planner/src/planner_utils.cpp index 45cced4599602..be0ef13626f92 100644 --- a/planning/obstacle_stop_planner/src/planner_utils.cpp +++ b/planning/obstacle_stop_planner/src/planner_utils.cpp @@ -17,10 +17,15 @@ #include #include #include +#include #include #include +#include +#include +#include +#include #include diff --git a/planning/obstacle_velocity_limiter/README.md b/planning/obstacle_velocity_limiter/README.md index d670f07d26e9d..36aa35bb31156 100644 --- a/planning/obstacle_velocity_limiter/README.md +++ b/planning/obstacle_velocity_limiter/README.md @@ -161,7 +161,7 @@ For example a value of `1` means all trajectory points will be evaluated while a | ------------------------------- | ---------------------------------------- | -------------------------------------------------------- | | `~/output/trajectory` | `autoware_auto_planning_msgs/Trajectory` | Trajectory with adjusted velocities | | `~/output/debug_markers` | `visualization_msgs/MarkerArray` | Debug markers (envelopes, obstacle polygons) | -| `~/output/runtime_microseconds` | `std_msgs/Int64` | Time taken to calculate the trajectory (in microseconds) | +| `~/output/runtime_microseconds` | `tier4_debug_msgs/Float64` | Time taken to calculate the trajectory (in microseconds) | ## Parameters diff --git a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacle_velocity_limiter_node.hpp b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacle_velocity_limiter_node.hpp index 8916137c84077..cbdd390183d61 100644 --- a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacle_velocity_limiter_node.hpp +++ b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacle_velocity_limiter_node.hpp @@ -18,6 +18,7 @@ #include "obstacle_velocity_limiter/obstacles.hpp" #include "obstacle_velocity_limiter/parameters.hpp" #include "obstacle_velocity_limiter/types.hpp" +#include "tier4_autoware_utils/ros/logger_level_configure.hpp" #include #include @@ -28,7 +29,7 @@ #include #include #include -#include +#include #include #include @@ -55,7 +56,7 @@ class ObstacleVelocityLimiterNode : public rclcpp::Node pub_trajectory_; //!< @brief publisher for output trajectory rclcpp::Publisher::SharedPtr pub_debug_markers_; //!< @brief publisher for debug markers - rclcpp::Publisher::SharedPtr + rclcpp::Publisher::SharedPtr pub_runtime_; //!< @brief publisher for callback runtime rclcpp::Subscription::SharedPtr sub_trajectory_; //!< @brief subscriber for reference trajectory @@ -101,6 +102,8 @@ class ObstacleVelocityLimiterNode : public rclcpp::Node /// @brief validate the inputs of the node /// @return true if the inputs are valid bool validInputs(); + + std::unique_ptr logger_configure_; }; } // namespace obstacle_velocity_limiter diff --git a/planning/obstacle_velocity_limiter/src/obstacle_velocity_limiter_node.cpp b/planning/obstacle_velocity_limiter/src/obstacle_velocity_limiter_node.cpp index 9ab609378621a..086d76e9ef0fe 100644 --- a/planning/obstacle_velocity_limiter/src/obstacle_velocity_limiter_node.cpp +++ b/planning/obstacle_velocity_limiter/src/obstacle_velocity_limiter_node.cpp @@ -23,7 +23,7 @@ #include "obstacle_velocity_limiter/types.hpp" #include -#include +#include #include #include #include @@ -68,7 +68,8 @@ ObstacleVelocityLimiterNode::ObstacleVelocityLimiterNode(const rclcpp::NodeOptio pub_trajectory_ = create_publisher("~/output/trajectory", 1); pub_debug_markers_ = create_publisher("~/output/debug_markers", 1); - pub_runtime_ = create_publisher("~/output/runtime_microseconds", 1); + pub_runtime_ = + create_publisher("~/output/runtime_microseconds", 1); const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); vehicle_lateral_offset_ = static_cast(vehicle_info.max_lateral_offset_m); @@ -79,6 +80,8 @@ ObstacleVelocityLimiterNode::ObstacleVelocityLimiterNode(const rclcpp::NodeOptio set_param_res_ = add_on_set_parameters_callback([this](const auto & params) { return onParameter(params); }); + + logger_configure_ = std::make_unique(this); } rcl_interfaces::msg::SetParametersResult ObstacleVelocityLimiterNode::onParameter( @@ -221,7 +224,8 @@ void ObstacleVelocityLimiterNode::onTrajectory(const Trajectory::ConstSharedPtr const auto t_end = std::chrono::system_clock::now(); const auto runtime = std::chrono::duration_cast(t_end - t_start); - pub_runtime_->publish(std_msgs::msg::Int64().set__data(runtime.count())); + pub_runtime_->publish(tier4_debug_msgs::msg::Float64Stamped().set__stamp(now()).set__data( + static_cast(runtime.count()))); if (pub_debug_markers_->get_subscription_count() > 0) { const auto safe_projected_linestrings = diff --git a/planning/path_smoother/include/path_smoother/common_structs.hpp b/planning/path_smoother/include/path_smoother/common_structs.hpp index 94f4d62c4a6fd..d44c964cf634c 100644 --- a/planning/path_smoother/include/path_smoother/common_structs.hpp +++ b/planning/path_smoother/include/path_smoother/common_structs.hpp @@ -17,7 +17,8 @@ #include "path_smoother/type_alias.hpp" #include "rclcpp/rclcpp.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" +#include "tier4_autoware_utils/ros/update_param.hpp" +#include "tier4_autoware_utils/system/stop_watch.hpp" #include #include @@ -43,7 +44,11 @@ struct PlannerData struct TimeKeeper { - void init() { accumulated_msg = "\n"; } + void init() + { + accumulated_msg = "\n"; + accumulated_time = 0.0; + } template TimeKeeper & operator<<(const T & msg) @@ -65,6 +70,7 @@ struct TimeKeeper { const double elapsed_time = stop_watch_.toc(func_name); *this << white_spaces << func_name << ":= " << elapsed_time << " [ms]"; + accumulated_time = elapsed_time; endLine(); } @@ -73,6 +79,10 @@ struct TimeKeeper std::string accumulated_msg = "\n"; std::stringstream latest_stream; + double getAccumulatedTime() const { return accumulated_time; } + + double accumulated_time{0.0}; + tier4_autoware_utils::StopWatch< std::chrono::milliseconds, std::chrono::microseconds, std::chrono::steady_clock> stop_watch_; diff --git a/planning/path_smoother/include/path_smoother/elastic_band_smoother.hpp b/planning/path_smoother/include/path_smoother/elastic_band_smoother.hpp index 1085ca3815728..217a138084310 100644 --- a/planning/path_smoother/include/path_smoother/elastic_band_smoother.hpp +++ b/planning/path_smoother/include/path_smoother/elastic_band_smoother.hpp @@ -15,13 +15,13 @@ #ifndef PATH_SMOOTHER__ELASTIC_BAND_SMOOTHER_HPP_ #define PATH_SMOOTHER__ELASTIC_BAND_SMOOTHER_HPP_ -#include "motion_utils/motion_utils.hpp" +#include "motion_utils/trajectory/trajectory.hpp" #include "path_smoother/common_structs.hpp" #include "path_smoother/elastic_band.hpp" #include "path_smoother/replan_checker.hpp" #include "path_smoother/type_alias.hpp" #include "rclcpp/rclcpp.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" +#include "tier4_autoware_utils/ros/logger_level_configure.hpp" #include #include @@ -82,7 +82,8 @@ class ElasticBandSmoother : public rclcpp::Node // debug publisher rclcpp::Publisher::SharedPtr debug_extended_traj_pub_; - rclcpp::Publisher::SharedPtr debug_calculation_time_pub_; + rclcpp::Publisher::SharedPtr debug_calculation_time_str_pub_; + rclcpp::Publisher::SharedPtr debug_calculation_time_float_pub_; // parameter callback rcl_interfaces::msg::SetParametersResult onParam( @@ -105,6 +106,8 @@ class ElasticBandSmoother : public rclcpp::Node std::vector extendTrajectory( const std::vector & traj_points, const std::vector & optimized_points) const; + + std::unique_ptr logger_configure_; }; } // namespace path_smoother diff --git a/planning/path_smoother/include/path_smoother/type_alias.hpp b/planning/path_smoother/include/path_smoother/type_alias.hpp index 75bf1cff20857..c8f6d6da98dcd 100644 --- a/planning/path_smoother/include/path_smoother/type_alias.hpp +++ b/planning/path_smoother/include/path_smoother/type_alias.hpp @@ -22,6 +22,7 @@ #include "geometry_msgs/msg/point.hpp" #include "nav_msgs/msg/odometry.hpp" #include "std_msgs/msg/header.hpp" +#include "tier4_debug_msgs/msg/float64_stamped.hpp" #include "tier4_debug_msgs/msg/string_stamped.hpp" namespace path_smoother @@ -36,6 +37,7 @@ using autoware_auto_planning_msgs::msg::TrajectoryPoint; // navigation using nav_msgs::msg::Odometry; // debug +using tier4_debug_msgs::msg::Float64Stamped; using tier4_debug_msgs::msg::StringStamped; } // namespace path_smoother diff --git a/planning/path_smoother/src/elastic_band.cpp b/planning/path_smoother/src/elastic_band.cpp index d77ad542e2e7e..fb3aff7b18419 100644 --- a/planning/path_smoother/src/elastic_band.cpp +++ b/planning/path_smoother/src/elastic_band.cpp @@ -14,7 +14,7 @@ #include "path_smoother/elastic_band.hpp" -#include "motion_utils/motion_utils.hpp" +#include "motion_utils/trajectory/trajectory.hpp" #include "path_smoother/type_alias.hpp" #include "path_smoother/utils/geometry_utils.hpp" #include "path_smoother/utils/trajectory_utils.hpp" diff --git a/planning/path_smoother/src/elastic_band_smoother.cpp b/planning/path_smoother/src/elastic_band_smoother.cpp index dbd0f2de92f29..923b753e83ac6 100644 --- a/planning/path_smoother/src/elastic_band_smoother.cpp +++ b/planning/path_smoother/src/elastic_band_smoother.cpp @@ -43,6 +43,14 @@ StringStamped createStringStamped(const rclcpp::Time & now, const std::string & return msg; } +Float64Stamped createFloat64Stamped(const rclcpp::Time & now, const float & data) +{ + Float64Stamped msg; + msg.stamp = now; + msg.data = data; + return msg; +} + void setZeroVelocityAfterStopPoint(std::vector & traj_points) { const auto opt_zero_vel_idx = motion_utils::searchZeroVelocityIndex(traj_points); @@ -75,7 +83,9 @@ ElasticBandSmoother::ElasticBandSmoother(const rclcpp::NodeOptions & node_option // debug publisher debug_extended_traj_pub_ = create_publisher("~/debug/extended_traj", 1); - debug_calculation_time_pub_ = create_publisher("~/debug/calculation_time", 1); + debug_calculation_time_str_pub_ = create_publisher("~/debug/calculation_time", 1); + debug_calculation_time_float_pub_ = + create_publisher("~/debug/processing_time_ms", 1); { // parameters // parameters for ego nearest search @@ -95,6 +105,8 @@ ElasticBandSmoother::ElasticBandSmoother(const rclcpp::NodeOptions & node_option // set parameter callback set_param_res_ = this->add_on_set_parameters_callback( std::bind(&ElasticBandSmoother::onParam, this, std::placeholders::_1)); + + logger_configure_ = std::make_unique(this); } rcl_interfaces::msg::SetParametersResult ElasticBandSmoother::onParam( @@ -203,7 +215,9 @@ void ElasticBandSmoother::onPath(const Path::ConstSharedPtr path_ptr) // publish calculation_time // NOTE: This function must be called after measuring onPath calculation time const auto calculation_time_msg = createStringStamped(now(), time_keeper_ptr_->getLog()); - debug_calculation_time_pub_->publish(calculation_time_msg); + debug_calculation_time_str_pub_->publish(calculation_time_msg); + debug_calculation_time_float_pub_->publish( + createFloat64Stamped(now(), time_keeper_ptr_->getAccumulatedTime())); const auto output_traj_msg = trajectory_utils::createTrajectory(path_ptr->header, full_traj_points); @@ -270,12 +284,36 @@ void ElasticBandSmoother::applyInputVelocity( // insert stop point explicitly const auto stop_idx = motion_utils::searchZeroVelocityIndex(forward_cropped_input_traj_points); if (stop_idx) { - const auto input_stop_pose = forward_cropped_input_traj_points.at(stop_idx.get()).pose; - const size_t stop_seg_idx = trajectory_utils::findEgoSegmentIndex( - output_traj_points, input_stop_pose, ego_nearest_param_); + const auto & input_stop_pose = forward_cropped_input_traj_points.at(stop_idx.get()).pose; + // NOTE: motion_utils::findNearestSegmentIndex is used instead of + // trajectory_utils::findEgoSegmentIndex + // for the case where input_traj_points is much longer than output_traj_points, and the + // former has a stop point but the latter will not have. + const auto stop_seg_idx = motion_utils::findNearestSegmentIndex( + output_traj_points, input_stop_pose, ego_nearest_param_.dist_threshold, + ego_nearest_param_.yaw_threshold); // calculate and insert stop pose on output trajectory - trajectory_utils::insertStopPoint(output_traj_points, input_stop_pose, stop_seg_idx); + const bool is_stop_point_inside_trajectory = [&]() { + if (!stop_seg_idx) { + return false; + } + if (*stop_seg_idx == output_traj_points.size() - 2) { + const double signed_projected_length_to_segment = + motion_utils::calcLongitudinalOffsetToSegment( + output_traj_points, *stop_seg_idx, input_stop_pose.position); + const double segment_length = + motion_utils::calcSignedArcLength(output_traj_points, *stop_seg_idx, *stop_seg_idx + 1); + if (segment_length < signed_projected_length_to_segment) { + // NOTE: input_stop_pose is outside output_traj_points. + return false; + } + } + return true; + }(); + if (is_stop_point_inside_trajectory) { + trajectory_utils::insertStopPoint(output_traj_points, input_stop_pose, *stop_seg_idx); + } } time_keeper_ptr_->toc(__func__, " "); @@ -299,25 +337,25 @@ std::vector ElasticBandSmoother::extendTrajectory( const auto joint_end_traj_point_idx = trajectory_utils::getPointIndexAfter( traj_points, joint_start_pose.position, joint_start_traj_seg_idx, joint_traj_max_length_for_smoothing, joint_traj_min_length_for_smoothing); + if (!joint_end_traj_point_idx) { + return trajectory_utils::resampleTrajectoryPoints( + optimized_traj_points, common_param_.output_delta_arc_length); + } // calculate full trajectory points const auto full_traj_points = [&]() { - if (!joint_end_traj_point_idx) { - return optimized_traj_points; - } - - const auto extended_traj_points = std::vector{ + auto extended_traj_points = std::vector{ traj_points.begin() + *joint_end_traj_point_idx, traj_points.end()}; - // NOTE: if optimized_traj_points's back is non zero velocity and extended_traj_points' front is - // zero velocity, the zero velocity will be inserted in the whole joint trajectory. - auto modified_optimized_traj_points = optimized_traj_points; - if (!extended_traj_points.empty() && !modified_optimized_traj_points.empty()) { - modified_optimized_traj_points.back().longitudinal_velocity_mps = - extended_traj_points.front().longitudinal_velocity_mps; + if (!extended_traj_points.empty() && !optimized_traj_points.empty()) { + // NOTE: Without this code, if optimized_traj_points's back is non zero velocity and + // extended_traj_points' front + // is zero velocity, the zero velocity will be inserted in the whole joint trajectory. + // The input stop point will be inserted explicitly in the latter part. + extended_traj_points.front().longitudinal_velocity_mps = + optimized_traj_points.back().longitudinal_velocity_mps; } - - return concatVectors(modified_optimized_traj_points, extended_traj_points); + return concatVectors(optimized_traj_points, extended_traj_points); }(); // resample trajectory points @@ -325,7 +363,7 @@ std::vector ElasticBandSmoother::extendTrajectory( full_traj_points, common_param_.output_delta_arc_length); // update stop velocity on joint - for (size_t i = joint_start_traj_seg_idx + 1; i <= joint_end_traj_point_idx; ++i) { + for (size_t i = joint_start_traj_seg_idx + 1; i <= *joint_end_traj_point_idx; ++i) { if (hasZeroVelocity(traj_points.at(i))) { if (i != 0 && !hasZeroVelocity(traj_points.at(i - 1))) { // Here is when current point is 0 velocity, but previous point is not 0 velocity. diff --git a/planning/path_smoother/src/replan_checker.cpp b/planning/path_smoother/src/replan_checker.cpp index 95c4e1eb1c002..11a4a3d5f4dad 100644 --- a/planning/path_smoother/src/replan_checker.cpp +++ b/planning/path_smoother/src/replan_checker.cpp @@ -14,9 +14,10 @@ #include "path_smoother/replan_checker.hpp" -#include "motion_utils/motion_utils.hpp" +#include "motion_utils/trajectory/trajectory.hpp" #include "path_smoother/utils/trajectory_utils.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" +#include "tier4_autoware_utils/ros/update_param.hpp" #include diff --git a/planning/path_smoother/src/utils/trajectory_utils.cpp b/planning/path_smoother/src/utils/trajectory_utils.cpp index 56432d16d1a7f..d9d02a15628dd 100644 --- a/planning/path_smoother/src/utils/trajectory_utils.cpp +++ b/planning/path_smoother/src/utils/trajectory_utils.cpp @@ -14,7 +14,8 @@ #include "path_smoother/utils/trajectory_utils.hpp" -#include "motion_utils/motion_utils.hpp" +#include "motion_utils/resample/resample.hpp" +#include "motion_utils/trajectory/tmp_conversion.hpp" #include "path_smoother/utils/geometry_utils.hpp" #include "autoware_auto_planning_msgs/msg/path_point.hpp" diff --git a/planning/planning_debug_tools/CMakeLists.txt b/planning/planning_debug_tools/CMakeLists.txt index bf7e36c4c0c0a..58f73ca3836c2 100644 --- a/planning/planning_debug_tools/CMakeLists.txt +++ b/planning/planning_debug_tools/CMakeLists.txt @@ -48,6 +48,7 @@ ament_auto_package( ) install(PROGRAMS + scripts/processing_time_checker.py scripts/trajectory_visualizer.py scripts/closest_velocity_checker.py scripts/perception_replayer/perception_reproducer.py diff --git a/planning/planning_debug_tools/Readme.md b/planning/planning_debug_tools/README.md similarity index 89% rename from planning/planning_debug_tools/Readme.md rename to planning/planning_debug_tools/README.md index fe9615614de81..aa46c0e2fc7ef 100644 --- a/planning/planning_debug_tools/Readme.md +++ b/planning/planning_debug_tools/README.md @@ -4,6 +4,8 @@ This package contains several planning-related debug tools. - **Trajectory analyzer**: visualizes the information (speed, curvature, yaw, etc) along the trajectory - **Closest velocity checker**: prints the velocity information indicated by each modules +- **Perception reproducer**: generates detected objects from rosbag data in planning simulator environment +- **processing time checker**: displays processing_time of modules on the terminal ## Trajectory analyzer @@ -241,3 +243,24 @@ ros2 run planning_debug_tools perception_replayer.py -b ``` Instead of publishing predicted objects, you can publish detected/tracked objects by designating `-d` or `-t`, respectively. + +## Processing time checker + +The purpose of the Processing Time Subscriber is to monitor and visualize the processing times of various ROS 2 topics in a system. By providing a real-time terminal-based visualization, users can easily confirm the processing time performance as in the picture below. + +![processing_time_checker](image/processing_time_checker.png) + +You can run the program by the following command. + +```bash +ros2 run planning_debug_tools processing_time_checker.py -f -m +``` + +This program subscribes to ROS 2 topics that have a suffix of `processing_time_ms`. + +The program allows users to customize two parameters via command-line arguments: + +- --max_display_time (or -m): This sets the maximum display time in milliseconds. The default value is 150ms. +- --display_frequency (or -f): This sets the frequency at which the terminal UI updates. The default value is 5Hz. + +By adjusting these parameters, users can tailor the display to their specific monitoring needs. diff --git a/planning/planning_debug_tools/image/processing_time_checker.png b/planning/planning_debug_tools/image/processing_time_checker.png new file mode 100644 index 0000000000000..fb064372796ae Binary files /dev/null and b/planning/planning_debug_tools/image/processing_time_checker.png differ diff --git a/planning/planning_debug_tools/include/planning_debug_tools/trajectory_analyzer.hpp b/planning/planning_debug_tools/include/planning_debug_tools/trajectory_analyzer.hpp index a70ab56d98a94..d460cbf0c10e2 100644 --- a/planning/planning_debug_tools/include/planning_debug_tools/trajectory_analyzer.hpp +++ b/planning/planning_debug_tools/include/planning_debug_tools/trajectory_analyzer.hpp @@ -20,7 +20,6 @@ #include "planning_debug_tools/util.hpp" #include "rclcpp/rclcpp.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" -#include "tier4_autoware_utils/geometry/path_with_lane_id_geometry.hpp" #include "autoware_auto_planning_msgs/msg/path.hpp" #include "autoware_auto_planning_msgs/msg/path_with_lane_id.hpp" diff --git a/planning/planning_debug_tools/include/planning_debug_tools/util.hpp b/planning/planning_debug_tools/include/planning_debug_tools/util.hpp index 1e332e7127dc4..1f0b9a9345854 100644 --- a/planning/planning_debug_tools/include/planning_debug_tools/util.hpp +++ b/planning/planning_debug_tools/include/planning_debug_tools/util.hpp @@ -18,7 +18,6 @@ #include "motion_utils/trajectory/trajectory.hpp" #include "rclcpp/rclcpp.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" -#include "tier4_autoware_utils/geometry/path_with_lane_id_geometry.hpp" #include "autoware_auto_planning_msgs/msg/path.hpp" #include "autoware_auto_planning_msgs/msg/path_with_lane_id.hpp" diff --git a/planning/planning_debug_tools/package.xml b/planning/planning_debug_tools/package.xml index deadcd54fffdd..14db1277cc02a 100644 --- a/planning/planning_debug_tools/package.xml +++ b/planning/planning_debug_tools/package.xml @@ -6,6 +6,7 @@ The planning_debug_tools package Takamasa Horibe Taiki Tanaka + Takayuki Murooka Apache License 2.0 Takamasa Horibe diff --git a/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer.py b/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer.py index b45357c7bd8b2..498f7e458b2e2 100755 --- a/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer.py +++ b/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer.py @@ -20,6 +20,7 @@ import sys from PyQt5.QtWidgets import QApplication +from geometry_msgs.msg import PoseWithCovarianceStamped from perception_replayer_common import PerceptionReplayerCommon import rclpy from time_manager_widget import TimeManagerWidget @@ -34,6 +35,7 @@ def __init__(self, args): self.bag_timestamp = self.rosbag_objects_data[0][0] self.is_pause = False self.rate = 1.0 + self.prev_traffic_signals_msg = None # initialize widget self.widget = TimeManagerWidget( @@ -43,6 +45,7 @@ def __init__(self, args): self.widget.button.clicked.connect(self.onPushed) for button in self.widget.rate_button: button.clicked.connect(functools.partial(self.onSetRate, button)) + self.widget.pub_recorded_ego_pose_button.clicked.connect(self.publish_recorded_ego_pose) # start timer callback self.delta_time = 0.1 @@ -92,13 +95,22 @@ def on_timer(self): self.objects_pub.publish(objects_msg) # traffic signals + # temporary support old auto msgs if traffic_signals_msg: - traffic_signals_msg.header.stamp = timestamp - self.traffic_signals_pub.publish(traffic_signals_msg) + if self.is_auto_traffic_signals: + traffic_signals_msg.header.stamp = timestamp + self.auto_traffic_signals_pub.publish(traffic_signals_msg) + else: + traffic_signals_msg.stamp = timestamp + self.traffic_signals_pub.publish(traffic_signals_msg) self.prev_traffic_signals_msg = traffic_signals_msg elif self.prev_traffic_signals_msg: - self.prev_traffic_signals_msg.header.stamp = timestamp - self.traffic_signals_pub.publish(self.prev_traffic_signals_msg) + if self.is_auto_traffic_signals: + self.prev_traffic_signals_msg.header.stamp = timestamp + self.auto_traffic_signals_pub.publish(self.prev_traffic_signals_msg) + else: + self.prev_traffic_signals_msg.stamp = timestamp + self.traffic_signals_pub.publish(self.prev_traffic_signals_msg) def onPushed(self, event): if self.widget.button.isChecked(): @@ -109,6 +121,57 @@ def onPushed(self, event): def onSetRate(self, button): self.rate = float(button.text()) + def publish_recorded_ego_pose(self): + ego_odom = self.find_ego_odom_by_timestamp(self.bag_timestamp) + if not ego_odom: + return + + recorded_ego_pose = PoseWithCovarianceStamped() + recorded_ego_pose.header.stamp = self.get_clock().now().to_msg() + recorded_ego_pose.header.frame_id = "map" + recorded_ego_pose.pose.pose = ego_odom.pose.pose + recorded_ego_pose.pose.covariance = [ + 0.25, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.25, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.06853892326654787, + ] + + self.recorded_ego_pub_as_initialpose.publish(recorded_ego_pose) + print("Published recorded ego pose as /initialpose") + if __name__ == "__main__": parser = argparse.ArgumentParser() @@ -119,6 +182,9 @@ def onSetRate(self, button): parser.add_argument( "-t", "--tracked-object", help="publish tracked object", action="store_true" ) + parser.add_argument( + "-f", "--rosbag-format", help="rosbag data format (default is db3)", default="db3" + ) args = parser.parse_args() app = QApplication(sys.argv) diff --git a/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer_common.py b/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer_common.py index a86a9ae9b2bb0..7bf54c0278a27 100644 --- a/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer_common.py +++ b/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer_common.py @@ -22,7 +22,9 @@ from autoware_auto_perception_msgs.msg import DetectedObjects from autoware_auto_perception_msgs.msg import PredictedObjects from autoware_auto_perception_msgs.msg import TrackedObjects -from autoware_auto_perception_msgs.msg import TrafficSignalArray +from autoware_auto_perception_msgs.msg import TrafficSignalArray as AutoTrafficSignalArray +from autoware_perception_msgs.msg import TrafficSignalArray +from geometry_msgs.msg import PoseWithCovarianceStamped from nav_msgs.msg import Odometry import psutil from rclpy.node import Node @@ -42,6 +44,7 @@ def __init__(self, args, name): self.rosbag_objects_data = [] self.rosbag_ego_odom_data = [] self.rosbag_traffic_signals_data = [] + self.is_auto_traffic_signals = False # subscriber self.sub_odom = self.create_subscription( @@ -65,19 +68,34 @@ def __init__(self, args, name): self.pointcloud_pub = self.create_publisher( PointCloud2, "/perception/obstacle_segmentation/pointcloud", 1 ) - self.traffic_signals_pub = self.create_publisher( - TrafficSignalArray, "/perception/traffic_light_recognition/traffic_signals", 1 + self.recorded_ego_pub_as_initialpose = self.create_publisher( + PoseWithCovarianceStamped, "/initialpose", 1 + ) + + self.recorded_ego_pub = self.create_publisher( + Odometry, "/perception_reproducer/rosbag_ego_odom", 1 ) # load rosbag print("Stared loading rosbag") if os.path.isdir(args.bag): for bag_file in sorted(os.listdir(args.bag)): - self.load_rosbag(args.bag + "/" + bag_file) + if bag_file.endswith(self.args.rosbag_format): + self.load_rosbag(args.bag + "/" + bag_file) else: self.load_rosbag(args.bag) print("Ended loading rosbag") + # temporary support old auto msgs + if self.is_auto_traffic_signals: + self.auto_traffic_signals_pub = self.create_publisher( + AutoTrafficSignalArray, "/perception/traffic_light_recognition/traffic_signals", 1 + ) + else: + self.traffic_signals_pub = self.create_publisher( + TrafficSignalArray, "/perception/traffic_light_recognition/traffic_signals", 1 + ) + # wait for ready to publish/subscribe time.sleep(1.0) @@ -113,6 +131,9 @@ def load_rosbag(self, rosbag2_path: str): self.rosbag_ego_odom_data.append((stamp, msg)) if topic == traffic_signals_topic: self.rosbag_traffic_signals_data.append((stamp, msg)) + self.is_auto_traffic_signals = ( + "autoware_auto_perception_msgs" in type(msg).__module__ + ) def kill_online_perception_node(self): # kill node if required @@ -131,26 +152,32 @@ def kill_online_perception_node(self): except CalledProcessError: pass - def find_topics_by_timestamp(self, timestamp): - objects_data = None - for data in self.rosbag_objects_data: - if timestamp < data[0]: - objects_data = data[1] - break - - traffic_signals_data = None - for data in self.rosbag_traffic_signals_data: - if timestamp < data[0]: - traffic_signals_data = data[1] - break + def binary_search(self, data, timestamp): + if data[-1][0] < timestamp: + return data[-1][1] + elif data[0][0] > timestamp: + return data[0][1] + + low, high = 0, len(data) - 1 + + while low <= high: + mid = (low + high) // 2 + if data[mid][0] < timestamp: + low = mid + 1 + elif data[mid][0] > timestamp: + high = mid - 1 + else: + return data[mid][1] + # Return the next timestamp's data if available + if low < len(data): + return data[low][1] + return None + + def find_topics_by_timestamp(self, timestamp): + objects_data = self.binary_search(self.rosbag_objects_data, timestamp) + traffic_signals_data = self.binary_search(self.rosbag_traffic_signals_data, timestamp) return objects_data, traffic_signals_data def find_ego_odom_by_timestamp(self, timestamp): - ego_odom_data = None - for data in self.rosbag_ego_odom_data: - if timestamp < data[0]: - ego_odom_data = data[1] - break - - return ego_odom_data + return self.binary_search(self.rosbag_ego_odom_data, timestamp) diff --git a/planning/planning_debug_tools/scripts/perception_replayer/perception_reproducer.py b/planning/planning_debug_tools/scripts/perception_replayer/perception_reproducer.py index 4228d506d5bec..d837abccc0670 100755 --- a/planning/planning_debug_tools/scripts/perception_replayer/perception_reproducer.py +++ b/planning/planning_debug_tools/scripts/perception_replayer/perception_reproducer.py @@ -15,11 +15,12 @@ # limitations under the License. import argparse -import copy +import pickle +import numpy as np from perception_replayer_common import PerceptionReplayerCommon import rclpy -from utils import calc_squared_distance +from utils import StopWatch from utils import create_empty_pointcloud from utils import translate_objects_coordinate @@ -28,18 +29,39 @@ class PerceptionReproducer(PerceptionReplayerCommon): def __init__(self, args): super().__init__(args, "perception_reproducer") - self.ego_pose_idx = None self.prev_traffic_signals_msg = None + self.stopwatch = StopWatch(self.args.verbose) # for debug - # start timer callback + # to make some data to accelerate computation + self.preprocess_data() + + # start main timer callback self.timer = self.create_timer(0.1, self.on_timer) + + # kill perception process to avoid a conflict of the perception topics + self.timer_check_perception_process = self.create_timer(3.0, self.on_timer_kill_perception) + print("Start timer callback") - def on_timer(self): - timestamp = self.get_clock().now().to_msg() + def preprocess_data(self): + # closest search with numpy data is much faster than usual + self.rosbag_ego_odom_data_numpy = np.array( + [ + [data[1].pose.pose.position.x, data[1].pose.pose.position.y] + for data in self.rosbag_ego_odom_data + ] + ) + def on_timer_kill_perception(self): self.kill_online_perception_node() + def on_timer(self): + if self.args.verbose: + print("\n-- on_timer start --") + self.stopwatch.tic("total on_timer") + + timestamp = self.get_clock().now().to_msg() + if self.args.detected_object: pointcloud_msg = create_empty_pointcloud(timestamp) self.pointcloud_pub.publish(pointcloud_msg) @@ -49,15 +71,25 @@ def on_timer(self): return # find nearest ego odom by simulation observation + self.stopwatch.tic("find_nearest_ego_odom_by_observation") ego_odom = self.find_nearest_ego_odom_by_observation(self.ego_pose) pose_timestamp = ego_odom[0] log_ego_pose = ego_odom[1].pose.pose + self.stopwatch.toc("find_nearest_ego_odom_by_observation") # extract message by the nearest ego odom timestamp - msgs = copy.deepcopy(self.find_topics_by_timestamp(pose_timestamp)) + self.stopwatch.tic("find_topics_by_timestamp") + msgs_orig = self.find_topics_by_timestamp(pose_timestamp) + self.stopwatch.toc("find_topics_by_timestamp") + + # copy the messages + self.stopwatch.tic("message deepcopy") + msgs = pickle.loads(pickle.dumps(msgs_orig)) # this is x5 faster than deepcopy objects_msg = msgs[0] traffic_signals_msg = msgs[1] + self.stopwatch.toc("message deepcopy") + self.stopwatch.tic("transform and publish") # objects if objects_msg: objects_msg.header.stamp = timestamp @@ -65,31 +97,35 @@ def on_timer(self): translate_objects_coordinate(self.ego_pose, log_ego_pose, objects_msg) self.objects_pub.publish(objects_msg) + # ego odom + self.recorded_ego_pub.publish(ego_odom[1]) + # traffic signals + # temporary support old auto msgs if traffic_signals_msg: - traffic_signals_msg.header.stamp = timestamp - self.traffic_signals_pub.publish(traffic_signals_msg) + if self.is_auto_traffic_signals: + traffic_signals_msg.header.stamp = timestamp + self.auto_traffic_signals_pub.publish(traffic_signals_msg) + else: + traffic_signals_msg.stamp = timestamp + self.traffic_signals_pub.publish(traffic_signals_msg) self.prev_traffic_signals_msg = traffic_signals_msg elif self.prev_traffic_signals_msg: - self.prev_traffic_signals_msg.header.stamp = timestamp - self.traffic_signals_pub.publish(self.prev_traffic_signals_msg) + if self.is_auto_traffic_signals: + self.prev_traffic_signals_msg.header.stamp = timestamp + self.auto_traffic_signals_pub.publish(self.prev_traffic_signals_msg) + else: + self.prev_traffic_signals_msg.stamp = timestamp + self.traffic_signals_pub.publish(self.prev_traffic_signals_msg) + self.stopwatch.toc("transform and publish") + + self.stopwatch.toc("total on_timer") def find_nearest_ego_odom_by_observation(self, ego_pose): - if self.ego_pose_idx: - start_idx = self.ego_pose_idx - 10 - end_idx = self.ego_pose_idx + 10 - else: - start_idx = 0 - end_idx = len(self.rosbag_ego_odom_data) - 1 - - nearest_idx = 0 - nearest_dist = float("inf") - for idx in range(start_idx, end_idx + 1): - data = self.rosbag_ego_odom_data[idx] - dist = calc_squared_distance(data[1].pose.pose.position, ego_pose.position) - if dist < nearest_dist: - nearest_dist = dist - nearest_idx = idx + # nearest search with numpy format is much (~ x100) faster than regular for loop + self_pose = np.array([ego_pose.position.x, ego_pose.position.y]) + dists_squared = np.sum((self.rosbag_ego_odom_data_numpy - self_pose) ** 2, axis=1) + nearest_idx = np.argmin(dists_squared) return self.rosbag_ego_odom_data[nearest_idx] @@ -103,6 +139,12 @@ def find_nearest_ego_odom_by_observation(self, ego_pose): parser.add_argument( "-t", "--tracked-object", help="publish tracked object", action="store_true" ) + parser.add_argument( + "-f", "--rosbag-format", help="rosbag data format (default is db3)", default="db3" + ) + parser.add_argument( + "-v", "--verbose", help="output debug data", action="store_true", default=False + ) args = parser.parse_args() rclpy.init() diff --git a/planning/planning_debug_tools/scripts/perception_replayer/time_manager_widget.py b/planning/planning_debug_tools/scripts/perception_replayer/time_manager_widget.py index 382e725b114cd..a1d87a8b06266 100644 --- a/planning/planning_debug_tools/scripts/perception_replayer/time_manager_widget.py +++ b/planning/planning_debug_tools/scripts/perception_replayer/time_manager_widget.py @@ -89,13 +89,15 @@ def setupUI(self): self.button.setCheckable(True) self.button.setSizePolicy(QSizePolicy.Expanding, QSizePolicy.Expanding) self.grid_layout.addWidget(self.button, 1, 0, 1, -1) + self.pub_recorded_ego_pose_button = QPushButton("publish recorded ego pose") + self.grid_layout.addWidget(self.pub_recorded_ego_pose_button, 2, 0, 1, -1) # slider self.slider = QJumpSlider(QtCore.Qt.Horizontal, self.max_value) self.slider.setMinimum(0) self.slider.setMaximum(self.max_value) self.slider.setValue(0) - self.grid_layout.addWidget(self.slider, 2, 0, 1, -1) + self.grid_layout.addWidget(self.slider, 3, 0, 1, -1) self.setCentralWidget(self.central_widget) diff --git a/planning/planning_debug_tools/scripts/perception_replayer/utils.py b/planning/planning_debug_tools/scripts/perception_replayer/utils.py index 572922d4c7abb..5ded8d2ea409c 100644 --- a/planning/planning_debug_tools/scripts/perception_replayer/utils.py +++ b/planning/planning_debug_tools/scripts/perception_replayer/utils.py @@ -15,6 +15,7 @@ # limitations under the License. import math +import time from geometry_msgs.msg import Quaternion import numpy as np @@ -123,3 +124,29 @@ def translate_objects_coordinate(ego_pose, log_ego_pose, objects_msg): object_pose.position.x = object_pos_vec[0] object_pose.position.y = object_pos_vec[1] object_pose.orientation = get_quaternion_from_yaw(log_object_yaw + log_ego_yaw - ego_yaw) + + +class StopWatch: + def __init__(self, verbose): + # A dictionary to store the starting times + self.start_times = {} + self.verbose = verbose + + def tic(self, name): + """Store the current time with the given name.""" + self.start_times[name] = time.perf_counter() + + def toc(self, name): + """Print the elapsed time since the last call to tic() with the same name.""" + if name not in self.start_times: + print(f"No start time found for {name}!") + return + + elapsed_time = ( + time.perf_counter() - self.start_times[name] + ) * 1000 # Convert to milliseconds + if self.verbose: + print(f"Time for {name}: {elapsed_time:.2f} ms") + + # Reset the starting time for the name + del self.start_times[name] diff --git a/planning/planning_debug_tools/scripts/processing_time_checker.py b/planning/planning_debug_tools/scripts/processing_time_checker.py new file mode 100755 index 0000000000000..fa6bc57fb7ae5 --- /dev/null +++ b/planning/planning_debug_tools/scripts/processing_time_checker.py @@ -0,0 +1,155 @@ +#!/usr/bin/env python3 + +# Copyright 2023 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 +from collections import deque +import os +import sys + +import rclpy +from rclpy.node import Node +from tier4_debug_msgs.msg import Float64Stamped + + +class ProcessingTimeSubscriber(Node): + def __init__( + self, + max_display_time=150, + display_frequency=5.0, + window_size=1, + bar_scale=2, + display_character="|", + ): + super().__init__("processing_time_subscriber") + self.data_map = {} # {topic_name: data_value} + self.data_queue_map = {} # {topic_name: deque} + self.window_size = window_size + self.max_display_time = max_display_time + self.bar_scale = bar_scale + self.display_character = display_character + + # Initialize a set to keep track of subscribed topics + self.subscribed_topics = set() + + # Call get_topic_list immediately and set a timer to call it regularly + self.get_topic_list() + self.create_timer(1.0, self.get_topic_list) # update topic list every 1 second + + # Timer to print data at given frequency + self.create_timer(1.0 / display_frequency, self.print_data) + + def get_topic_list(self): + # Get list of existing topics in the system + topic_list = self.get_topic_names_and_types() + + # Subscribe to topics with 'processing_time_ms' suffix + for topic, types in topic_list: + if topic.endswith("processing_time_ms") and topic not in self.subscribed_topics: + self.create_subscription( + Float64Stamped, topic, lambda msg, topic=topic: self.callback(msg, topic), 1 + ) + self.get_logger().info(f"Subscribed to {topic} | Type: {types}") + self.subscribed_topics.add(topic) + + def callback(self, msg, topic): + # Add the new data to the queue + if topic not in self.data_queue_map: + self.data_queue_map[topic] = deque(maxlen=self.window_size) + self.data_queue_map[topic].append(msg.data) + + # Calculate the average + avg_value = sum(self.data_queue_map[topic]) / len(self.data_queue_map[topic]) + self.data_map[topic] = avg_value + + def print_data(self): + # Clear terminal + os.system("cls" if os.name == "nt" else "clear") + + if not self.data_map: + print("No topics available.") + return + + # Get the maximum topic name length for alignment + max_topic_length = max(map(len, self.data_map.keys())) + + # Generate the header based on the max_display_time + header_str = f"topics (window size = {self.window_size})".ljust(max_topic_length) + ":" + for i in range(0, self.max_display_time + 1, 10): + header_str += f" {i}ms".ljust(self.bar_scale * 10) + + # Print the header + print(header_str) + print("-" * len(header_str)) + + # Print each topic's data + for topic, data in self.data_map.items(): + # Round the data to the third decimal place for display + data_rounded = round(data, 3) + # Calculate the number of bars to be displayed (clamped to max_display_time) + num_bars = ( + int(min(data * self.bar_scale, self.max_display_time * self.bar_scale - 1)) + 1 + ) # Convert bar_scale's reciprocal to milliseconds + print( + f"{topic.ljust(max_topic_length)}: {self.display_character * num_bars} [{data_rounded}ms]" + ) + + +def main(args=None): + # Get the command line arguments before argparse processes them + cmd_args = sys.argv[1:] + + parser = argparse.ArgumentParser(description="Processing Time Subscriber Parameters") + parser.add_argument( + "-m", "--max_display_time", type=int, default=50, help="Maximum display time in ms" + ) + parser.add_argument( + "-f", "--display_frequency", type=float, default=5.0, help="Display frequency in Hz" + ) + parser.add_argument( + "-w", "--window_size", type=int, default=5, help="Number of samples to average" + ) + parser.add_argument( + "-bs", + "--bar_scale", + type=int, + default=2, + help="Number of bars per second. Default is 2 bars per second.", + ) + parser.add_argument( + "-dc", + "--display_character", + type=str, + default="|", + help="Character to use for the display. Default is '|'.", + ) + args = parser.parse_args() + + rclpy.init(args=cmd_args) # Use the original command line arguments here + subscriber = ProcessingTimeSubscriber( + max_display_time=args.max_display_time, + display_frequency=args.display_frequency, + window_size=args.window_size, + bar_scale=args.bar_scale, + display_character=args.display_character, + ) + rclpy.spin(subscriber) + subscriber.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/planning/planning_debug_tools/scripts/trajectory_visualizer.py b/planning/planning_debug_tools/scripts/trajectory_visualizer.py index 3ce69aa265090..10bd41c5c9c93 100755 --- a/planning/planning_debug_tools/scripts/trajectory_visualizer.py +++ b/planning/planning_debug_tools/scripts/trajectory_visualizer.py @@ -17,8 +17,11 @@ import argparse from autoware_auto_planning_msgs.msg import Path +from autoware_auto_planning_msgs.msg import PathPoint +from autoware_auto_planning_msgs.msg import PathPointWithLaneId from autoware_auto_planning_msgs.msg import PathWithLaneId from autoware_auto_planning_msgs.msg import Trajectory +from autoware_auto_planning_msgs.msg import TrajectoryPoint from autoware_auto_vehicle_msgs.msg import VelocityReport from geometry_msgs.msg import Pose from matplotlib import animation @@ -40,6 +43,13 @@ help="Options VEL(default): show velocity only, VEL_ACC_JERK: show vel & acc & jerk", ) +parser.add_argument( + "-v", + "--max-velocity", + type=int, + help="maximum plotting velocity in Matplotlib", +) + args = parser.parse_args() PLOT_MIN_ARCLENGTH = -5 @@ -61,6 +71,11 @@ PLOT_TYPE = "VEL" print("plot type = " + PLOT_TYPE) +if args.max_velocity is None: + MAX_VELOCITY = 20 +else: + MAX_VELOCITY = args.max_velocity + class TrajectoryVisualizer(Node): def __init__(self): @@ -163,15 +178,15 @@ def __init__(self): # main process if PLOT_TYPE == "VEL_ACC_JERK": + self.setPlotTrajectory() self.ani = animation.FuncAnimation( self.fig, self.plotTrajectory, interval=100, blit=True ) - self.setPlotTrajectory() else: + self.setPlotTrajectoryVelocity() self.ani = animation.FuncAnimation( self.fig, self.plotTrajectoryVelocity, interval=100, blit=True ) - self.setPlotTrajectoryVelocity() plt.show() @@ -189,7 +204,7 @@ def CallbackVelocityLimit(self, cmd): self.velocity_limit = cmd.max_velocity def CallbackMotionVelOptTraj(self, cmd1, cmd2, cmd3, cmd4, cmd5): - print("CallbackMotionVelOptTraj called") + self.get_logger().info("CallbackMotionVelOptTraj called") self.CallBackTrajExVelLim(cmd1) self.CallBackTrajLatAccFiltered(cmd2) self.CallBackTrajRaw(cmd3) @@ -221,7 +236,7 @@ def CallBackTrajFinal(self, cmd): self.update_traj_final = True def CallBackLaneDrivingTraj(self, cmd5, cmd6, cmd7, cmd8, cmd9): - print("CallBackLaneDrivingTraj called") + self.get_logger().info("CallBackLaneDrivingTraj called") self.CallBackTrajFinal(cmd5) self.CallBackLBehaviorPathPlannerPath(cmd6) self.CallBackBehaviorVelocityPlannerPath(cmd7) @@ -274,6 +289,7 @@ def setPlotTrajectoryVelocity(self): self.ax1.set_title("trajectory's velocity") self.ax1.legend() self.ax1.set_xlim([PLOT_MIN_ARCLENGTH, PLOT_MAX_ARCLENGTH]) + self.ax1.set_ylim([0, MAX_VELOCITY]) self.ax1.set_ylabel("vel [m/s]") return ( @@ -293,9 +309,8 @@ def setPlotTrajectoryVelocity(self): ) def plotTrajectoryVelocity(self, data): - # self.updatePose(PATH_ORIGIN_FRAME, SELF_POSE_FRAME) if self.self_pose_received is False: - print("plot start but self pose is not received") + self.get_logger().info("plot start but self pose is not received") return ( self.im1, self.im2, @@ -311,82 +326,9 @@ def plotTrajectoryVelocity(self, data): self.im11, self.im12, ) - print("plot start") - - # copy - behavior_path_planner_path = self.behavior_path_planner_path - behavior_velocity_planner_path = self.behavior_velocity_planner_path - obstacle_avoid_traj = self.obstacle_avoid_traj - obstacle_stop_traj = self.obstacle_stop_traj - trajectory_raw = self.trajectory_raw - trajectory_external_velocity_limited = self.trajectory_external_velocity_limited - trajectory_lateral_acc_filtered = self.trajectory_lateral_acc_filtered - trajectory_steer_rate_filtered = self.trajectory_steer_rate_filtered - trajectory_time_resampled = self.trajectory_time_resampled - trajectory_final = self.trajectory_final - - if self.update_behavior_path_planner_path: - x = self.CalcArcLengthPathWLid(behavior_path_planner_path) - y = self.ToVelListPathWLid(behavior_path_planner_path) - self.im1.set_data(x, y) - self.update_behavior_path_planner_path = False - if len(y) != 0: - self.max_vel = max(10.0, np.max(y)) - - if self.update_behavior_velocity_planner_path: - x = self.CalcArcLengthPath(behavior_velocity_planner_path) - y = self.ToVelListPath(behavior_velocity_planner_path) - self.im2.set_data(x, y) - self.update_behavior_velocity_planner_path = False - - if self.update_traj_ob_avoid: - x = self.CalcArcLength(obstacle_avoid_traj) - y = self.ToVelList(obstacle_avoid_traj) - self.im3.set_data(x, y) - self.update_traj_ob_avoid = False - - if self.update_traj_ob_stop: - x = self.CalcArcLength(obstacle_stop_traj) - y = self.ToVelList(obstacle_stop_traj) - self.im4.set_data(x, y) - self.update_traj_ob_stop = False - - if self.update_traj_raw: - x = self.CalcArcLength(trajectory_raw) - y = self.ToVelList(trajectory_raw) - self.im5.set_data(x, y) - self.update_traj_raw = False - - if self.update_ex_vel_lim: - x = self.CalcArcLength(trajectory_external_velocity_limited) - y = self.ToVelList(trajectory_external_velocity_limited) - self.im6.set_data(x, y) - self.update_ex_vel_lim = False - - if self.update_lat_acc_fil: - x = self.CalcArcLength(trajectory_lateral_acc_filtered) - y = self.ToVelList(trajectory_lateral_acc_filtered) - self.im7.set_data(x, y) - self.update_lat_acc_fil = False - - if self.update_steer_rate_fil: - x = self.CalcArcLength(trajectory_steer_rate_filtered) - y = self.ToVelList(trajectory_steer_rate_filtered) - self.im71.set_data(x, y) - self.update_steer_rate_fil = False - - if self.update_traj_resample: - x = self.CalcArcLength(trajectory_time_resampled) - y = self.ToVelList(trajectory_time_resampled) - self.im8.set_data(x, y) - self.update_traj_resample = False + self.get_logger().info("plot start") if self.update_traj_final: - x = self.CalcArcLength(trajectory_final) - y = self.ToVelList(trajectory_final) - self.im9.set_data(x, y) - self.update_traj_final = False - self.im10.set_data(0, self.localization_vx) self.im11.set_data(0, self.vehicle_vx) @@ -395,11 +337,33 @@ def plotTrajectoryVelocity(self, data): y = [self.velocity_limit, self.velocity_limit] self.im12.set_data(x, y) - if len(y) != 0: - self.min_vel = np.min(y) + if len(y) != 0: + self.min_vel = np.min(y) - # change y-range - self.ax1.set_ylim([self.min_vel - 1.0, self.max_vel + 1.0]) + trajectory_data = [ + (self.behavior_path_planner_path, self.update_behavior_path_planner_path, self.im1), + ( + self.behavior_velocity_planner_path, + self.update_behavior_velocity_planner_path, + self.im2, + ), + (self.obstacle_avoid_traj, self.update_traj_ob_avoid, self.im3), + (self.obstacle_stop_traj, self.update_traj_ob_stop, self.im4), + (self.trajectory_raw, self.update_traj_raw, self.im5), + (self.trajectory_external_velocity_limited, self.update_ex_vel_lim, self.im6), + (self.trajectory_lateral_acc_filtered, self.update_lat_acc_fil, self.im7), + (self.trajectory_steer_rate_filtered, self.update_steer_rate_fil, self.im71), + (self.trajectory_time_resampled, self.update_traj_resample, self.im8), + (self.trajectory_final, self.update_traj_final, self.im9), + ] + + # update all trajectory plots + for traj, update_flag, image in trajectory_data: + if update_flag: + x = self.CalcArcLength(traj) + y = self.ToVelList(traj) + image.set_data(x, y) + update_flag = False return ( self.im1, @@ -417,107 +381,43 @@ def plotTrajectoryVelocity(self, data): self.im12, ) - def CalcArcLength(self, traj): - if len(traj.points) == 0: - return - - s_arr = [] - ds = 0.0 - s_sum = 0.0 - - closest_id = self.calcClosestTrajectory(traj) - for i in range(1, closest_id): - p0 = traj.points[i - 1] - p1 = traj.points[i] - dx = p1.pose.position.x - p0.pose.position.x - dy = p1.pose.position.y - p0.pose.position.y - ds = np.sqrt(dx**2 + dy**2) - s_sum -= ds - - s_arr.append(s_sum) - for i in range(1, len(traj.points)): - p0 = traj.points[i - 1] - p1 = traj.points[i] - dx = p1.pose.position.x - p0.pose.position.x - dy = p1.pose.position.y - p0.pose.position.y - ds = np.sqrt(dx**2 + dy**2) - s_sum += ds - s_arr.append(s_sum) - return s_arr - - def CalcArcLengthPathWLid(self, traj): - if len(traj.points) == 0: - return - - s_arr = [] - ds = 0.0 - s_sum = 0.0 - - closest_id = self.calcClosestPathWLid(traj) - for i in range(1, closest_id): - p0 = traj.points[i - 1].point - p1 = traj.points[i].point - dx = p1.pose.position.x - p0.pose.position.x - dy = p1.pose.position.y - p0.pose.position.y - ds = np.sqrt(dx**2 + dy**2) - s_sum -= ds + def getPoint(self, path_point): + if isinstance(path_point, (TrajectoryPoint, PathPoint)): + return path_point + elif isinstance(path_point, PathPointWithLaneId): + return path_point.point + else: + raise TypeError("invalid path_point argument type") - s_arr.append(s_sum) - for i in range(1, len(traj.points)): - p0 = traj.points[i - 1].point - p1 = traj.points[i].point - dx = p1.pose.position.x - p0.pose.position.x - dy = p1.pose.position.y - p0.pose.position.y - ds = np.sqrt(dx**2 + dy**2) - s_sum += ds - s_arr.append(s_sum) - return s_arr + def CalcDistance(self, point0, point1): + p0 = self.getPoint(point0).pose.position + p1 = self.getPoint(point1).pose.position + dx = p1.x - p0.x + dy = p1.y - p0.y + return np.sqrt(dx**2 + dy**2) - def CalcArcLengthPath(self, traj): + def CalcArcLength(self, traj): if len(traj.points) == 0: return s_arr = [] - ds = 0.0 s_sum = 0.0 - closest_id = self.calcClosestPath(traj) + closest_id = self.calcClosestIndex(traj) for i in range(1, closest_id): - p0 = traj.points[i - 1] - p1 = traj.points[i] - dx = p1.pose.position.x - p0.pose.position.x - dy = p1.pose.position.y - p0.pose.position.y - ds = np.sqrt(dx**2 + dy**2) - s_sum -= ds - + s_sum -= self.CalcDistance( + self.getPoint(traj.points[i - 1]), self.getPoint(traj.points[i]) + ) s_arr.append(s_sum) for i in range(1, len(traj.points)): - p0 = traj.points[i - 1] - p1 = traj.points[i] - dx = p1.pose.position.x - p0.pose.position.x - dy = p1.pose.position.y - p0.pose.position.y - ds = np.sqrt(dx**2 + dy**2) - s_sum += ds + s_sum += self.CalcDistance( + self.getPoint(traj.points[i - 1]), self.getPoint(traj.points[i]) + ) s_arr.append(s_sum) return s_arr def ToVelList(self, traj): - v_list = [] - for p in traj.points: - v_list.append(p.longitudinal_velocity_mps) - return v_list - - def ToVelListPathWLid(self, traj): - v_list = [] - for p in traj.points: - v_list.append(p.point.longitudinal_velocity_mps) - return v_list - - def ToVelListPath(self, traj): - v_list = [] - for p in traj.points: - v_list.append(p.longitudinal_velocity_mps) - return v_list + return [self.getPoint(p).longitudinal_velocity_mps for p in traj.points] def CalcAcceleration(self, traj): a_arr = [] @@ -577,6 +477,7 @@ def setPlotTrajectory(self): self.ax1.set_title("trajectory's velocity") self.ax1.legend() self.ax1.set_xlim([PLOT_MIN_ARCLENGTH, PLOT_MAX_ARCLENGTH]) + self.ax1.set_ylim([0, MAX_VELOCITY]) self.ax1.set_ylabel("vel [m/s]") self.ax2 = plt.subplot(3, 1, 2) @@ -595,8 +496,7 @@ def setPlotTrajectory(self): return self.im0, self.im1, self.im2, self.im3, self.im4 def plotTrajectory(self, data): - print("plot called") - # self.updatePose(PATH_ORIGIN_FRAME, SELF_POSE_FRAME) + self.get_logger().info("plot called") # copy trajectory_raw = self.trajectory_raw @@ -652,40 +552,16 @@ def plotTrajectory(self, data): return self.im0, self.im1, self.im2, self.im3, self.im4 - def calcClosestPath(self, path): - closest = -1 - min_dist_squared = 1.0e10 - for i in range(0, len(path.points)): - dist_sq = self.calcSquaredDist2d(self.self_pose, path.points[i].pose) - if dist_sq < min_dist_squared: - min_dist_squared = dist_sq - closest = i - return closest - - def calcClosestPathWLid(self, path): - closest = -1 - min_dist_squared = 1.0e10 - for i in range(0, len(path.points)): - dist_sq = self.calcSquaredDist2d(self.self_pose, path.points[i].point.pose) - if dist_sq < min_dist_squared: - min_dist_squared = dist_sq - closest = i - return closest - - def calcClosestTrajectory(self, path): - closest = -1 - min_dist_squared = 1.0e10 - for i in range(0, len(path.points)): - dist_sq = self.calcSquaredDist2d(self.self_pose, path.points[i].pose) - if dist_sq < min_dist_squared: - min_dist_squared = dist_sq - closest = i - return closest - - def calcSquaredDist2d(self, p1, p2): - dx = p1.position.x - p2.position.x - dy = p1.position.y - p2.position.y - return dx * dx + dy * dy + def calcClosestIndex(self, path): + points = np.array( + [ + [self.getPoint(p).pose.position.x, self.getPoint(p).pose.position.y] + for p in path.points + ] + ) + self_pose = np.array([self.self_pose.position.x, self.self_pose.position.y]) + dists_squared = np.sum((points - self_pose) ** 2, axis=1) + return np.argmin(dists_squared) def closeFigure(self): plt.close(self.fig) diff --git a/planning/planning_test_utils/include/planning_interface_test_manager/planning_interface_test_manager.hpp b/planning/planning_test_utils/include/planning_interface_test_manager/planning_interface_test_manager.hpp index ddc304c99ea6a..c5a30be126661 100644 --- a/planning/planning_test_utils/include/planning_interface_test_manager/planning_interface_test_manager.hpp +++ b/planning/planning_test_utils/include/planning_interface_test_manager/planning_interface_test_manager.hpp @@ -38,11 +38,11 @@ #include #include #include -#include #include #include #include #include +#include #include #include #include @@ -77,10 +77,10 @@ namespace planning_test_utils using autoware_adapi_v1_msgs::msg::OperationModeState; using autoware_auto_mapping_msgs::msg::HADMapBin; using autoware_auto_perception_msgs::msg::PredictedObjects; -using autoware_auto_perception_msgs::msg::TrafficSignalArray; using autoware_auto_planning_msgs::msg::Path; using autoware_auto_planning_msgs::msg::PathWithLaneId; using autoware_auto_planning_msgs::msg::Trajectory; +using autoware_perception_msgs::msg::TrafficSignalArray; using autoware_planning_msgs::msg::LaneletRoute; using geometry_msgs::msg::AccelWithCovarianceStamped; using geometry_msgs::msg::Point; diff --git a/planning/planning_test_utils/package.xml b/planning/planning_test_utils/package.xml index 72662f7ee7b76..23bfb2f44cb96 100644 --- a/planning/planning_test_utils/package.xml +++ b/planning/planning_test_utils/package.xml @@ -15,9 +15,9 @@ autoware_auto_control_msgs autoware_auto_mapping_msgs - autoware_auto_perception_msgs autoware_auto_planning_msgs autoware_auto_vehicle_msgs + autoware_perception_msgs autoware_planning_msgs component_interface_specs component_interface_utils diff --git a/planning/planning_validator/include/planning_validator/planning_validator.hpp b/planning/planning_validator/include/planning_validator/planning_validator.hpp index 321eb23aa7fd4..ef570b57773bb 100644 --- a/planning/planning_validator/include/planning_validator/planning_validator.hpp +++ b/planning/planning_validator/include/planning_validator/planning_validator.hpp @@ -17,6 +17,8 @@ #include "planning_validator/debug_marker.hpp" #include "planning_validator/msg/planning_validator_status.hpp" +#include "tier4_autoware_utils/ros/logger_level_configure.hpp" +#include "tier4_autoware_utils/system/stop_watch.hpp" #include "vehicle_info_util/vehicle_info_util.hpp" #include @@ -25,6 +27,7 @@ #include #include #include +#include #include #include @@ -37,6 +40,8 @@ using diagnostic_updater::DiagnosticStatusWrapper; using diagnostic_updater::Updater; using nav_msgs::msg::Odometry; using planning_validator::msg::PlanningValidatorStatus; +using tier4_autoware_utils::StopWatch; +using tier4_debug_msgs::msg::Float64Stamped; struct ValidationParams { @@ -80,6 +85,7 @@ class PlanningValidator : public rclcpp::Node void validate(const Trajectory & trajectory); + void publishProcessingTime(const double processing_time_ms); void publishTrajectory(); void publishDebugInfo(); void displayStatus(); @@ -90,6 +96,7 @@ class PlanningValidator : public rclcpp::Node rclcpp::Subscription::SharedPtr sub_traj_; rclcpp::Publisher::SharedPtr pub_traj_; rclcpp::Publisher::SharedPtr pub_status_; + rclcpp::Publisher::SharedPtr pub_processing_time_ms_; rclcpp::Publisher::SharedPtr pub_markers_; // system parameters @@ -117,6 +124,10 @@ class PlanningValidator : public rclcpp::Node Odometry::ConstSharedPtr current_kinematics_; std::shared_ptr debug_pose_publisher_; + + std::unique_ptr logger_configure_; + + StopWatch stop_watch_; }; } // namespace planning_validator diff --git a/planning/planning_validator/include/planning_validator/utils.hpp b/planning/planning_validator/include/planning_validator/utils.hpp index 6c0d2e86943dd..5831993d748b6 100644 --- a/planning/planning_validator/include/planning_validator/utils.hpp +++ b/planning/planning_validator/include/planning_validator/utils.hpp @@ -36,7 +36,9 @@ std::pair getAbsMaxValAndIdx(const std::vector & v); Trajectory resampleTrajectory(const Trajectory & trajectory, const double min_interval); -void calcCurvature(const Trajectory & trajectory, std::vector & curvatures); +void calcCurvature( + const Trajectory & trajectory, std::vector & curvatures, + const double curvature_distance = 1.0); void calcSteeringAngles( const Trajectory & trajectory, const double wheelbase, std::vector & steering_array); diff --git a/planning/planning_validator/package.xml b/planning/planning_validator/package.xml index 685c2823796cc..1f1c1cc55e93c 100644 --- a/planning/planning_validator/package.xml +++ b/planning/planning_validator/package.xml @@ -5,7 +5,7 @@ 0.1.0 ros node for planning_validator Takamasa Horibe - Yutaka Shimizu + Kosuke Takeuchi Apache License 2.0 Takamasa Horibe diff --git a/planning/planning_validator/src/debug_marker.cpp b/planning/planning_validator/src/debug_marker.cpp index 89614f5c74f5c..1a0793957402a 100644 --- a/planning/planning_validator/src/debug_marker.cpp +++ b/planning/planning_validator/src/debug_marker.cpp @@ -14,8 +14,8 @@ #include "planning_validator/debug_marker.hpp" -#include -#include +#include +#include #include #include diff --git a/planning/planning_validator/src/planning_validator.cpp b/planning/planning_validator/src/planning_validator.cpp index 7bb37fdcdd467..875781d47b25d 100644 --- a/planning/planning_validator/src/planning_validator.cpp +++ b/planning/planning_validator/src/planning_validator.cpp @@ -17,7 +17,7 @@ #include "planning_validator/utils.hpp" #include -#include +#include #include #include @@ -41,6 +41,7 @@ PlanningValidator::PlanningValidator(const rclcpp::NodeOptions & options) pub_traj_ = create_publisher("~/output/trajectory", 1); pub_status_ = create_publisher("~/output/validation_status", 1); pub_markers_ = create_publisher("~/output/markers", 1); + pub_processing_time_ms_ = create_publisher("~/debug/processing_time_ms", 1); debug_pose_publisher_ = std::make_shared(this); @@ -49,6 +50,8 @@ PlanningValidator::PlanningValidator(const rclcpp::NodeOptions & options) if (publish_diag_) { setupDiag(); } + + logger_configure_ = std::make_unique(this); } void PlanningValidator::setupParameters() @@ -165,6 +168,8 @@ bool PlanningValidator::isDataReady() void PlanningValidator::onTrajectory(const Trajectory::ConstSharedPtr msg) { + stop_watch_.tic(__func__); + current_trajectory_ = msg; if (!isDataReady()) return; @@ -178,6 +183,7 @@ void PlanningValidator::onTrajectory(const Trajectory::ConstSharedPtr msg) publishTrajectory(); // for debug + publishProcessingTime(stop_watch_.toc(__func__)); publishDebugInfo(); displayStatus(); } @@ -220,6 +226,14 @@ void PlanningValidator::publishTrajectory() return; } +void PlanningValidator::publishProcessingTime(const double processing_time_ms) +{ + Float64Stamped msg{}; + msg.stamp = this->now(); + msg.data = processing_time_ms; + pub_processing_time_ms_->publish(msg); +} + void PlanningValidator::publishDebugInfo() { validation_status_.stamp = get_clock()->now(); diff --git a/planning/planning_validator/src/utils.cpp b/planning/planning_validator/src/utils.cpp index e4c882cfaedb9..3155a6f2e0bc8 100644 --- a/planning/planning_validator/src/utils.cpp +++ b/planning/planning_validator/src/utils.cpp @@ -15,7 +15,7 @@ #include "planning_validator/utils.hpp" #include -#include +#include #include #include @@ -89,26 +89,68 @@ Trajectory resampleTrajectory(const Trajectory & trajectory, const double min_in return resampled; } -void calcCurvature(const Trajectory & trajectory, std::vector & curvature_arr) +// calculate curvature from three points with curvature_distance +void calcCurvature( + const Trajectory & trajectory, std::vector & curvature_arr, + const double curvature_distance) { if (trajectory.points.size() < 3) { curvature_arr = std::vector(trajectory.points.size(), 0.0); return; } - curvature_arr.push_back(0.0); + // calc arc length array: arc_length(3) - arc_length(0) is distance from point(3) to point(0) + std::vector arc_length(trajectory.points.size(), 0.0); + for (size_t i = 1; i < trajectory.points.size(); ++i) { + arc_length.at(i) = + arc_length.at(i - 1) + calcDistance2d(trajectory.points.at(i - 1), trajectory.points.at(i)); + } + + // initialize with 0 curvature + curvature_arr = std::vector(trajectory.points.size(), 0.0); + + size_t first_distant_index = 0; + size_t last_distant_index = trajectory.points.size() - 1; for (size_t i = 1; i < trajectory.points.size() - 1; ++i) { - const auto p1 = getPoint(trajectory.points.at(i - 1)); + // find the previous point + size_t prev_idx = 0; + for (size_t j = i - 1; j > 0; --j) { + if (arc_length.at(i) - arc_length.at(j) > curvature_distance) { + if (first_distant_index == 0) { + first_distant_index = i; // save first index that meets distance requirement + } + prev_idx = j; + break; + } + } + + // find the next point + size_t next_idx = trajectory.points.size() - 1; + for (size_t j = i + 1; j < trajectory.points.size(); ++j) { + if (arc_length.at(j) - arc_length.at(i) > curvature_distance) { + last_distant_index = i; // save last index that meets distance requirement + next_idx = j; + break; + } + } + + const auto p1 = getPoint(trajectory.points.at(prev_idx)); const auto p2 = getPoint(trajectory.points.at(i)); - const auto p3 = getPoint(trajectory.points.at(i + 1)); + const auto p3 = getPoint(trajectory.points.at(next_idx)); try { - curvature_arr.push_back(tier4_autoware_utils::calcCurvature(p1, p2, p3)); + curvature_arr.at(i) = tier4_autoware_utils::calcCurvature(p1, p2, p3); } catch (...) { - // maybe distance is too close - curvature_arr.push_back(0.0); + curvature_arr.at(i) = 0.0; // maybe distance is too close } } - curvature_arr.push_back(0.0); + + // use previous or last curvature where the distance is not enough + for (size_t i = first_distant_index; i > 0; --i) { + curvature_arr.at(i - 1) = curvature_arr.at(i); + } + for (size_t i = last_distant_index; i < curvature_arr.size() - 1; ++i) { + curvature_arr.at(i + 1) = curvature_arr.at(i); + } } std::pair calcMaxCurvature(const Trajectory & trajectory) @@ -117,22 +159,13 @@ std::pair calcMaxCurvature(const Trajectory & trajectory) return {0.0, 0}; } - double max_curvature = 0.0; - size_t max_index = 0; - for (size_t i = 1; i < trajectory.points.size() - 1; ++i) { - const auto p1 = getPoint(trajectory.points.at(i - 1)); - const auto p2 = getPoint(trajectory.points.at(i)); - const auto p3 = getPoint(trajectory.points.at(i + 1)); + std::vector curvature_arr; + calcCurvature(trajectory, curvature_arr); - try { - const auto k = tier4_autoware_utils::calcCurvature(p1, p2, p3); - takeBigger(max_curvature, max_index, std::abs(k), i); - } catch (...) { - // maybe distance is too close - } - } + const auto max_curvature_it = std::max_element(curvature_arr.begin(), curvature_arr.end()); + const size_t index = std::distance(curvature_arr.begin(), max_curvature_it); - return {max_curvature, max_index}; + return {*max_curvature_it, index}; } std::pair calcMaxIntervalDistance(const Trajectory & trajectory) diff --git a/planning/route_handler/include/route_handler/route_handler.hpp b/planning/route_handler/include/route_handler/route_handler.hpp index 1818c460e407b..b12ca7f648a7d 100644 --- a/planning/route_handler/include/route_handler/route_handler.hpp +++ b/planning/route_handler/include/route_handler/route_handler.hpp @@ -15,26 +15,19 @@ #ifndef ROUTE_HANDLER__ROUTE_HANDLER_HPP_ #define ROUTE_HANDLER__ROUTE_HANDLER_HPP_ -#include -#include -#include -#include +#include #include -#include -#include #include -#include #include #include #include #include -#include -#include -#include -#include -#include +#include +#include +#include +#include #include #include @@ -43,8 +36,6 @@ namespace route_handler { using autoware_auto_mapping_msgs::msg::HADMapBin; -using autoware_auto_planning_msgs::msg::Path; -using autoware_auto_planning_msgs::msg::PathPointWithLaneId; using autoware_auto_planning_msgs::msg::PathWithLaneId; using autoware_planning_msgs::msg::LaneletRoute; using autoware_planning_msgs::msg::LaneletSegment; @@ -98,6 +89,7 @@ class RouteHandler Pose getGoalPose() const; Pose getStartPose() const; Pose getOriginalStartPose() const; + Pose getOriginalGoalPose() const; lanelet::Id getGoalLaneId() const; bool getGoalLanelet(lanelet::ConstLanelet * goal_lanelet) const; std::vector getLanesBeforePose( @@ -121,7 +113,8 @@ class RouteHandler * @return vector of lanelet having same direction if true */ boost::optional getRightLanelet( - const lanelet::ConstLanelet & lanelet, const bool enable_same_root = false) const; + const lanelet::ConstLanelet & lanelet, const bool enable_same_root = false, + const bool get_shoulder_lane = false) const; /** * @brief Check if same-direction lane is available at the left side of the lanelet @@ -131,7 +124,8 @@ class RouteHandler * @return vector of lanelet having same direction if true */ boost::optional getLeftLanelet( - const lanelet::ConstLanelet & lanelet, const bool enable_same_root = false) const; + const lanelet::ConstLanelet & lanelet, const bool enable_same_root = false, + const bool get_shoulder_lane = false) const; lanelet::ConstLanelets getNextLanelets(const lanelet::ConstLanelet & lanelet) const; lanelet::ConstLanelets getPreviousLanelets(const lanelet::ConstLanelet & lanelet) const; @@ -194,7 +188,8 @@ class RouteHandler * @return vector of lanelet having same direction if true */ lanelet::ConstLanelet getMostRightLanelet( - const lanelet::ConstLanelet & lanelet, const bool enable_same_root = false) const; + const lanelet::ConstLanelet & lanelet, const bool enable_same_root = false, + const bool get_shoulder_lane = false) const; /** * @brief Check if same-direction lane is available at the left side of the lanelet @@ -204,7 +199,8 @@ class RouteHandler * @return vector of lanelet having same direction if true */ lanelet::ConstLanelet getMostLeftLanelet( - const lanelet::ConstLanelet & lanelet, const bool enable_same_root = false) const; + const lanelet::ConstLanelet & lanelet, const bool enable_same_root = false, + const bool get_shoulder_lane = false) const; /** * @brief Searches the furthest linestring to the right side of the lanelet @@ -378,6 +374,7 @@ class RouteHandler // save original(not modified) route start pose for start planer execution Pose original_start_pose_; + Pose original_goal_pose_; // non-const methods void setLaneletsFromRouteMsg(); diff --git a/planning/route_handler/package.xml b/planning/route_handler/package.xml index ef2051d1b244a..29f0411a5aa80 100644 --- a/planning/route_handler/package.xml +++ b/planning/route_handler/package.xml @@ -9,6 +9,7 @@ Yutaka Shimizu Kosuke Takeuchi Takayuki Murooka + Mamoru Sobue Apache License 2.0 Fumiya Watanabe @@ -24,7 +25,6 @@ autoware_planning_msgs geometry_msgs lanelet2_extension - motion_utils rclcpp rclcpp_components tf2_ros diff --git a/planning/route_handler/src/route_handler.cpp b/planning/route_handler/src/route_handler.cpp index b8cedaa947455..d74a8712a6e3f 100644 --- a/planning/route_handler/src/route_handler.cpp +++ b/planning/route_handler/src/route_handler.cpp @@ -14,15 +14,23 @@ #include "route_handler/route_handler.hpp" +#include #include #include #include #include #include +#include + +#include +#include +#include -#include #include #include +#include +#include +#include #include #include @@ -35,6 +43,7 @@ namespace { using autoware_auto_planning_msgs::msg::Path; +using autoware_auto_planning_msgs::msg::PathPointWithLaneId; using autoware_auto_planning_msgs::msg::PathWithLaneId; using autoware_planning_msgs::msg::LaneletPrimitive; using geometry_msgs::msg::Pose; @@ -176,6 +185,7 @@ void RouteHandler::setRoute(const LaneletRoute & route_msg) // if get not modified route but new route, reset original start pose if (!route_ptr_ || route_ptr_->uuid != route_msg.uuid) { original_start_pose_ = route_msg.start_pose; + original_goal_pose_ = route_msg.goal_pose; } route_ptr_ = std::make_shared(route_msg); is_handler_ready_ = false; @@ -426,13 +436,17 @@ Pose RouteHandler::getStartPose() const { if (!route_ptr_) { RCLCPP_WARN(logger_, "[Route Handler] getStartPose: Route has not been set yet"); - Pose(); + return Pose(); } return route_ptr_->start_pose; } Pose RouteHandler::getOriginalStartPose() const { + if (!route_ptr_) { + RCLCPP_WARN(logger_, "[Route Handler] getOriginalStartPose: Route has not been set yet"); + return Pose(); + } return original_start_pose_; } @@ -440,11 +454,20 @@ Pose RouteHandler::getGoalPose() const { if (!route_ptr_) { RCLCPP_WARN(logger_, "[Route Handler] getGoalPose: Route has not been set yet"); - Pose(); + return Pose(); } return route_ptr_->goal_pose; } +Pose RouteHandler::getOriginalGoalPose() const +{ + if (!route_ptr_) { + RCLCPP_WARN(logger_, "[Route Handler] getOriginalGoalPose: Route has not been set yet"); + return Pose(); + } + return original_goal_pose_; +} + lanelet::Id RouteHandler::getGoalLaneId() const { if (!route_ptr_ || route_ptr_->segments.empty()) { @@ -514,7 +537,14 @@ lanelet::ConstLanelets RouteHandler::getLaneletSequenceAfter( while (rclcpp::ok() && length < min_length) { lanelet::ConstLanelet next_lanelet; if (!getNextLaneletWithinRoute(current_lanelet, &next_lanelet)) { - break; + if (only_route_lanes) { + break; + } + const auto next_lanes = getNextLanelets(current_lanelet); + if (next_lanes.empty()) { + break; + } + next_lanelet = next_lanes.front(); } // loop check if (lanelet.id() == next_lanelet.id()) { @@ -542,7 +572,14 @@ lanelet::ConstLanelets RouteHandler::getLaneletSequenceUpTo( while (rclcpp::ok() && length < min_length) { lanelet::ConstLanelets candidate_lanelets; if (!getPreviousLaneletsWithinRoute(current_lanelet, &candidate_lanelets)) { - break; + if (only_route_lanes) { + break; + } + const auto prev_lanes = getPreviousLanelets(current_lanelet); + if (prev_lanes.empty()) { + break; + } + candidate_lanelets = prev_lanes; } // loop check if (std::any_of( @@ -819,12 +856,11 @@ bool RouteHandler::getNextLaneletWithinRoute( return false; } - lanelet::ConstLanelet start_lanelet; - const bool flag_check = getClosestLaneletWithinRoute(route_ptr_->start_pose, &start_lanelet); + const auto start_lane_id = route_ptr_->segments.front().preferred_primitive.id; const auto following_lanelets = routing_graph_ptr_->following(lanelet); for (const auto & llt : following_lanelets) { - if (!(flag_check && start_lanelet.id() == llt.id()) && exists(route_lanelets_, llt)) { + if (start_lane_id != llt.id() && exists(route_lanelets_, llt)) { *next_lanelet = llt; return true; } @@ -940,7 +976,8 @@ bool RouteHandler::isBijectiveConnection( } boost::optional RouteHandler::getRightLanelet( - const lanelet::ConstLanelet & lanelet, const bool enable_same_root) const + const lanelet::ConstLanelet & lanelet, const bool enable_same_root, + const bool get_shoulder_lane) const { // right road lanelet of shoulder lanelet if (isShoulderLanelet(lanelet)) { @@ -952,6 +989,14 @@ boost::optional RouteHandler::getRightLanelet( return boost::none; } + // right shoulder lanelet + if (get_shoulder_lane) { + lanelet::ConstLanelet right_shoulder_lanelet; + if (getRightShoulderLanelet(lanelet, &right_shoulder_lanelet)) { + return right_shoulder_lanelet; + } + } + // routable lane const auto & right_lane = routing_graph_ptr_->right(lanelet); if (right_lane) { @@ -1012,7 +1057,8 @@ bool RouteHandler::getLeftLaneletWithinRoute( } boost::optional RouteHandler::getLeftLanelet( - const lanelet::ConstLanelet & lanelet, const bool enable_same_root) const + const lanelet::ConstLanelet & lanelet, const bool enable_same_root, + const bool get_shoulder_lane) const { // left road lanelet of shoulder lanelet if (isShoulderLanelet(lanelet)) { @@ -1024,6 +1070,14 @@ boost::optional RouteHandler::getLeftLanelet( return boost::none; } + // left shoulder lanelet + if (get_shoulder_lane) { + lanelet::ConstLanelet left_shoulder_lanelet; + if (getLeftShoulderLanelet(lanelet, &left_shoulder_lanelet)) { + return left_shoulder_lanelet; + } + } + // routable lane const auto & left_lane = routing_graph_ptr_->left(lanelet); if (left_lane) { @@ -1199,26 +1253,28 @@ lanelet::Lanelets RouteHandler::getLeftOppositeLanelets(const lanelet::ConstLane } lanelet::ConstLanelet RouteHandler::getMostRightLanelet( - const lanelet::ConstLanelet & lanelet, const bool enable_same_root) const + const lanelet::ConstLanelet & lanelet, const bool enable_same_root, + const bool get_shoulder_lane) const { // recursively compute the width of the lanes - const auto & same = getRightLanelet(lanelet, enable_same_root); + const auto & same = getRightLanelet(lanelet, enable_same_root, get_shoulder_lane); if (same) { - return getMostRightLanelet(same.get(), enable_same_root); + return getMostRightLanelet(same.get(), enable_same_root, get_shoulder_lane); } return lanelet; } lanelet::ConstLanelet RouteHandler::getMostLeftLanelet( - const lanelet::ConstLanelet & lanelet, const bool enable_same_root) const + const lanelet::ConstLanelet & lanelet, const bool enable_same_root, + const bool get_shoulder_lane) const { // recursively compute the width of the lanes - const auto & same = getLeftLanelet(lanelet, enable_same_root); + const auto & same = getLeftLanelet(lanelet, enable_same_root, get_shoulder_lane); if (same) { - return getMostLeftLanelet(same.get(), enable_same_root); + return getMostLeftLanelet(same.get(), enable_same_root, get_shoulder_lane); } return lanelet; @@ -2072,10 +2128,24 @@ bool RouteHandler::planPathLaneletsBetweenCheckpoints( double shortest_path_length2d = std::numeric_limits::max(); for (const auto & st_llt : start_lanelets) { + // check if the angle difference between start_checkpoint and start lanelet center line + // orientation is in yaw_threshold range + double yaw_threshold = M_PI / 2.0; + bool is_proper_angle = false; + { + double lanelet_angle = lanelet::utils::getLaneletAngle(st_llt, start_checkpoint.position); + double pose_yaw = tf2::getYaw(start_checkpoint.orientation); + double angle_diff = std::abs(autoware_utils::normalize_radian(lanelet_angle - pose_yaw)); + + if (angle_diff <= std::abs(yaw_threshold)) { + is_proper_angle = true; + } + } + optional_route = routing_graph_ptr_->getRoute(st_llt, goal_lanelet, 0); - if (!optional_route) { + if (!optional_route || !is_proper_angle) { RCLCPP_ERROR_STREAM( - logger_, "Failed to find a proper path!" + logger_, "Failed to find a proper route!" << std::endl << " - start checkpoint: " << toString(start_checkpoint) << std::endl << " - goal checkpoint: " << toString(goal_checkpoint) << std::endl diff --git a/planning/rtc_auto_mode_manager/CMakeLists.txt b/planning/rtc_auto_mode_manager/CMakeLists.txt deleted file mode 100644 index 52c0c68384a57..0000000000000 --- a/planning/rtc_auto_mode_manager/CMakeLists.txt +++ /dev/null @@ -1,29 +0,0 @@ -cmake_minimum_required(VERSION 3.5) -project(rtc_auto_mode_manager) - -### Compile options -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) -endif() -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic -Werror) -endif() - -find_package(autoware_cmake REQUIRED) -autoware_package() - -ament_auto_add_library(${PROJECT_NAME} SHARED - src/rtc_auto_mode_manager_interface.cpp - src/node.cpp -) - -rclcpp_components_register_node(${PROJECT_NAME} - PLUGIN "rtc_auto_mode_manager::RTCAutoModeManagerNode" - EXECUTABLE ${PROJECT_NAME}_node -) - -ament_auto_package( - INSTALL_TO_SHARE - config - launch -) diff --git a/planning/rtc_auto_mode_manager/README.md b/planning/rtc_auto_mode_manager/README.md deleted file mode 100644 index 0e2614baaf1c8..0000000000000 --- a/planning/rtc_auto_mode_manager/README.md +++ /dev/null @@ -1,47 +0,0 @@ -# RTC Auto Mode Manager - -## Purpose - -RTC Auto Mode Manager is a node to approve request to cooperate from behavior planning modules automatically. - -## Inputs / Outputs - -### Input - -| Name | Type | Description | -| ------------------------------- | --------------------------- | ------------------------------------------ | -| `/planning/enable_auto_mode/**` | tier4_rtc_msgs/srv/AutoMode | Service to enable auto mode for the module | - -### Output - -| Name | Type | Description | -| ---------------------------------------- | --------------------------- | ------------------------------------------ | -| `/planning/enable_auto_mode/internal/**` | tier4_rtc_msgs/srv/AutoMode | Service to enable auto mode for the module | - -## Parameters - -| Name | Type | Description | -| :-------------------- | :--------------- | :----------------------------------------------- | -| `module_list` | List of `string` | Module names managing in `rtc_auto_mode_manager` | -| `default_enable_list` | List of `string` | Module names enabled auto mode at initialization | - -## Inner-workings / Algorithms - -```plantuml - -start -:Read parameters; -:Send enable auto mode service to the module listed in `default_enable_list`; -repeat - if (Enable auto mode command received?) then (yes) - :Send enable auto mode command to rtc_interface; - else (no) - endif -repeat while (Is node running?) is (yes) not (no) -end - -``` - -## Assumptions / Known limits - -## Future extensions / Unimplemented parts diff --git a/planning/rtc_auto_mode_manager/config/rtc_auto_mode_manager.param.yaml b/planning/rtc_auto_mode_manager/config/rtc_auto_mode_manager.param.yaml deleted file mode 100644 index 0aa3cbd49e8e9..0000000000000 --- a/planning/rtc_auto_mode_manager/config/rtc_auto_mode_manager.param.yaml +++ /dev/null @@ -1,31 +0,0 @@ -/**: - ros__parameters: - module_list: - - "blind_spot" - - "crosswalk" - - "detection_area" - - "intersection" - - "no_stopping_area" - - "traffic_light" - - "lane_change_left" - - "lane_change_right" - - "avoidance_left" - - "avoidance_right" - - "goal_planner" - - "start_planner" - - "intersection_occlusion" - - default_enable_list: - - "blind_spot" - - "crosswalk" - - "detection_area" - - "intersection" - - "no_stopping_area" - - "traffic_light" - - "lane_change_left" - - "lane_change_right" - - "avoidance_left" - - "avoidance_right" - - "goal_planner" - - "start_planner" - - "intersection_occlusion" diff --git a/planning/rtc_auto_mode_manager/include/rtc_auto_mode_manager/node.hpp b/planning/rtc_auto_mode_manager/include/rtc_auto_mode_manager/node.hpp deleted file mode 100644 index d36974508d2df..0000000000000 --- a/planning/rtc_auto_mode_manager/include/rtc_auto_mode_manager/node.hpp +++ /dev/null @@ -1,44 +0,0 @@ -// 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 RTC_AUTO_MODE_MANAGER__NODE_HPP_ -#define RTC_AUTO_MODE_MANAGER__NODE_HPP_ - -#include "rclcpp/rclcpp.hpp" -#include "rtc_auto_mode_manager/rtc_auto_mode_manager_interface.hpp" - -#include "tier4_rtc_msgs/msg/auto_mode_status_array.hpp" - -#include -#include -#include - -namespace rtc_auto_mode_manager -{ -using tier4_rtc_msgs::msg::AutoModeStatusArray; -class RTCAutoModeManagerNode : public rclcpp::Node -{ -public: - explicit RTCAutoModeManagerNode(const rclcpp::NodeOptions & node_options); - -private: - rclcpp::TimerBase::SharedPtr timer_; - AutoModeStatusArray auto_mode_statuses_; - rclcpp::Publisher::SharedPtr statuses_pub_; - std::vector> managers_; -}; - -} // namespace rtc_auto_mode_manager - -#endif // RTC_AUTO_MODE_MANAGER__NODE_HPP_ diff --git a/planning/rtc_auto_mode_manager/include/rtc_auto_mode_manager/rtc_auto_mode_manager_interface.hpp b/planning/rtc_auto_mode_manager/include/rtc_auto_mode_manager/rtc_auto_mode_manager_interface.hpp deleted file mode 100644 index 098852c397161..0000000000000 --- a/planning/rtc_auto_mode_manager/include/rtc_auto_mode_manager/rtc_auto_mode_manager_interface.hpp +++ /dev/null @@ -1,53 +0,0 @@ -// 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 RTC_AUTO_MODE_MANAGER__RTC_AUTO_MODE_MANAGER_INTERFACE_HPP_ -#define RTC_AUTO_MODE_MANAGER__RTC_AUTO_MODE_MANAGER_INTERFACE_HPP_ - -#include "rclcpp/rclcpp.hpp" - -#include "tier4_rtc_msgs/msg/auto_mode_status.hpp" -#include "tier4_rtc_msgs/msg/module.hpp" -#include "tier4_rtc_msgs/srv/auto_mode.hpp" - -#include -#include -#include - -namespace rtc_auto_mode_manager -{ -using tier4_rtc_msgs::msg::AutoModeStatus; -using tier4_rtc_msgs::msg::Module; -using tier4_rtc_msgs::srv::AutoMode; - -class RTCAutoModeManagerInterface -{ -public: - RTCAutoModeManagerInterface( - rclcpp::Node * node, const std::string & module_name, const bool default_enable); - AutoModeStatus getAutoModeStatus() const { return auto_mode_status_; } - -private: - void onEnableService( - const AutoMode::Request::SharedPtr request, const AutoMode::Response::SharedPtr response); - AutoMode::Request createRequest(const AutoMode::Request::SharedPtr request) const; - AutoModeStatus auto_mode_status_; - rclcpp::Client::SharedPtr enable_cli_; - rclcpp::Service::SharedPtr enable_srv_; - - std::string enable_auto_mode_namespace_ = "/planning/enable_auto_mode"; -}; -} // namespace rtc_auto_mode_manager - -#endif // RTC_AUTO_MODE_MANAGER__RTC_AUTO_MODE_MANAGER_INTERFACE_HPP_ diff --git a/planning/rtc_auto_mode_manager/launch/rtc_auto_mode_manager.launch.xml b/planning/rtc_auto_mode_manager/launch/rtc_auto_mode_manager.launch.xml deleted file mode 100644 index 85d4b3446fb89..0000000000000 --- a/planning/rtc_auto_mode_manager/launch/rtc_auto_mode_manager.launch.xml +++ /dev/null @@ -1,7 +0,0 @@ - - - - - - - diff --git a/planning/rtc_auto_mode_manager/src/node.cpp b/planning/rtc_auto_mode_manager/src/node.cpp deleted file mode 100644 index 6092d2741d909..0000000000000 --- a/planning/rtc_auto_mode_manager/src/node.cpp +++ /dev/null @@ -1,51 +0,0 @@ -// 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 "rtc_auto_mode_manager/node.hpp" - -#include - -namespace rtc_auto_mode_manager -{ - -RTCAutoModeManagerNode::RTCAutoModeManagerNode(const rclcpp::NodeOptions & node_options) -: Node("rtc_auto_mode_manager_node", node_options) -{ - const std::vector module_list = - declare_parameter>("module_list"); - const std::vector default_enable_list = - declare_parameter>("default_enable_list"); - - for (const auto & module_name : module_list) { - const bool enabled = - std::count(default_enable_list.begin(), default_enable_list.end(), module_name) != 0; - managers_.push_back(std::make_shared(this, module_name, enabled)); - } - statuses_pub_ = create_publisher("output/auto_mode_statuses", 1); - using std::chrono_literals::operator""ms; - timer_ = rclcpp::create_timer(this, get_clock(), 100ms, [this] { - AutoModeStatusArray auto_mode_statuses; - for (const auto & m : managers_) { - auto_mode_statuses.stamp = get_clock()->now(); - auto_mode_statuses.statuses.emplace_back(m->getAutoModeStatus()); - } - statuses_pub_->publish(auto_mode_statuses); - auto_mode_statuses.statuses.clear(); - }); -} - -} // namespace rtc_auto_mode_manager - -#include -RCLCPP_COMPONENTS_REGISTER_NODE(rtc_auto_mode_manager::RTCAutoModeManagerNode) diff --git a/planning/rtc_auto_mode_manager/src/rtc_auto_mode_manager_interface.cpp b/planning/rtc_auto_mode_manager/src/rtc_auto_mode_manager_interface.cpp deleted file mode 100644 index e28957cee161b..0000000000000 --- a/planning/rtc_auto_mode_manager/src/rtc_auto_mode_manager_interface.cpp +++ /dev/null @@ -1,109 +0,0 @@ -// 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 "rtc_auto_mode_manager/rtc_auto_mode_manager_interface.hpp" - -namespace rtc_auto_mode_manager -{ -Module getModuleType(const std::string & module_name) -{ - Module module; - if (module_name == "blind_spot") { - module.type = Module::BLIND_SPOT; - } else if (module_name == "crosswalk") { - module.type = Module::CROSSWALK; - } else if (module_name == "detection_area") { - module.type = Module::DETECTION_AREA; - } else if (module_name == "intersection") { - module.type = Module::INTERSECTION; - } else if (module_name == "no_stopping_area") { - module.type = Module::NO_STOPPING_AREA; - } else if (module_name == "occlusion_spot") { - module.type = Module::OCCLUSION_SPOT; - } else if (module_name == "traffic_light") { - module.type = Module::TRAFFIC_LIGHT; - } else if (module_name == "virtual_traffic_light") { - module.type = Module::TRAFFIC_LIGHT; - } else if (module_name == "external_request_lane_change_left") { - module.type = Module::EXT_REQUEST_LANE_CHANGE_LEFT; - } else if (module_name == "external_request_lane_change_right") { - module.type = Module::EXT_REQUEST_LANE_CHANGE_RIGHT; - } else if (module_name == "lane_change_left") { - module.type = Module::LANE_CHANGE_LEFT; - } else if (module_name == "lane_change_right") { - module.type = Module::LANE_CHANGE_RIGHT; - } else if (module_name == "avoidance_by_lane_change_left") { - module.type = Module::AVOIDANCE_BY_LC_LEFT; - } else if (module_name == "avoidance_by_lane_change_right") { - module.type = Module::AVOIDANCE_BY_LC_RIGHT; - } else if (module_name == "avoidance_left") { - module.type = Module::AVOIDANCE_LEFT; - } else if (module_name == "avoidance_right") { - module.type = Module::AVOIDANCE_RIGHT; - } else if (module_name == "goal_planner") { - module.type = Module::GOAL_PLANNER; - } else if (module_name == "start_planner") { - module.type = Module::START_PLANNER; - } else if (module_name == "intersection_occlusion") { - module.type = Module::INTERSECTION_OCCLUSION; - } else { - module.type = Module::NONE; - } - return module; -} - -RTCAutoModeManagerInterface::RTCAutoModeManagerInterface( - rclcpp::Node * node, const std::string & module_name, const bool default_enable) -{ - using std::chrono_literals::operator""s; - using std::placeholders::_1; - using std::placeholders::_2; - - // Service client - enable_cli_ = - node->create_client(enable_auto_mode_namespace_ + "/internal/" + module_name); - - while (!enable_cli_->wait_for_service(1s)) { - if (!rclcpp::ok()) { - RCLCPP_ERROR(node->get_logger(), "Interrupted while waiting for service."); - rclcpp::shutdown(); - return; - } - RCLCPP_INFO_STREAM(node->get_logger(), "Waiting for service... [" << module_name << "]"); - } - - // Service - enable_srv_ = node->create_service( - enable_auto_mode_namespace_ + "/" + module_name, - std::bind(&RTCAutoModeManagerInterface::onEnableService, this, _1, _2)); - - auto_mode_status_.module = getModuleType(module_name); - // Send enable auto mode request - if (default_enable) { - auto_mode_status_.is_auto_mode = true; - AutoMode::Request::SharedPtr request = std::make_shared(); - request->enable = true; - enable_cli_->async_send_request(request); - } -} - -void RTCAutoModeManagerInterface::onEnableService( - const AutoMode::Request::SharedPtr request, const AutoMode::Response::SharedPtr response) -{ - auto_mode_status_.is_auto_mode = request->enable; - enable_cli_->async_send_request(request); - response->success = true; -} - -} // namespace rtc_auto_mode_manager diff --git a/planning/rtc_interface/include/rtc_interface/rtc_interface.hpp b/planning/rtc_interface/include/rtc_interface/rtc_interface.hpp index 4929d0f8e27f3..d6e083421c0b7 100644 --- a/planning/rtc_interface/include/rtc_interface/rtc_interface.hpp +++ b/planning/rtc_interface/include/rtc_interface/rtc_interface.hpp @@ -85,7 +85,7 @@ class RTCInterface std::string cooperate_status_namespace_ = "/planning/cooperate_status"; std::string cooperate_commands_namespace_ = "/planning/cooperate_commands"; - std::string enable_auto_mode_namespace_ = "/planning/enable_auto_mode/internal"; + std::string enable_auto_mode_namespace_ = "/planning/enable_auto_mode"; mutable std::mutex mutex_; }; diff --git a/planning/rtc_replayer/launch/rtc_replayer.launch.xml b/planning/rtc_replayer/launch/rtc_replayer.launch.xml index 9b832b369c77c..70d7baf2dce4a 100644 --- a/planning/rtc_replayer/launch/rtc_replayer.launch.xml +++ b/planning/rtc_replayer/launch/rtc_replayer.launch.xml @@ -1,7 +1,3 @@ - - - - - + diff --git a/planning/sampling_based_planner/path_sampler/include/path_sampler/common_structs.hpp b/planning/sampling_based_planner/path_sampler/include/path_sampler/common_structs.hpp index 30d9a77f31fad..7cf88cb75e13a 100644 --- a/planning/sampling_based_planner/path_sampler/include/path_sampler/common_structs.hpp +++ b/planning/sampling_based_planner/path_sampler/include/path_sampler/common_structs.hpp @@ -18,7 +18,10 @@ #include "path_sampler/type_alias.hpp" #include "rclcpp/rclcpp.hpp" #include "sampler_common/structures.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" +#include "tier4_autoware_utils/ros/update_param.hpp" +#include "tier4_autoware_utils/system/stop_watch.hpp" + +#include #include #include diff --git a/planning/sampling_based_planner/path_sampler/include/path_sampler/node.hpp b/planning/sampling_based_planner/path_sampler/include/path_sampler/node.hpp index b1c6ec61db845..a9002c8cf8a9f 100644 --- a/planning/sampling_based_planner/path_sampler/include/path_sampler/node.hpp +++ b/planning/sampling_based_planner/path_sampler/include/path_sampler/node.hpp @@ -15,13 +15,11 @@ #ifndef PATH_SAMPLER__NODE_HPP_ #define PATH_SAMPLER__NODE_HPP_ -#include "motion_utils/motion_utils.hpp" #include "path_sampler/common_structs.hpp" #include "path_sampler/parameters.hpp" #include "path_sampler/type_alias.hpp" #include "rclcpp/rclcpp.hpp" #include "sampler_common/transform/spline_transform.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" #include "vehicle_info_util/vehicle_info_util.hpp" #include diff --git a/planning/sampling_based_planner/path_sampler/src/node.cpp b/planning/sampling_based_planner/path_sampler/src/node.cpp index 588f29b06c40d..aa037755bbedd 100644 --- a/planning/sampling_based_planner/path_sampler/src/node.cpp +++ b/planning/sampling_based_planner/path_sampler/src/node.cpp @@ -15,6 +15,7 @@ #include "path_sampler/node.hpp" #include "interpolation/spline_interpolation_points_2d.hpp" +#include "motion_utils/marker/marker_helper.hpp" #include "path_sampler/path_generation.hpp" #include "path_sampler/prepare_inputs.hpp" #include "path_sampler/utils/geometry_utils.hpp" @@ -23,6 +24,8 @@ #include "sampler_common/constraints/hard_constraint.hpp" #include "sampler_common/constraints/soft_constraint.hpp" +#include + #include #include diff --git a/planning/sampling_based_planner/path_sampler/src/prepare_inputs.cpp b/planning/sampling_based_planner/path_sampler/src/prepare_inputs.cpp index c17f3479932f3..fd52764950ca9 100644 --- a/planning/sampling_based_planner/path_sampler/src/prepare_inputs.cpp +++ b/planning/sampling_based_planner/path_sampler/src/prepare_inputs.cpp @@ -18,6 +18,7 @@ #include "path_sampler/utils/geometry_utils.hpp" #include "sampler_common/structures.hpp" #include "sampler_common/transform/spline_transform.hpp" +#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" #include diff --git a/planning/sampling_based_planner/path_sampler/src/utils/trajectory_utils.cpp b/planning/sampling_based_planner/path_sampler/src/utils/trajectory_utils.cpp index ec41f3032e150..db55e5f8557e4 100644 --- a/planning/sampling_based_planner/path_sampler/src/utils/trajectory_utils.cpp +++ b/planning/sampling_based_planner/path_sampler/src/utils/trajectory_utils.cpp @@ -14,7 +14,8 @@ #include "path_sampler/utils/trajectory_utils.hpp" -#include "motion_utils/motion_utils.hpp" +#include "motion_utils/resample/resample.hpp" +#include "motion_utils/trajectory/tmp_conversion.hpp" #include "path_sampler/utils/geometry_utils.hpp" #include "tf2/utils.h" diff --git a/planning/static_centerline_optimizer/include/static_centerline_optimizer/type_alias.hpp b/planning/static_centerline_optimizer/include/static_centerline_optimizer/type_alias.hpp index 5798c19d0d563..1a700d36deaaf 100644 --- a/planning/static_centerline_optimizer/include/static_centerline_optimizer/type_alias.hpp +++ b/planning/static_centerline_optimizer/include/static_centerline_optimizer/type_alias.hpp @@ -15,6 +15,7 @@ #define STATIC_CENTERLINE_OPTIMIZER__TYPE_ALIAS_HPP_ #include "route_handler/route_handler.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" #include "autoware_auto_mapping_msgs/msg/had_map_bin.hpp" #include "autoware_auto_perception_msgs/msg/predicted_objects.hpp" diff --git a/planning/static_centerline_optimizer/include/static_centerline_optimizer/utils.hpp b/planning/static_centerline_optimizer/include/static_centerline_optimizer/utils.hpp index 91b27c74a7fcb..8c8c346557fce 100644 --- a/planning/static_centerline_optimizer/include/static_centerline_optimizer/utils.hpp +++ b/planning/static_centerline_optimizer/include/static_centerline_optimizer/utils.hpp @@ -18,6 +18,8 @@ #include "route_handler/route_handler.hpp" #include "static_centerline_optimizer/type_alias.hpp" +#include + #include #include #include diff --git a/planning/static_centerline_optimizer/package.xml b/planning/static_centerline_optimizer/package.xml index c30a9f2807df3..89976be5bdd47 100644 --- a/planning/static_centerline_optimizer/package.xml +++ b/planning/static_centerline_optimizer/package.xml @@ -32,6 +32,7 @@ rclcpp_components route_handler tier4_autoware_utils + tier4_map_msgs vehicle_info_util python3-flask-cors diff --git a/planning/static_centerline_optimizer/rviz/static_centerline_optimizer.rviz b/planning/static_centerline_optimizer/rviz/static_centerline_optimizer.rviz index c2df606fbaf41..1e7573d561ba1 100644 --- a/planning/static_centerline_optimizer/rviz/static_centerline_optimizer.rviz +++ b/planning/static_centerline_optimizer/rviz/static_centerline_optimizer.rviz @@ -18,8 +18,6 @@ Panels: - /Current View1 Name: Views Splitter Ratio: 0.5 - - Class: tier4_localization_rviz_plugin/InitialPoseButtonPanel - Name: InitialPoseButtonPanel - Class: rviz_common/Time Experimental: false Name: Time @@ -423,8 +421,6 @@ Window Geometry: Height: 1043 Hide Left Dock: false Hide Right Dock: false - InitialPoseButtonPanel: - collapsed: false QMainWindow State: 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 Selection: collapsed: false diff --git a/planning/static_centerline_optimizer/src/static_centerline_optimizer_node.cpp b/planning/static_centerline_optimizer/src/static_centerline_optimizer_node.cpp index 9c30c57533335..4532eb66978cb 100644 --- a/planning/static_centerline_optimizer/src/static_centerline_optimizer_node.cpp +++ b/planning/static_centerline_optimizer/src/static_centerline_optimizer_node.cpp @@ -18,16 +18,24 @@ #include "lanelet2_extension/utility/query.hpp" #include "lanelet2_extension/utility/utilities.hpp" #include "map_loader/lanelet2_map_loader_node.hpp" -#include "motion_utils/motion_utils.hpp" +#include "motion_utils/trajectory/tmp_conversion.hpp" #include "static_centerline_optimizer/msg/points_with_lane_id.hpp" #include "static_centerline_optimizer/successive_trajectory_optimizer_node.hpp" #include "static_centerline_optimizer/type_alias.hpp" #include "static_centerline_optimizer/utils.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" #include #include +#include + +#include + +#include +#include #include +#include #include #include @@ -242,7 +250,9 @@ void StaticCenterlineOptimizerNode::load_map(const std::string & lanelet2_input_ map_bin_ptr_ = [&]() -> HADMapBin::ConstSharedPtr { // load map lanelet::LaneletMapPtr map_ptr; - map_ptr = Lanelet2MapLoaderNode::load_map(lanelet2_input_file_path, "MGRS"); + tier4_map_msgs::msg::MapProjectorInfo map_projector_info; + map_projector_info.projector_type = tier4_map_msgs::msg::MapProjectorInfo::MGRS; + map_ptr = Lanelet2MapLoaderNode::load_map(lanelet2_input_file_path, map_projector_info); if (!map_ptr) { return nullptr; } diff --git a/planning/static_centerline_optimizer/src/successive_trajectory_optimizer_node.cpp b/planning/static_centerline_optimizer/src/successive_trajectory_optimizer_node.cpp index 325c257e228f9..ab147ab1f4f74 100644 --- a/planning/static_centerline_optimizer/src/successive_trajectory_optimizer_node.cpp +++ b/planning/static_centerline_optimizer/src/successive_trajectory_optimizer_node.cpp @@ -14,9 +14,11 @@ #include "static_centerline_optimizer/successive_trajectory_optimizer_node.hpp" -#include "motion_utils/motion_utils.hpp" +#include "motion_utils/resample/resample.hpp" +#include "motion_utils/trajectory/tmp_conversion.hpp" +#include "motion_utils/trajectory/trajectory.hpp" #include "obstacle_avoidance_planner/utils/trajectory_utils.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" #include "vehicle_info_util/vehicle_info_util.hpp" #include diff --git a/planning/static_centerline_optimizer/src/utils.cpp b/planning/static_centerline_optimizer/src/utils.cpp index 691ae7960968c..d5169ac17af8a 100644 --- a/planning/static_centerline_optimizer/src/utils.cpp +++ b/planning/static_centerline_optimizer/src/utils.cpp @@ -16,8 +16,11 @@ #include "behavior_path_planner/data_manager.hpp" #include "behavior_path_planner/utils/utils.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" +#include "tier4_autoware_utils/ros/marker_helper.hpp" +#include +#include namespace static_centerline_optimizer { namespace diff --git a/planning/surround_obstacle_checker/config/surround_obstacle_checker.param.yaml b/planning/surround_obstacle_checker/config/surround_obstacle_checker.param.yaml index 10b5c2814e2a8..5ec10572ffe83 100644 --- a/planning/surround_obstacle_checker/config/surround_obstacle_checker.param.yaml +++ b/planning/surround_obstacle_checker/config/surround_obstacle_checker.param.yaml @@ -1,12 +1,61 @@ /**: ros__parameters: - # obstacle check - use_pointcloud: true # use pointcloud as obstacle check - use_dynamic_object: true # use dynamic object as obstacle check - surround_check_distance: 0.5 # if objects exist in this distance, transit to "exist-surrounding-obstacle" status [m] - surround_check_recover_distance: 0.8 # if no object exists in this distance, transit to "non-surrounding-obstacle" status [m] + # surround_check_*_distance: if objects exist in this distance, transit to "exist-surrounding-obstacle" status [m] + # surround_check_hysteresis_distance: if no object exists in this hysteresis distance added to the above distance, transit to "non-surrounding-obstacle" status [m] + pointcloud: + enable_check: false + surround_check_front_distance: 0.5 + surround_check_side_distance: 0.5 + surround_check_back_distance: 0.5 + unknown: + enable_check: true + surround_check_front_distance: 0.5 + surround_check_side_distance: 0.5 + surround_check_back_distance: 0.5 + car: + enable_check: true + surround_check_front_distance: 0.5 + surround_check_side_distance: 0.0 + surround_check_back_distance: 0.5 + truck: + enable_check: true + surround_check_front_distance: 0.5 + surround_check_side_distance: 0.0 + surround_check_back_distance: 0.5 + bus: + enable_check: true + surround_check_front_distance: 0.5 + surround_check_side_distance: 0.0 + surround_check_back_distance: 0.5 + trailer: + enable_check: true + surround_check_front_distance: 0.5 + surround_check_side_distance: 0.0 + surround_check_back_distance: 0.5 + motorcycle: + enable_check: true + surround_check_front_distance: 0.5 + surround_check_side_distance: 0.0 + surround_check_back_distance: 0.5 + bicycle: + enable_check: true + surround_check_front_distance: 0.5 + surround_check_side_distance: 0.5 + surround_check_back_distance: 0.5 + pedestrian: + enable_check: true + surround_check_front_distance: 0.5 + surround_check_side_distance: 0.5 + surround_check_back_distance: 0.5 + + surround_check_hysteresis_distance: 0.3 + state_clear_time: 2.0 + # ego stop state + stop_state_ego_speed: 0.1 #[m/s] + # debug publish_debug_footprints: true # publish vehicle footprint & footprints with surround_check_distance and surround_check_recover_distance offsets + debug_footprint_label: "car" diff --git a/planning/surround_obstacle_checker/include/surround_obstacle_checker/debug_marker.hpp b/planning/surround_obstacle_checker/include/surround_obstacle_checker/debug_marker.hpp index bd0fe48bbda28..590f026942801 100644 --- a/planning/surround_obstacle_checker/include/surround_obstacle_checker/debug_marker.hpp +++ b/planning/surround_obstacle_checker/include/surround_obstacle_checker/debug_marker.hpp @@ -16,6 +16,7 @@ #define SURROUND_OBSTACLE_CHECKER__DEBUG_MARKER_HPP_ #include +#include #include #include @@ -38,6 +39,7 @@ using geometry_msgs::msg::PolygonStamped; using tier4_planning_msgs::msg::StopFactor; using tier4_planning_msgs::msg::StopReason; using tier4_planning_msgs::msg::StopReasonArray; +using vehicle_info_util::VehicleInfo; using visualization_msgs::msg::Marker; using visualization_msgs::msg::MarkerArray; @@ -52,15 +54,19 @@ class SurroundObstacleCheckerDebugNode { public: explicit SurroundObstacleCheckerDebugNode( - const Polygon2d & ego_polygon, const double base_link2front, - const double & surround_check_distance, const double & surround_check_recover_distance, - const geometry_msgs::msg::Pose & self_pose, const rclcpp::Clock::SharedPtr clock, - rclcpp::Node & node); + const vehicle_info_util::VehicleInfo & vehicle_info, const double base_link2front, + const std::string & object_label, const double & surround_check_front_distance, + const double & surround_check_side_distance, const double & surround_check_back_distance, + const double & surround_check_hysteresis_distance, const geometry_msgs::msg::Pose & self_pose, + const rclcpp::Clock::SharedPtr clock, rclcpp::Node & node); bool pushPose(const geometry_msgs::msg::Pose & pose, const PoseType & type); bool pushObstaclePoint(const geometry_msgs::msg::Point & obstacle_point, const PointType & type); void publish(); void publishFootprints(); + void updateFootprintMargin( + const std::string & object_label, const double front_distance, const double side_distance, + const double back_distance); private: rclcpp::Publisher::SharedPtr debug_virtual_wall_pub_; @@ -72,10 +78,13 @@ class SurroundObstacleCheckerDebugNode rclcpp::Publisher::SharedPtr vehicle_footprint_offset_pub_; rclcpp::Publisher::SharedPtr vehicle_footprint_recover_offset_pub_; - Polygon2d ego_polygon_; + vehicle_info_util::VehicleInfo vehicle_info_; double base_link2front_; - double surround_check_distance_; - double surround_check_recover_distance_; + std::string object_label_; + double surround_check_front_distance_; + double surround_check_side_distance_; + double surround_check_back_distance_; + double surround_check_hysteresis_distance_; geometry_msgs::msg::Pose self_pose_; MarkerArray makeVirtualWallMarker(); @@ -83,7 +92,6 @@ class SurroundObstacleCheckerDebugNode StopReasonArray makeStopReasonArray(); VelocityFactorArray makeVelocityFactorArray(); - Polygon2d createSelfPolygonWithOffset(const Polygon2d & base_polygon, const double & offset); PolygonStamped boostPolygonToPolygonStamped(const Polygon2d & boost_polygon, const double & z); std::shared_ptr stop_obstacle_point_ptr_; diff --git a/planning/surround_obstacle_checker/include/surround_obstacle_checker/node.hpp b/planning/surround_obstacle_checker/include/surround_obstacle_checker/node.hpp index de3e4bf120b13..c40fef5b0c43e 100644 --- a/planning/surround_obstacle_checker/include/surround_obstacle_checker/node.hpp +++ b/planning/surround_obstacle_checker/include/surround_obstacle_checker/node.hpp @@ -16,11 +16,10 @@ #define SURROUND_OBSTACLE_CHECKER__NODE_HPP_ #include "surround_obstacle_checker/debug_marker.hpp" +#include "tier4_autoware_utils/ros/logger_level_configure.hpp" -#include +#include #include -#include -#include #include #include @@ -32,12 +31,15 @@ #include #include +#include + #include #include #include #include #include +#include #include #include @@ -62,18 +64,27 @@ class SurroundObstacleCheckerNode : public rclcpp::Node struct NodeParam { - bool use_pointcloud; - bool use_dynamic_object; bool is_surround_obstacle; - // For preventing chattering, - // surround_check_recover_distance_ must be bigger than surround_check_distance_ - double surround_check_recover_distance; - double surround_check_distance; + std::unordered_map enable_check_map; + std::unordered_map surround_check_front_distance_map; + std::unordered_map surround_check_side_distance_map; + std::unordered_map surround_check_back_distance_map; + bool pointcloud_enable_check; + double pointcloud_surround_check_front_distance; + double pointcloud_surround_check_side_distance; + double pointcloud_surround_check_back_distance; + double surround_check_hysteresis_distance; double state_clear_time; bool publish_debug_footprints; + std::string debug_footprint_label; }; private: + rcl_interfaces::msg::SetParametersResult onParam( + const std::vector & parameters); + + std::array getCheckDistances(const std::string & str_label) const; + void onTimer(); void onPointCloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg); @@ -107,6 +118,9 @@ class SurroundObstacleCheckerNode : public rclcpp::Node rclcpp::Publisher::SharedPtr pub_clear_velocity_limit_; rclcpp::Publisher::SharedPtr pub_velocity_limit_; + // parameter callback result + OnSetParametersCallbackHandle::SharedPtr set_param_res_; + // stop checker std::unique_ptr vehicle_stop_checker_; @@ -125,6 +139,8 @@ class SurroundObstacleCheckerNode : public rclcpp::Node // State Machine State state_ = State::PASS; std::shared_ptr last_obstacle_found_time_; + + std::unique_ptr logger_configure_; }; } // namespace surround_obstacle_checker diff --git a/planning/surround_obstacle_checker/src/debug_marker.cpp b/planning/surround_obstacle_checker/src/debug_marker.cpp index c30f778584fd7..ebe5552c44c92 100644 --- a/planning/surround_obstacle_checker/src/debug_marker.cpp +++ b/planning/surround_obstacle_checker/src/debug_marker.cpp @@ -14,8 +14,10 @@ #include "surround_obstacle_checker/debug_marker.hpp" -#include -#include +#include +#include +#include +#include #ifdef ROS_DISTRO_GALACTIC #include #else @@ -26,6 +28,29 @@ namespace surround_obstacle_checker { +namespace +{ +Polygon2d createSelfPolygon( + const VehicleInfo & vehicle_info, const double front_margin = 0.0, const double side_margin = 0.0, + const double rear_margin = 0.0) +{ + const double & front_m = vehicle_info.max_longitudinal_offset_m + front_margin; + const double & width_left_m = vehicle_info.max_lateral_offset_m + side_margin; + const double & width_right_m = vehicle_info.min_lateral_offset_m - side_margin; + const double & rear_m = vehicle_info.min_longitudinal_offset_m - rear_margin; + + Polygon2d ego_polygon; + + ego_polygon.outer().push_back(Point2d(front_m, width_left_m)); + ego_polygon.outer().push_back(Point2d(front_m, width_right_m)); + ego_polygon.outer().push_back(Point2d(rear_m, width_right_m)); + ego_polygon.outer().push_back(Point2d(rear_m, width_left_m)); + + bg::correct(ego_polygon); + + return ego_polygon; +} +} // namespace using motion_utils::createStopVirtualWallMarker; using tier4_autoware_utils::appendMarkerArray; @@ -36,14 +61,18 @@ using tier4_autoware_utils::createMarkerScale; using tier4_autoware_utils::createPoint; SurroundObstacleCheckerDebugNode::SurroundObstacleCheckerDebugNode( - const Polygon2d & ego_polygon, const double base_link2front, - const double & surround_check_distance, const double & surround_check_recover_distance, - const geometry_msgs::msg::Pose & self_pose, const rclcpp::Clock::SharedPtr clock, - rclcpp::Node & node) -: ego_polygon_(ego_polygon), + const vehicle_info_util::VehicleInfo & vehicle_info, const double base_link2front, + const std::string & object_label, const double & surround_check_front_distance, + const double & surround_check_side_distance, const double & surround_check_back_distance, + const double & surround_check_hysteresis_distance, const geometry_msgs::msg::Pose & self_pose, + const rclcpp::Clock::SharedPtr clock, rclcpp::Node & node) +: vehicle_info_(vehicle_info), base_link2front_(base_link2front), - surround_check_distance_(surround_check_distance), - surround_check_recover_distance_(surround_check_recover_distance), + object_label_(object_label), + surround_check_front_distance_(surround_check_front_distance), + surround_check_side_distance_(surround_check_side_distance), + surround_check_back_distance_(surround_check_back_distance), + surround_check_hysteresis_distance_(surround_check_hysteresis_distance), self_pose_(self_pose), clock_(clock) { @@ -86,20 +115,25 @@ bool SurroundObstacleCheckerDebugNode::pushObstaclePoint( void SurroundObstacleCheckerDebugNode::publishFootprints() { + const auto ego_polygon = createSelfPolygon(vehicle_info_); + /* publish vehicle footprint polygon */ - const auto footprint = boostPolygonToPolygonStamped(ego_polygon_, self_pose_.position.z); + const auto footprint = boostPolygonToPolygonStamped(ego_polygon, self_pose_.position.z); vehicle_footprint_pub_->publish(footprint); /* publish vehicle footprint polygon with offset */ - const auto polygon_with_offset = - createSelfPolygonWithOffset(ego_polygon_, surround_check_distance_); + const auto polygon_with_offset = createSelfPolygon( + vehicle_info_, surround_check_front_distance_, surround_check_side_distance_, + surround_check_back_distance_); const auto footprint_with_offset = boostPolygonToPolygonStamped(polygon_with_offset, self_pose_.position.z); vehicle_footprint_offset_pub_->publish(footprint_with_offset); /* publish vehicle footprint polygon with recover offset */ - const auto polygon_with_recover_offset = - createSelfPolygonWithOffset(ego_polygon_, surround_check_recover_distance_); + const auto polygon_with_recover_offset = createSelfPolygon( + vehicle_info_, surround_check_front_distance_ + surround_check_hysteresis_distance_, + surround_check_side_distance_ + surround_check_hysteresis_distance_, + surround_check_back_distance_ + surround_check_hysteresis_distance_); const auto footprint_with_recover_offset = boostPolygonToPolygonStamped(polygon_with_recover_offset, self_pose_.position.z); vehicle_footprint_recover_offset_pub_->publish(footprint_with_recover_offset); @@ -206,26 +240,6 @@ VelocityFactorArray SurroundObstacleCheckerDebugNode::makeVelocityFactorArray() return velocity_factor_array; } -Polygon2d SurroundObstacleCheckerDebugNode::createSelfPolygonWithOffset( - const Polygon2d & base_polygon, const double & offset) -{ - typedef double coordinate_type; - const double buffer_distance = offset; - const int points_per_circle = 36; - boost::geometry::strategy::buffer::distance_symmetric distance_strategy( - buffer_distance); - boost::geometry::strategy::buffer::join_round join_strategy(points_per_circle); - boost::geometry::strategy::buffer::end_round end_strategy(points_per_circle); - boost::geometry::strategy::buffer::point_circle circle_strategy(points_per_circle); - boost::geometry::strategy::buffer::side_straight side_strategy; - boost::geometry::model::multi_polygon result; - // Create the buffer of a multi polygon - boost::geometry::buffer( - base_polygon, result, distance_strategy, side_strategy, join_strategy, end_strategy, - circle_strategy); - return result.front(); -} - PolygonStamped SurroundObstacleCheckerDebugNode::boostPolygonToPolygonStamped( const Polygon2d & boost_polygon, const double & z) { @@ -244,4 +258,14 @@ PolygonStamped SurroundObstacleCheckerDebugNode::boostPolygonToPolygonStamped( return polygon_stamped; } +void SurroundObstacleCheckerDebugNode::updateFootprintMargin( + const std::string & object_label, const double front_distance, const double side_distance, + const double back_distance) +{ + object_label_ = object_label; + surround_check_front_distance_ = front_distance; + surround_check_side_distance_ = side_distance; + surround_check_back_distance_ = back_distance; +} + } // namespace surround_obstacle_checker diff --git a/planning/surround_obstacle_checker/src/node.cpp b/planning/surround_obstacle_checker/src/node.cpp index 17a47be8c0596..1b981fcddb155 100644 --- a/planning/surround_obstacle_checker/src/node.cpp +++ b/planning/surround_obstacle_checker/src/node.cpp @@ -15,6 +15,9 @@ #include "surround_obstacle_checker/node.hpp" #include +#include +#include +#include #include #include @@ -47,6 +50,7 @@ namespace surround_obstacle_checker namespace bg = boost::geometry; using Point2d = bg::model::d2::point_xy; using Polygon2d = bg::model::polygon; +using autoware_auto_perception_msgs::msg::ObjectClassification; using tier4_autoware_utils::createPoint; using tier4_autoware_utils::pose2transform; @@ -120,12 +124,14 @@ Polygon2d createObjPolygon( return createObjPolygon(pose, polygon); } -Polygon2d createSelfPolygon(const VehicleInfo & vehicle_info) +Polygon2d createSelfPolygon( + const VehicleInfo & vehicle_info, const double front_margin, const double side_margin, + const double rear_margin) { - const double & front_m = vehicle_info.max_longitudinal_offset_m; - const double & width_left_m = vehicle_info.max_lateral_offset_m; - const double & width_right_m = vehicle_info.min_lateral_offset_m; - const double & rear_m = vehicle_info.min_longitudinal_offset_m; + const double & front_m = vehicle_info.max_longitudinal_offset_m + front_margin; + const double & width_left_m = vehicle_info.max_lateral_offset_m + side_margin; + const double & width_right_m = vehicle_info.min_lateral_offset_m - side_margin; + const double & rear_m = vehicle_info.min_longitudinal_offset_m - rear_margin; Polygon2d ego_polygon; @@ -146,13 +152,44 @@ SurroundObstacleCheckerNode::SurroundObstacleCheckerNode(const rclcpp::NodeOptio // Parameters { auto & p = node_param_; - p.use_pointcloud = this->declare_parameter("use_pointcloud"); - p.use_dynamic_object = this->declare_parameter("use_dynamic_object"); - p.surround_check_distance = this->declare_parameter("surround_check_distance"); - p.surround_check_recover_distance = - this->declare_parameter("surround_check_recover_distance"); + + // for object label + std::unordered_map label_map{ + {"unknown", ObjectClassification::UNKNOWN}, {"car", ObjectClassification::CAR}, + {"truck", ObjectClassification::TRUCK}, {"bus", ObjectClassification::BUS}, + {"trailer", ObjectClassification::TRAILER}, {"motorcycle", ObjectClassification::MOTORCYCLE}, + {"bicycle", ObjectClassification::BICYCLE}, {"pedestrian", ObjectClassification::PEDESTRIAN}}; + for (const auto & label_pair : label_map) { + p.enable_check_map.emplace( + label_pair.second, this->declare_parameter(label_pair.first + ".enable_check")); + p.surround_check_front_distance_map.emplace( + label_pair.second, + this->declare_parameter(label_pair.first + ".surround_check_front_distance")); + p.surround_check_side_distance_map.emplace( + label_pair.second, + this->declare_parameter(label_pair.first + ".surround_check_side_distance")); + p.surround_check_back_distance_map.emplace( + label_pair.second, + this->declare_parameter(label_pair.first + ".surround_check_back_distance")); + } + + // for pointcloud + p.pointcloud_enable_check = this->declare_parameter("pointcloud.enable_check"); + p.pointcloud_surround_check_front_distance = + this->declare_parameter("pointcloud.surround_check_front_distance"); + p.pointcloud_surround_check_side_distance = + this->declare_parameter("pointcloud.surround_check_side_distance"); + p.pointcloud_surround_check_back_distance = + this->declare_parameter("pointcloud.surround_check_back_distance"); + + p.surround_check_hysteresis_distance = + this->declare_parameter("surround_check_hysteresis_distance"); + p.state_clear_time = this->declare_parameter("state_clear_time"); p.publish_debug_footprints = this->declare_parameter("publish_debug_footprints"); + p.debug_footprint_label = this->declare_parameter("debug_footprint_label"); + + logger_configure_ = std::make_unique(this); } vehicle_info_ = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); @@ -176,6 +213,10 @@ SurroundObstacleCheckerNode::SurroundObstacleCheckerNode(const rclcpp::NodeOptio "~/input/odometry", 1, std::bind(&SurroundObstacleCheckerNode::onOdometry, this, std::placeholders::_1)); + // Parameter callback + set_param_res_ = this->add_on_set_parameters_callback( + std::bind(&SurroundObstacleCheckerNode::onParam, this, std::placeholders::_1)); + using std::chrono_literals::operator""ms; timer_ = rclcpp::create_timer( this, get_clock(), 100ms, std::bind(&SurroundObstacleCheckerNode::onTimer, this)); @@ -184,15 +225,55 @@ SurroundObstacleCheckerNode::SurroundObstacleCheckerNode(const rclcpp::NodeOptio vehicle_stop_checker_ = std::make_unique(this); // Debug - auto const self_polygon = createSelfPolygon(vehicle_info_); odometry_ptr_ = std::make_shared(); + const auto check_distances = getCheckDistances(node_param_.debug_footprint_label); debug_ptr_ = std::make_shared( - self_polygon, vehicle_info_.max_longitudinal_offset_m, node_param_.surround_check_distance, - node_param_.surround_check_recover_distance, odometry_ptr_->pose.pose, this->get_clock(), + vehicle_info_, vehicle_info_.max_longitudinal_offset_m, node_param_.debug_footprint_label, + check_distances.at(0), check_distances.at(1), check_distances.at(2), + node_param_.surround_check_hysteresis_distance, odometry_ptr_->pose.pose, this->get_clock(), *this); } +std::array SurroundObstacleCheckerNode::getCheckDistances( + const std::string & str_label) const +{ + if (str_label == "pointcloud") { + return { + node_param_.pointcloud_surround_check_front_distance, + node_param_.pointcloud_surround_check_side_distance, + node_param_.pointcloud_surround_check_back_distance}; + } + + std::unordered_map label_map{ + {"unknown", ObjectClassification::UNKNOWN}, {"car", ObjectClassification::CAR}, + {"truck", ObjectClassification::TRUCK}, {"bus", ObjectClassification::BUS}, + {"trailer", ObjectClassification::TRAILER}, {"motorcycle", ObjectClassification::MOTORCYCLE}, + {"bicycle", ObjectClassification::BICYCLE}, {"pedestrian", ObjectClassification::PEDESTRIAN}}; + + const int int_label = label_map.at(str_label); + return { + node_param_.surround_check_front_distance_map.at(int_label), + node_param_.surround_check_side_distance_map.at(int_label), + node_param_.surround_check_back_distance_map.at(int_label)}; +} + +rcl_interfaces::msg::SetParametersResult SurroundObstacleCheckerNode::onParam( + const std::vector & parameters) +{ + tier4_autoware_utils::updateParam( + parameters, "debug_footprint_label", node_param_.debug_footprint_label); + const auto check_distances = getCheckDistances(node_param_.debug_footprint_label); + debug_ptr_->updateFootprintMargin( + node_param_.debug_footprint_label, check_distances.at(0), check_distances.at(1), + check_distances.at(2)); + + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + result.reason = "success"; + return result; +} + void SurroundObstacleCheckerNode::onTimer() { if (!odometry_ptr_) { @@ -205,13 +286,22 @@ void SurroundObstacleCheckerNode::onTimer() debug_ptr_->publishFootprints(); } - if (node_param_.use_pointcloud && !pointcloud_ptr_) { + if (node_param_.pointcloud_enable_check && !pointcloud_ptr_) { RCLCPP_INFO_THROTTLE( this->get_logger(), *this->get_clock(), 5000 /* ms */, "waiting for pointcloud info..."); return; } - if (node_param_.use_dynamic_object && !object_ptr_) { + const bool use_dynamic_object = + node_param_.enable_check_map.at(ObjectClassification::UNKNOWN) || + node_param_.enable_check_map.at(ObjectClassification::CAR) || + node_param_.enable_check_map.at(ObjectClassification::TRUCK) || + node_param_.enable_check_map.at(ObjectClassification::BUS) || + node_param_.enable_check_map.at(ObjectClassification::TRAILER) || + node_param_.enable_check_map.at(ObjectClassification::MOTORCYCLE) || + node_param_.enable_check_map.at(ObjectClassification::BICYCLE) || + node_param_.enable_check_map.at(ObjectClassification::PEDESTRIAN); + if (use_dynamic_object && !object_ptr_) { RCLCPP_INFO_THROTTLE( this->get_logger(), *this->get_clock(), 5000 /* ms */, "waiting for dynamic object info..."); return; @@ -220,11 +310,11 @@ void SurroundObstacleCheckerNode::onTimer() const auto nearest_obstacle = getNearestObstacle(); const auto is_vehicle_stopped = vehicle_stop_checker_->isVehicleStopped(); + constexpr double epsilon = 1e-3; switch (state_) { case State::PASS: { const auto is_obstacle_found = - !nearest_obstacle ? false - : nearest_obstacle.get().first < node_param_.surround_check_distance; + !nearest_obstacle ? false : nearest_obstacle.get().first < epsilon; if (!isStopRequired(is_obstacle_found, is_vehicle_stopped)) { break; @@ -250,7 +340,7 @@ void SurroundObstacleCheckerNode::onTimer() const auto is_obstacle_found = !nearest_obstacle ? false - : nearest_obstacle.get().first < node_param_.surround_check_recover_distance; + : nearest_obstacle.get().first < node_param_.surround_check_hysteresis_distance; if (isStopRequired(is_obstacle_found, is_vehicle_stopped)) { break; @@ -304,17 +394,8 @@ void SurroundObstacleCheckerNode::onOdometry(const nav_msgs::msg::Odometry::Cons boost::optional SurroundObstacleCheckerNode::getNearestObstacle() const { - boost::optional nearest_pointcloud{boost::none}; - boost::optional nearest_object{boost::none}; - - if (node_param_.use_pointcloud) { - nearest_pointcloud = getNearestObstacleByPointCloud(); - } - - if (node_param_.use_dynamic_object) { - nearest_object = getNearestObstacleByDynamicObject(); - } - + const auto nearest_pointcloud = getNearestObstacleByPointCloud(); + const auto nearest_object = getNearestObstacleByDynamicObject(); if (!nearest_pointcloud && !nearest_object) { return {}; } @@ -333,15 +414,13 @@ boost::optional SurroundObstacleCheckerNode::getNearestObstacle() cons boost::optional SurroundObstacleCheckerNode::getNearestObstacleByPointCloud() const { - if (pointcloud_ptr_->data.empty()) { + if (!node_param_.pointcloud_enable_check || pointcloud_ptr_->data.empty()) { return boost::none; } + const auto transform_stamped = getTransform("base_link", pointcloud_ptr_->header.frame_id, pointcloud_ptr_->header.stamp, 0.5); - geometry_msgs::msg::Point nearest_point; - auto minimum_distance = std::numeric_limits::max(); - if (!transform_stamped) { return {}; } @@ -352,8 +431,14 @@ boost::optional SurroundObstacleCheckerNode::getNearestObstacleByPoint tier4_autoware_utils::transformPointCloud( transformed_pointcloud, transformed_pointcloud, isometry); - const auto ego_polygon = createSelfPolygon(vehicle_info_); + const double front_margin = node_param_.pointcloud_surround_check_front_distance; + const double side_margin = node_param_.pointcloud_surround_check_side_distance; + const double back_margin = node_param_.pointcloud_surround_check_back_distance; + const auto ego_polygon = createSelfPolygon(vehicle_info_, front_margin, side_margin, back_margin); + geometry_msgs::msg::Point nearest_point; + double minimum_distance = std::numeric_limits::max(); + bool was_minimum_distance_updated = false; for (const auto & p : transformed_pointcloud) { Point2d boost_point(p.x, p.y); @@ -362,10 +447,14 @@ boost::optional SurroundObstacleCheckerNode::getNearestObstacleByPoint if (distance_to_object < minimum_distance) { nearest_point = createPoint(p.x, p.y, p.z); minimum_distance = distance_to_object; + was_minimum_distance_updated = true; } } - return std::make_pair(minimum_distance, nearest_point); + if (was_minimum_distance_updated) { + return std::make_pair(minimum_distance, nearest_point); + } + return boost::none; } boost::optional SurroundObstacleCheckerNode::getNearestObstacleByDynamicObject() const @@ -373,9 +462,6 @@ boost::optional SurroundObstacleCheckerNode::getNearestObstacleByDynam const auto transform_stamped = getTransform(object_ptr_->header.frame_id, "base_link", object_ptr_->header.stamp, 0.5); - geometry_msgs::msg::Point nearest_point; - auto minimum_distance = std::numeric_limits::max(); - if (!transform_stamped) { return {}; } @@ -383,10 +469,23 @@ boost::optional SurroundObstacleCheckerNode::getNearestObstacleByDynam tf2::Transform tf_src2target; tf2::fromMsg(transform_stamped.get().transform, tf_src2target); - const auto ego_polygon = createSelfPolygon(vehicle_info_); - + // TODO(murooka) check computation cost + geometry_msgs::msg::Point nearest_point; + double minimum_distance = std::numeric_limits::max(); + bool was_minimum_distance_updated = false; for (const auto & object : object_ptr_->objects) { const auto & object_pose = object.kinematics.initial_pose_with_covariance.pose; + const int label = object.classification.front().label; + + if (!node_param_.enable_check_map.at(label)) { + continue; + } + + const double front_margin = node_param_.surround_check_front_distance_map.at(label); + const double side_margin = node_param_.surround_check_side_distance_map.at(label); + const double back_margin = node_param_.surround_check_back_distance_map.at(label); + const auto ego_polygon = + createSelfPolygon(vehicle_info_, front_margin, side_margin, back_margin); tf2::Transform tf_src2object; tf2::fromMsg(object_pose, tf_src2object); @@ -404,10 +503,14 @@ boost::optional SurroundObstacleCheckerNode::getNearestObstacleByDynam if (distance_to_object < minimum_distance) { nearest_point = object_pose.position; minimum_distance = distance_to_object; + was_minimum_distance_updated = true; } } - return std::make_pair(minimum_distance, nearest_point); + if (was_minimum_distance_updated) { + return std::make_pair(minimum_distance, nearest_point); + } + return boost::none; } boost::optional SurroundObstacleCheckerNode::getTransform( diff --git a/sensing/gnss_poser/CMakeLists.txt b/sensing/gnss_poser/CMakeLists.txt index 993cf2d9da9e1..775cde514945f 100644 --- a/sensing/gnss_poser/CMakeLists.txt +++ b/sensing/gnss_poser/CMakeLists.txt @@ -16,9 +16,7 @@ find_library(GeographicLib_LIBRARIES ) set(GNSS_POSER_HEADERS - include/gnss_poser/convert.hpp include/gnss_poser/gnss_poser_core.hpp - include/gnss_poser/gnss_stat.hpp ) ament_auto_add_library(gnss_poser_node SHARED @@ -36,6 +34,6 @@ rclcpp_components_register_node(gnss_poser_node ) ament_auto_package(INSTALL_TO_SHARE - launch config + launch ) diff --git a/sensing/gnss_poser/README.md b/sensing/gnss_poser/README.md index 6eef2cc243ef5..b271f75eeeb6c 100644 --- a/sensing/gnss_poser/README.md +++ b/sensing/gnss_poser/README.md @@ -12,6 +12,7 @@ The `gnss_poser` is a node that subscribes gnss sensing messages and calculates | Name | Type | Description | | ------------------------------ | ------------------------------------------------------- | ------------------------------------------------------------------------------------------------------------------------------ | +| `/map/map_projector_info` | `tier4_map_msgs::msg::MapProjectorInfo` | map projection info | | `~/input/fix` | `sensor_msgs::msg::NavSatFix` | gnss status message | | `~/input/autoware_orientation` | `autoware_sensing_msgs::msg::GnssInsOrientationStamped` | orientation [click here for more details](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_sensing_msgs) | @@ -27,14 +28,13 @@ The `gnss_poser` is a node that subscribes gnss sensing messages and calculates ### Core Parameters -| Name | Type | Default Value | Description | -| ---------------------- | ------ | ---------------- | -------------------------------------------------------------------------------------------------------- | -| `base_frame` | string | "base_link" | frame id | -| `gnss_frame` | string | "gnss" | frame id | -| `gnss_base_frame` | string | "gnss_base_link" | frame id | -| `map_frame` | string | "map" | frame id | -| `coordinate_system` | int | "4" | coordinate system enumeration; 1: MGRS, 3: WGS84 Local Coordinate System, 4: UTM Local Coordinate System | -| `gnss_pose_pub_method` | int | 0 | 0: Instant Value 1: Average Value 2: Median Value. If 0 is chosen buffer_epoch parameter loses affect. | +| Name | Type | Default Value | Description | +| ---------------------- | ------ | ---------------- | ------------------------------------------------------------------------------------------------------ | +| `base_frame` | string | "base_link" | frame id | +| `gnss_frame` | string | "gnss" | frame id | +| `gnss_base_frame` | string | "gnss_base_link" | frame id | +| `map_frame` | string | "map" | frame id | +| `gnss_pose_pub_method` | int | 0 | 0: Instant Value 1: Average Value 2: Median Value. If 0 is chosen buffer_epoch parameter loses affect. | ## Assumptions / Known limits diff --git a/sensing/gnss_poser/config/gnss_poser.param.yaml b/sensing/gnss_poser/config/gnss_poser.param.yaml new file mode 100644 index 0000000000000..30be98bba8cf1 --- /dev/null +++ b/sensing/gnss_poser/config/gnss_poser.param.yaml @@ -0,0 +1,9 @@ +/**: + ros__parameters: + base_frame: base_link + gnss_base_frame: gnss_base_link + gnss_frame: gnss + map_frame: map + buff_epoch: 1 + use_gnss_ins_orientation: true + gnss_pose_pub_method: 0 diff --git a/sensing/gnss_poser/config/set_local_origin.param.yaml b/sensing/gnss_poser/config/set_local_origin.param.yaml deleted file mode 100644 index e931e5adceb68..0000000000000 --- a/sensing/gnss_poser/config/set_local_origin.param.yaml +++ /dev/null @@ -1,11 +0,0 @@ -/**: - ros__parameters: - - # Latitude of local origin - latitude: 40.81187906 - - # Longitude of local origin - longitude: 29.35810110 - - # Altitude of local origin - altitude: 47.62 diff --git a/sensing/gnss_poser/include/gnss_poser/convert.hpp b/sensing/gnss_poser/include/gnss_poser/convert.hpp deleted file mode 100644 index 5c1f621626aa2..0000000000000 --- a/sensing/gnss_poser/include/gnss_poser/convert.hpp +++ /dev/null @@ -1,182 +0,0 @@ -// 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. -#ifndef GNSS_POSER__CONVERT_HPP_ -#define GNSS_POSER__CONVERT_HPP_ - -#include "gnss_poser/gnss_stat.hpp" - -#include -#include -#include -#include -#include - -#include - -#include - -namespace gnss_poser -{ -enum class MGRSPrecision { - _10_KILO_METER = 1, - _1_KILO_METER = 2, - _100_METER = 3, - _10_METER = 4, - _1_METER = 5, - _100_MIllI_METER = 6, - _10_MIllI_METER = 7, - _1_MIllI_METER = 8, - _100MICRO_METER = 9, -}; -// EllipsoidHeight:height above ellipsoid -// OrthometricHeight:height above geoid -double EllipsoidHeight2OrthometricHeight( - const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg, const rclcpp::Logger & logger) -{ - double OrthometricHeight{0.0}; - try { - GeographicLib::Geoid egm2008("egm2008-1"); - OrthometricHeight = egm2008.ConvertHeight( - nav_sat_fix_msg.latitude, nav_sat_fix_msg.longitude, nav_sat_fix_msg.altitude, - GeographicLib::Geoid::ELLIPSOIDTOGEOID); - } catch (const GeographicLib::GeographicErr & err) { - RCLCPP_ERROR_STREAM( - logger, "Failed to convert Height from Ellipsoid to Orthometric" << err.what()); - } - return OrthometricHeight; -} -GNSSStat NavSatFix2LocalCartesianWGS84( - const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg, - sensor_msgs::msg::NavSatFix nav_sat_fix_origin_, const rclcpp::Logger & logger) -{ - GNSSStat local_cartesian; - - try { - GeographicLib::LocalCartesian localCartesian_origin( - nav_sat_fix_origin_.latitude, nav_sat_fix_origin_.longitude, nav_sat_fix_origin_.altitude); - localCartesian_origin.Forward( - nav_sat_fix_msg.latitude, nav_sat_fix_msg.longitude, nav_sat_fix_msg.altitude, - local_cartesian.x, local_cartesian.y, local_cartesian.z); - - local_cartesian.latitude = nav_sat_fix_msg.latitude; - local_cartesian.longitude = nav_sat_fix_msg.longitude; - local_cartesian.altitude = nav_sat_fix_msg.altitude; - } catch (const GeographicLib::GeographicErr & err) { - RCLCPP_ERROR_STREAM(logger, "Failed to convert NavSatFix to LocalCartesian" << err.what()); - } - return local_cartesian; -} -GNSSStat NavSatFix2UTM( - const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg, const rclcpp::Logger & logger, - int height_system) -{ - GNSSStat utm; - - try { - GeographicLib::UTMUPS::Forward( - nav_sat_fix_msg.latitude, nav_sat_fix_msg.longitude, utm.zone, utm.east_north_up, utm.x, - utm.y); - if (height_system == 0) { - utm.z = EllipsoidHeight2OrthometricHeight(nav_sat_fix_msg, logger); - } else { - utm.z = nav_sat_fix_msg.altitude; - } - utm.latitude = nav_sat_fix_msg.latitude; - utm.longitude = nav_sat_fix_msg.longitude; - utm.altitude = nav_sat_fix_msg.altitude; - } catch (const GeographicLib::GeographicErr & err) { - RCLCPP_ERROR_STREAM(logger, "Failed to convert from LLH to UTM" << err.what()); - } - return utm; -} -GNSSStat NavSatFix2LocalCartesianUTM( - const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg, - sensor_msgs::msg::NavSatFix nav_sat_fix_origin, const rclcpp::Logger & logger, int height_system) -{ - GNSSStat utm_local; - try { - // origin of the local coordinate system in global frame - GNSSStat utm_origin; - GeographicLib::UTMUPS::Forward( - nav_sat_fix_origin.latitude, nav_sat_fix_origin.longitude, utm_origin.zone, - utm_origin.east_north_up, utm_origin.x, utm_origin.y); - if (height_system == 0) { - utm_origin.z = EllipsoidHeight2OrthometricHeight(nav_sat_fix_origin, logger); - } else { - utm_origin.z = nav_sat_fix_origin.altitude; - } - - // individual coordinates of global coordinate system - double global_x = 0.0; - double global_y = 0.0; - GeographicLib::UTMUPS::Forward( - nav_sat_fix_msg.latitude, nav_sat_fix_msg.longitude, utm_origin.zone, - utm_origin.east_north_up, global_x, global_y); - utm_local.latitude = nav_sat_fix_msg.latitude; - utm_local.longitude = nav_sat_fix_msg.longitude; - utm_local.altitude = nav_sat_fix_msg.altitude; - // individual coordinates of local coordinate system - utm_local.x = global_x - utm_origin.x; - utm_local.y = global_y - utm_origin.y; - if (height_system == 0) { - utm_local.z = EllipsoidHeight2OrthometricHeight(nav_sat_fix_msg, logger) - utm_origin.z; - } else { - utm_local.z = nav_sat_fix_msg.altitude - utm_origin.z; - } - } catch (const GeographicLib::GeographicErr & err) { - RCLCPP_ERROR_STREAM( - logger, "Failed to convert from LLH to UTM in local coordinates" << err.what()); - } - return utm_local; -} -GNSSStat UTM2MGRS( - const GNSSStat & utm, const MGRSPrecision & precision, const rclcpp::Logger & logger) -{ - constexpr int GZD_ID_size = 5; // size of header like "53SPU" - - GNSSStat mgrs = utm; - try { - std::string mgrs_code; - GeographicLib::MGRS::Forward( - utm.zone, utm.east_north_up, utm.x, utm.y, utm.latitude, static_cast(precision), - mgrs_code); - mgrs.mgrs_zone = std::string(mgrs_code.substr(0, GZD_ID_size)); - mgrs.x = std::stod(mgrs_code.substr(GZD_ID_size, static_cast(precision))) * - std::pow( - 10, static_cast(MGRSPrecision::_1_METER) - - static_cast(precision)); // set unit as [m] - mgrs.y = std::stod(mgrs_code.substr( - GZD_ID_size + static_cast(precision), static_cast(precision))) * - std::pow( - 10, static_cast(MGRSPrecision::_1_METER) - - static_cast(precision)); // set unit as [m] - mgrs.z = utm.z; // TODO(ryo.watanabe) - } catch (const GeographicLib::GeographicErr & err) { - RCLCPP_ERROR_STREAM(logger, "Failed to convert from UTM to MGRS" << err.what()); - } - return mgrs; -} - -GNSSStat NavSatFix2MGRS( - const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg, const MGRSPrecision & precision, - const rclcpp::Logger & logger, int height_system) -{ - const auto utm = NavSatFix2UTM(nav_sat_fix_msg, logger, height_system); - const auto mgrs = UTM2MGRS(utm, precision, logger); - return mgrs; -} - -} // namespace gnss_poser - -#endif // GNSS_POSER__CONVERT_HPP_ diff --git a/sensing/gnss_poser/include/gnss_poser/gnss_poser_core.hpp b/sensing/gnss_poser/include/gnss_poser/gnss_poser_core.hpp index 4baaec2dfe80e..2118d33bc4b30 100644 --- a/sensing/gnss_poser/include/gnss_poser/gnss_poser_core.hpp +++ b/sensing/gnss_poser/include/gnss_poser/gnss_poser_core.hpp @@ -14,9 +14,8 @@ #ifndef GNSS_POSER__GNSS_POSER_CORE_HPP_ #define GNSS_POSER__GNSS_POSER_CORE_HPP_ -#include "gnss_poser/convert.hpp" -#include "gnss_poser/gnss_stat.hpp" - +#include +#include #include #include @@ -48,16 +47,15 @@ class GNSSPoser : public rclcpp::Node explicit GNSSPoser(const rclcpp::NodeOptions & node_options); private: + using MapProjectorInfo = map_interface::MapProjectorInfo; + + void callbackMapProjectorInfo(const MapProjectorInfo::Message::ConstSharedPtr msg); void callbackNavSatFix(const sensor_msgs::msg::NavSatFix::ConstSharedPtr nav_sat_fix_msg_ptr); void callbackGnssInsOrientationStamped( const autoware_sensing_msgs::msg::GnssInsOrientationStamped::ConstSharedPtr msg); bool isFixed(const sensor_msgs::msg::NavSatStatus & nav_sat_status_msg); bool canGetCovariance(const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg); - GNSSStat convert( - const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg, CoordinateSystem coordinate_system, - int height_system); - geometry_msgs::msg::Point getPosition(const GNSSStat & gnss_stat); geometry_msgs::msg::Point getMedianPosition( const boost::circular_buffer & position_buffer); geometry_msgs::msg::Point getAveragePosition( @@ -81,6 +79,7 @@ class GNSSPoser : public rclcpp::Node tf2_ros::TransformListener tf2_listener_; tf2_ros::TransformBroadcaster tf2_broadcaster_; + component_interface_utils::Subscription::SharedPtr sub_map_projector_info_; rclcpp::Subscription::SharedPtr nav_sat_fix_sub_; rclcpp::Subscription::SharedPtr autoware_orientation_sub_; @@ -89,20 +88,18 @@ class GNSSPoser : public rclcpp::Node rclcpp::Publisher::SharedPtr pose_cov_pub_; rclcpp::Publisher::SharedPtr fixed_pub_; - CoordinateSystem coordinate_system_; + MapProjectorInfo::Message projector_info_; std::string base_frame_; std::string gnss_frame_; std::string gnss_base_frame_; std::string map_frame_; - - sensor_msgs::msg::NavSatFix nav_sat_fix_origin_; + bool received_map_projector_info_ = false; bool use_gnss_ins_orientation_; boost::circular_buffer position_buffer_; autoware_sensing_msgs::msg::GnssInsOrientationStamped::SharedPtr msg_gnss_ins_orientation_stamped_; - int height_system_; int gnss_pose_pub_method; }; } // namespace gnss_poser diff --git a/sensing/gnss_poser/include/gnss_poser/gnss_stat.hpp b/sensing/gnss_poser/include/gnss_poser/gnss_stat.hpp deleted file mode 100644 index fa17bf0e6e232..0000000000000 --- a/sensing/gnss_poser/include/gnss_poser/gnss_stat.hpp +++ /dev/null @@ -1,50 +0,0 @@ -// 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. -#ifndef GNSS_POSER__GNSS_STAT_HPP_ -#define GNSS_POSER__GNSS_STAT_HPP_ - -#include - -namespace gnss_poser -{ -enum class CoordinateSystem { MGRS = 1, LOCAL_CARTESIAN_WGS84 = 3, LOCAL_CARTESIAN_UTM = 4 }; - -struct GNSSStat -{ - GNSSStat() - : east_north_up(true), - zone(0), - mgrs_zone(""), - x(0), - y(0), - z(0), - latitude(0), - longitude(0), - altitude(0) - { - } - - bool east_north_up; - int zone; - std::string mgrs_zone; - double x; - double y; - double z; - double latitude; - double longitude; - double altitude; -}; -} // namespace gnss_poser - -#endif // GNSS_POSER__GNSS_STAT_HPP_ diff --git a/sensing/gnss_poser/launch/gnss_poser.launch.xml b/sensing/gnss_poser/launch/gnss_poser.launch.xml index 3883a492358d6..d9b850e6995ae 100755 --- a/sensing/gnss_poser/launch/gnss_poser.launch.xml +++ b/sensing/gnss_poser/launch/gnss_poser.launch.xml @@ -1,24 +1,14 @@ + + - - - - - - - - - - - - @@ -26,16 +16,6 @@ - - - - - - - - - - diff --git a/sensing/gnss_poser/package.xml b/sensing/gnss_poser/package.xml index 1488f329edecb..5bd75020985da 100644 --- a/sensing/gnss_poser/package.xml +++ b/sensing/gnss_poser/package.xml @@ -5,6 +5,7 @@ 1.0.0 The ROS 2 gnss_poser package Yamato Ando + Koji Minoda Apache License 2.0 Ryo Watanabe @@ -14,7 +15,11 @@ libboost-dev autoware_sensing_msgs + component_interface_specs + component_interface_utils + geographic_msgs geographiclib + geography_utils geometry_msgs rclcpp rclcpp_components diff --git a/sensing/gnss_poser/src/gnss_poser_core.cpp b/sensing/gnss_poser/src/gnss_poser_core.cpp index 7d244ce188eee..3a18dca815f12 100644 --- a/sensing/gnss_poser/src/gnss_poser_core.cpp +++ b/sensing/gnss_poser/src/gnss_poser_core.cpp @@ -14,6 +14,9 @@ #include "gnss_poser/gnss_poser_core.hpp" +#include +#include + #include #include @@ -27,25 +30,22 @@ GNSSPoser::GNSSPoser(const rclcpp::NodeOptions & node_options) : rclcpp::Node("gnss_poser", node_options), tf2_listener_(tf2_buffer_), tf2_broadcaster_(*this), - base_frame_(declare_parameter("base_frame", "base_link")), - 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_ins_orientation", true)), + base_frame_(declare_parameter("base_frame")), + gnss_frame_(declare_parameter("gnss_frame")), + gnss_base_frame_(declare_parameter("gnss_base_frame")), + map_frame_(declare_parameter("map_frame")), + use_gnss_ins_orientation_(declare_parameter("use_gnss_ins_orientation")), msg_gnss_ins_orientation_stamped_( std::make_shared()), - height_system_(declare_parameter("height_system", 1)), - gnss_pose_pub_method(declare_parameter("gnss_pose_pub_method", 0)) + gnss_pose_pub_method(declare_parameter("gnss_pose_pub_method")) { - int coordinate_system = - declare_parameter("coordinate_system", static_cast(CoordinateSystem::MGRS)); - coordinate_system_ = static_cast(coordinate_system); - - nav_sat_fix_origin_.latitude = declare_parameter("latitude", 0.0); - nav_sat_fix_origin_.longitude = declare_parameter("longitude", 0.0); - nav_sat_fix_origin_.altitude = declare_parameter("altitude", 0.0); + // Subscribe to map_projector_info topic + const auto adaptor = component_interface_utils::NodeAdaptor(this); + adaptor.init_sub( + sub_map_projector_info_, + [this](const MapProjectorInfo::Message::ConstSharedPtr msg) { callbackMapProjectorInfo(msg); }); - int buff_epoch = declare_parameter("buff_epoch", 1); + int buff_epoch = declare_parameter("buff_epoch"); position_buffer_.set_capacity(buff_epoch); nav_sat_fix_sub_ = create_subscription( @@ -61,9 +61,24 @@ GNSSPoser::GNSSPoser(const rclcpp::NodeOptions & node_options) fixed_pub_ = create_publisher("gnss_fixed", rclcpp::QoS{1}); } +void GNSSPoser::callbackMapProjectorInfo(const MapProjectorInfo::Message::ConstSharedPtr msg) +{ + projector_info_ = *msg; + received_map_projector_info_ = true; +} + void GNSSPoser::callbackNavSatFix( const sensor_msgs::msg::NavSatFix::ConstSharedPtr nav_sat_fix_msg_ptr) { + // Return immediately if map_projector_info has not been received yet. + if (!received_map_projector_info_) { + RCLCPP_WARN_THROTTLE( + this->get_logger(), *this->get_clock(), std::chrono::milliseconds(1000).count(), + "map_projector_info has not been received yet. Check if the map_projection_loader is " + "successfully launched."); + return; + } + // check fixed topic const bool is_fixed = isFixed(nav_sat_fix_msg_ptr->status); @@ -80,9 +95,15 @@ void GNSSPoser::callbackNavSatFix( return; } - // get position in coordinate_system - const auto gnss_stat = convert(*nav_sat_fix_msg_ptr, coordinate_system_, height_system_); - const auto position = getPosition(gnss_stat); + // get position + geographic_msgs::msg::GeoPoint gps_point; + gps_point.latitude = nav_sat_fix_msg_ptr->latitude; + gps_point.longitude = nav_sat_fix_msg_ptr->longitude; + gps_point.altitude = nav_sat_fix_msg_ptr->altitude; + geometry_msgs::msg::Point position = geography_utils::project_forward(gps_point, projector_info_); + position.z = geography_utils::convert_height( + position.z, gps_point.latitude, gps_point.longitude, MapProjectorInfo::Message::WGS84, + projector_info_.vertical_datum); geometry_msgs::msg::Pose gnss_antenna_pose{}; @@ -118,11 +139,11 @@ void GNSSPoser::callbackNavSatFix( tf2::Transform tf_map2gnss_antenna{}; tf2::fromMsg(gnss_antenna_pose, tf_map2gnss_antenna); - // get TF from base_link to gnss_antenna + // get TF from gnss_antenna to base_link auto tf_gnss_antenna2base_link_msg_ptr = std::make_shared(); getStaticTransform( - gnss_frame_, base_frame_, tf_gnss_antenna2base_link_msg_ptr, nav_sat_fix_msg_ptr->header.stamp); + base_frame_, gnss_frame_, tf_gnss_antenna2base_link_msg_ptr, nav_sat_fix_msg_ptr->header.stamp); tf2::Transform tf_gnss_antenna2base_link{}; tf2::fromMsg(tf_gnss_antenna2base_link_msg_ptr->transform, tf_gnss_antenna2base_link); @@ -185,37 +206,6 @@ bool GNSSPoser::canGetCovariance(const sensor_msgs::msg::NavSatFix & nav_sat_fix sensor_msgs::msg::NavSatFix::COVARIANCE_TYPE_UNKNOWN; } -GNSSStat GNSSPoser::convert( - const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg, CoordinateSystem coordinate_system, - int height_system) -{ - GNSSStat gnss_stat; - if (coordinate_system == CoordinateSystem::LOCAL_CARTESIAN_UTM) { - gnss_stat = NavSatFix2LocalCartesianUTM( - nav_sat_fix_msg, nav_sat_fix_origin_, this->get_logger(), height_system); - } else if (coordinate_system == CoordinateSystem::MGRS) { - gnss_stat = NavSatFix2MGRS( - nav_sat_fix_msg, MGRSPrecision::_100MICRO_METER, this->get_logger(), height_system); - } else if (coordinate_system == CoordinateSystem::LOCAL_CARTESIAN_WGS84) { - gnss_stat = - NavSatFix2LocalCartesianWGS84(nav_sat_fix_msg, nav_sat_fix_origin_, this->get_logger()); - } else { - RCLCPP_ERROR_STREAM_THROTTLE( - this->get_logger(), *this->get_clock(), std::chrono::milliseconds(1000).count(), - "Unknown Coordinate System"); - } - return gnss_stat; -} - -geometry_msgs::msg::Point GNSSPoser::getPosition(const GNSSStat & gnss_stat) -{ - geometry_msgs::msg::Point point; - point.x = gnss_stat.x; - point.y = gnss_stat.y; - point.z = gnss_stat.z; - return point; -} - geometry_msgs::msg::Point GNSSPoser::getMedianPosition( const boost::circular_buffer & position_buffer) { diff --git a/sensing/imu_corrector/CMakeLists.txt b/sensing/imu_corrector/CMakeLists.txt index 31e1727b1e034..ded596a8a9898 100644 --- a/sensing/imu_corrector/CMakeLists.txt +++ b/sensing/imu_corrector/CMakeLists.txt @@ -4,16 +4,36 @@ project(imu_corrector) find_package(autoware_cmake REQUIRED) autoware_package() -ament_auto_add_library(imu_corrector_node SHARED +ament_auto_add_library(gyro_bias_estimation_module SHARED + src/gyro_bias_estimation_module.cpp +) + +ament_auto_add_executable(imu_corrector src/imu_corrector_core.cpp - include/imu_corrector/imu_corrector_core.hpp + src/imu_corrector_node.cpp ) -rclcpp_components_register_node(imu_corrector_node - PLUGIN "imu_corrector::ImuCorrector" - EXECUTABLE imu_corrector +ament_auto_add_executable(gyro_bias_estimator + src/gyro_bias_estimator.cpp + src/gyro_bias_estimator_node.cpp ) +target_link_libraries(gyro_bias_estimator gyro_bias_estimation_module) + +function(add_testcase filepath) + get_filename_component(filename ${filepath} NAME) + string(REGEX REPLACE ".cpp" "" test_name ${filename}) + ament_add_gmock(${test_name} ${filepath}) + target_link_libraries("${test_name}" gyro_bias_estimation_module) + ament_target_dependencies(${test_name} ${${PROJECT_NAME}_FOUND_BUILD_DEPENDS}) +endfunction() + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() + add_testcase(test/test_gyro_bias_estimation_module.cpp) +endif() + ament_auto_package(INSTALL_TO_SHARE launch config diff --git a/sensing/imu_corrector/README.md b/sensing/imu_corrector/README.md index 8a00c4005ece2..7af82c1129aff 100644 --- a/sensing/imu_corrector/README.md +++ b/sensing/imu_corrector/README.md @@ -1,6 +1,6 @@ # imu_corrector -## Purpose +## imu_corrector `imu_corrector_node` is a node that correct imu data. @@ -19,8 +19,6 @@ We also assume that $n\sim\mathcal{N}(0, \sigma^2)$. -## Inputs / Outputs - ### Input | Name | Type | Description | @@ -33,9 +31,7 @@ We also assume that $n\sim\mathcal{N}(0, \sigma^2)$. | --------- | ----------------------- | ------------------ | | `~output` | `sensor_msgs::msg::Imu` | corrected imu data | -## Parameters - -### Core Parameters +### Parameters | Name | Type | Description | | ---------------------------- | ------ | ------------------------------------------------ | @@ -47,12 +43,38 @@ We also assume that $n\sim\mathcal{N}(0, \sigma^2)$. | `angular_velocity_stddev_zz` | double | yaw rate standard deviation imu_link [rad/s] | | `acceleration_stddev` | double | acceleration standard deviation imu_link [m/s^2] | -## Assumptions / Known limits +## gyro_bias_estimator + +`gyro_bias_validator` is a node that validates the bias of the gyroscope. It subscribes to the `sensor_msgs::msg::Imu` topic and validate if the bias of the gyroscope is within the specified range. + +Note that the node calculates bias from the gyroscope data by averaging the data only when the vehicle is stopped. + +### Input -## (Optional) Error detection and handling +| Name | Type | Description | +| ----------------- | ----------------------------------------------- | ---------------- | +| `~/input/imu_raw` | `sensor_msgs::msg::Imu` | **raw** imu data | +| `~/input/pose` | `geometry_msgs::msg::PoseWithCovarianceStamped` | ndt pose | + +Note that the input pose is assumed to be accurate enough. For example when using NDT, we assume that the NDT is appropriately converged. + +Currently, it is possible to use methods other than NDT as a `pose_source` for Autoware, but less accurate methods are not suitable for IMU bias estimation. + +In the future, with careful implementation for pose errors, the IMU bias estimated by NDT could potentially be used not only for validation but also for online calibration. + +### Output -## (Optional) Performance characterization +| Name | Type | Description | +| -------------------- | ------------------------------------ | ----------------------------- | +| `~/output/gyro_bias` | `geometry_msgs::msg::Vector3Stamped` | bias of the gyroscope [rad/s] | -## (Optional) References/External links +### Parameters -## (Optional) Future extensions / Unimplemented parts +| Name | Type | Description | +| ------------------------------------- | ------ | ------------------------------------------------------------------------------------------- | +| `angular_velocity_offset_x` | double | roll rate offset in imu_link [rad/s] | +| `angular_velocity_offset_y` | double | pitch rate offset imu_link [rad/s] | +| `angular_velocity_offset_z` | double | yaw rate offset imu_link [rad/s] | +| `gyro_bias_threshold` | double | threshold of the bias of the gyroscope [rad/s] | +| `timer_callback_interval_sec` | double | seconds about the timer callback function [sec] | +| `straight_motion_ang_vel_upper_limit` | double | upper limit of yaw angular velocity, beyond which motion is not considered straight [rad/s] | diff --git a/sensing/imu_corrector/config/gyro_bias_estimator.param.yaml b/sensing/imu_corrector/config/gyro_bias_estimator.param.yaml new file mode 100644 index 0000000000000..e10329c920137 --- /dev/null +++ b/sensing/imu_corrector/config/gyro_bias_estimator.param.yaml @@ -0,0 +1,5 @@ +/**: + ros__parameters: + gyro_bias_threshold: 0.0015 # [rad/s] + timer_callback_interval_sec: 0.5 # [sec] + straight_motion_ang_vel_upper_limit: 0.015 # [rad/s] diff --git a/sensing/imu_corrector/launch/gyro_bias_estimator.launch.xml b/sensing/imu_corrector/launch/gyro_bias_estimator.launch.xml new file mode 100644 index 0000000000000..e16ccef446aba --- /dev/null +++ b/sensing/imu_corrector/launch/gyro_bias_estimator.launch.xml @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + diff --git a/sensing/imu_corrector/package.xml b/sensing/imu_corrector/package.xml index 07c6cfd9a570d..3dff79bbf9f7f 100644 --- a/sensing/imu_corrector/package.xml +++ b/sensing/imu_corrector/package.xml @@ -13,13 +13,17 @@ ament_cmake_auto autoware_cmake + diagnostic_updater rclcpp rclcpp_components sensor_msgs tf2 + tf2_geometry_msgs tf2_ros tier4_autoware_utils + ament_cmake_gmock + ament_cmake_gtest ament_lint_auto autoware_lint_common diff --git a/sensing/imu_corrector/src/gyro_bias_estimation_module.cpp b/sensing/imu_corrector/src/gyro_bias_estimation_module.cpp new file mode 100644 index 0000000000000..9b5719de69524 --- /dev/null +++ b/sensing/imu_corrector/src/gyro_bias_estimation_module.cpp @@ -0,0 +1,119 @@ +// Copyright 2023 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 "gyro_bias_estimation_module.hpp" + +#include "tier4_autoware_utils/geometry/geometry.hpp" + +#include + +namespace imu_corrector +{ + +/** + * @brief perform dead reckoning based on "gyro_list" and return a relative pose (in RPY) + */ +geometry_msgs::msg::Vector3 integrate_orientation( + const std::vector & gyro_list, + const geometry_msgs::msg::Vector3 & gyro_bias) +{ + geometry_msgs::msg::Vector3 d_rpy{}; + double t_prev = rclcpp::Time(gyro_list.front().header.stamp).seconds(); + for (std::size_t i = 0; i < gyro_list.size() - 1; ++i) { + double t_cur = rclcpp::Time(gyro_list[i + 1].header.stamp).seconds(); + + d_rpy.x += (t_cur - t_prev) * (gyro_list[i].vector.x - gyro_bias.x); + d_rpy.y += (t_cur - t_prev) * (gyro_list[i].vector.y - gyro_bias.y); + d_rpy.z += (t_cur - t_prev) * (gyro_list[i].vector.z - gyro_bias.z); + + t_prev = t_cur; + } + return d_rpy; +} + +/** + * @brief calculate RPY error on dead-reckoning (calculated from "gyro_list") compared to the + * ground-truth pose from "pose_list". + */ +geometry_msgs::msg::Vector3 calculate_error_rpy( + const std::vector & pose_list, + const std::vector & gyro_list, + const geometry_msgs::msg::Vector3 & gyro_bias) +{ + const geometry_msgs::msg::Vector3 rpy_0 = + tier4_autoware_utils::getRPY(pose_list.front().pose.orientation); + const geometry_msgs::msg::Vector3 rpy_1 = + tier4_autoware_utils::getRPY(pose_list.back().pose.orientation); + const geometry_msgs::msg::Vector3 d_rpy = integrate_orientation(gyro_list, gyro_bias); + + geometry_msgs::msg::Vector3 error_rpy; + error_rpy.x = tier4_autoware_utils::normalizeRadian(-rpy_1.x + rpy_0.x + d_rpy.x); + error_rpy.y = tier4_autoware_utils::normalizeRadian(-rpy_1.y + rpy_0.y + d_rpy.y); + error_rpy.z = tier4_autoware_utils::normalizeRadian(-rpy_1.z + rpy_0.z + d_rpy.z); + return error_rpy; +} + +/** + * @brief update gyroscope bias based on a given trajectory data + */ +void GyroBiasEstimationModule::update_bias( + const std::vector & pose_list, + const std::vector & gyro_list) +{ + const double dt_pose = + (rclcpp::Time(pose_list.back().header.stamp) - rclcpp::Time(pose_list.front().header.stamp)) + .seconds(); + const double dt_gyro = + (rclcpp::Time(gyro_list.back().header.stamp) - rclcpp::Time(gyro_list.front().header.stamp)) + .seconds(); + if (dt_pose == 0 || dt_gyro == 0) { + throw std::runtime_error("dt_pose or dt_gyro is zero"); + } + + auto error_rpy = calculate_error_rpy(pose_list, gyro_list, geometry_msgs::msg::Vector3{}); + error_rpy.x *= dt_pose / dt_gyro; + error_rpy.y *= dt_pose / dt_gyro; + error_rpy.z *= dt_pose / dt_gyro; + + gyro_bias_pair_.first.x += dt_pose * error_rpy.x; + gyro_bias_pair_.first.y += dt_pose * error_rpy.y; + gyro_bias_pair_.first.z += dt_pose * error_rpy.z; + gyro_bias_pair_.second.x += dt_pose * dt_pose; + gyro_bias_pair_.second.y += dt_pose * dt_pose; + gyro_bias_pair_.second.z += dt_pose * dt_pose; + + geometry_msgs::msg::Vector3 gyro_bias; + gyro_bias.x = error_rpy.x / dt_pose; + gyro_bias.y = error_rpy.y / dt_pose; + gyro_bias.z = error_rpy.z / dt_pose; +} + +/** + * @brief getter function for current estimated bias + */ +geometry_msgs::msg::Vector3 GyroBiasEstimationModule::get_bias_base_link() const +{ + geometry_msgs::msg::Vector3 gyro_bias_base; + if ( + gyro_bias_pair_.second.x == 0 || gyro_bias_pair_.second.y == 0 || + gyro_bias_pair_.second.z == 0) { + throw std::runtime_error("gyro_bias_pair_.second is zero"); + } + gyro_bias_base.x = gyro_bias_pair_.first.x / gyro_bias_pair_.second.x; + gyro_bias_base.y = gyro_bias_pair_.first.y / gyro_bias_pair_.second.y; + gyro_bias_base.z = gyro_bias_pair_.first.z / gyro_bias_pair_.second.z; + return gyro_bias_base; +} + +} // namespace imu_corrector diff --git a/sensing/imu_corrector/src/gyro_bias_estimation_module.hpp b/sensing/imu_corrector/src/gyro_bias_estimation_module.hpp new file mode 100644 index 0000000000000..dfa152d32c0d9 --- /dev/null +++ b/sensing/imu_corrector/src/gyro_bias_estimation_module.hpp @@ -0,0 +1,41 @@ +// Copyright 2023 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 GYRO_BIAS_ESTIMATION_MODULE_HPP_ +#define GYRO_BIAS_ESTIMATION_MODULE_HPP_ + +#include +#include + +#include +#include +#include + +namespace imu_corrector +{ +class GyroBiasEstimationModule +{ +public: + GyroBiasEstimationModule() = default; + void update_bias( + const std::vector & pose_list, + const std::vector & gyro_list); + geometry_msgs::msg::Vector3 get_bias_base_link() const; + +private: + std::pair gyro_bias_pair_; +}; +} // namespace imu_corrector + +#endif // GYRO_BIAS_ESTIMATION_MODULE_HPP_ diff --git a/sensing/imu_corrector/src/gyro_bias_estimator.cpp b/sensing/imu_corrector/src/gyro_bias_estimator.cpp new file mode 100644 index 0000000000000..50e4a702ec949 --- /dev/null +++ b/sensing/imu_corrector/src/gyro_bias_estimator.cpp @@ -0,0 +1,207 @@ +// Copyright 2023 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 "gyro_bias_estimator.hpp" + +#include "tier4_autoware_utils/geometry/geometry.hpp" + +#include +#include + +#include + +namespace imu_corrector +{ +GyroBiasEstimator::GyroBiasEstimator() +: Node("gyro_bias_validator"), + gyro_bias_threshold_(declare_parameter("gyro_bias_threshold")), + angular_velocity_offset_x_(declare_parameter("angular_velocity_offset_x")), + angular_velocity_offset_y_(declare_parameter("angular_velocity_offset_y")), + angular_velocity_offset_z_(declare_parameter("angular_velocity_offset_z")), + timer_callback_interval_sec_(declare_parameter("timer_callback_interval_sec")), + straight_motion_ang_vel_upper_limit_( + declare_parameter("straight_motion_ang_vel_upper_limit")), + updater_(this), + gyro_bias_(std::nullopt) +{ + updater_.setHardwareID(get_name()); + updater_.add("gyro_bias_validator", this, &GyroBiasEstimator::update_diagnostics); + + gyro_bias_estimation_module_ = std::make_unique(); + + imu_sub_ = create_subscription( + "~/input/imu_raw", rclcpp::SensorDataQoS(), + [this](const Imu::ConstSharedPtr msg) { callback_imu(msg); }); + odom_sub_ = create_subscription( + "~/input/odom", rclcpp::SensorDataQoS(), + [this](const Odometry::ConstSharedPtr msg) { callback_odom(msg); }); + gyro_bias_pub_ = create_publisher("~/output/gyro_bias", rclcpp::SensorDataQoS()); + + auto timer_callback = std::bind(&GyroBiasEstimator::timer_callback, this); + auto period_control = std::chrono::duration_cast( + std::chrono::duration(timer_callback_interval_sec_)); + timer_ = std::make_shared>( + this->get_clock(), period_control, std::move(timer_callback), + this->get_node_base_interface()->get_context()); + this->get_node_timers_interface()->add_timer(timer_, nullptr); + + transform_listener_ = std::make_shared(this); +} + +void GyroBiasEstimator::callback_imu(const Imu::ConstSharedPtr imu_msg_ptr) +{ + imu_frame_ = imu_msg_ptr->header.frame_id; + geometry_msgs::msg::TransformStamped::ConstSharedPtr tf_imu2base_ptr = + transform_listener_->getLatestTransform(imu_frame_, output_frame_); + if (!tf_imu2base_ptr) { + RCLCPP_ERROR( + this->get_logger(), "Please publish TF %s to %s", output_frame_.c_str(), + (imu_frame_).c_str()); + return; + } + + geometry_msgs::msg::Vector3Stamped gyro; + gyro.header.stamp = imu_msg_ptr->header.stamp; + gyro.vector = transform_vector3(imu_msg_ptr->angular_velocity, *tf_imu2base_ptr); + + gyro_all_.push_back(gyro); + + // Publish results for debugging + if (gyro_bias_ != std::nullopt) { + Vector3Stamped gyro_bias_msg; + + gyro_bias_msg.header.stamp = this->now(); + gyro_bias_msg.vector = gyro_bias_.value(); + + gyro_bias_pub_->publish(gyro_bias_msg); + } +} + +void GyroBiasEstimator::callback_odom(const Odometry::ConstSharedPtr odom_msg_ptr) +{ + geometry_msgs::msg::PoseStamped pose; + pose.header = odom_msg_ptr->header; + pose.pose = odom_msg_ptr->pose.pose; + pose_buf_.push_back(pose); +} + +void GyroBiasEstimator::timer_callback() +{ + if (pose_buf_.empty()) { + return; + } + + // Copy data + const std::vector pose_buf = pose_buf_; + const std::vector gyro_all = gyro_all_; + pose_buf_.clear(); + gyro_all_.clear(); + + // Check time + const rclcpp::Time t0_rclcpp_time = rclcpp::Time(pose_buf.front().header.stamp); + const rclcpp::Time t1_rclcpp_time = rclcpp::Time(pose_buf.back().header.stamp); + if (t1_rclcpp_time <= t0_rclcpp_time) { + return; + } + + // Filter gyro data + std::vector gyro_filtered; + for (const auto & gyro : gyro_all) { + const rclcpp::Time t = rclcpp::Time(gyro.header.stamp); + if (t0_rclcpp_time <= t && t < t1_rclcpp_time) { + gyro_filtered.push_back(gyro); + } + } + + // Check gyro data size + // Data size must be greater than or equal to 2 since the time difference will be taken later + if (gyro_filtered.size() <= 1) { + return; + } + + // Check if the vehicle is moving straight + const geometry_msgs::msg::Vector3 rpy_0 = + tier4_autoware_utils::getRPY(pose_buf.front().pose.orientation); + const geometry_msgs::msg::Vector3 rpy_1 = + tier4_autoware_utils::getRPY(pose_buf.back().pose.orientation); + const double yaw_diff = std::abs(tier4_autoware_utils::normalizeRadian(rpy_1.z - rpy_0.z)); + const double time_diff = (t1_rclcpp_time - t0_rclcpp_time).seconds(); + const double yaw_vel = yaw_diff / time_diff; + const bool is_straight = (yaw_vel < straight_motion_ang_vel_upper_limit_); + if (!is_straight) { + return; + } + + // Calculate gyro bias + gyro_bias_estimation_module_->update_bias(pose_buf, gyro_filtered); + + geometry_msgs::msg::TransformStamped::ConstSharedPtr tf_base2imu_ptr = + transform_listener_->getLatestTransform(output_frame_, imu_frame_); + if (!tf_base2imu_ptr) { + RCLCPP_ERROR( + this->get_logger(), "Please publish TF %s to %s", imu_frame_.c_str(), output_frame_.c_str()); + return; + } + gyro_bias_ = + transform_vector3(gyro_bias_estimation_module_->get_bias_base_link(), *tf_base2imu_ptr); + + updater_.force_update(); +} + +geometry_msgs::msg::Vector3 GyroBiasEstimator::transform_vector3( + const geometry_msgs::msg::Vector3 & vec, const geometry_msgs::msg::TransformStamped & transform) +{ + geometry_msgs::msg::Vector3Stamped vec_stamped; + vec_stamped.vector = vec; + + geometry_msgs::msg::Vector3Stamped vec_stamped_transformed; + tf2::doTransform(vec_stamped, vec_stamped_transformed, transform); + return vec_stamped_transformed.vector; +} + +void GyroBiasEstimator::update_diagnostics(diagnostic_updater::DiagnosticStatusWrapper & stat) +{ + if (gyro_bias_ == std::nullopt) { + stat.add("gyro_bias", "Bias estimation is not yet ready because of insufficient data."); + stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "Not initialized"); + } else { + stat.add("gyro_bias_x_for_imu_corrector", gyro_bias_.value().x); + stat.add("gyro_bias_y_for_imu_corrector", gyro_bias_.value().y); + stat.add("gyro_bias_z_for_imu_corrector", gyro_bias_.value().z); + + stat.add("estimated_gyro_bias_x", gyro_bias_.value().x - angular_velocity_offset_x_); + stat.add("estimated_gyro_bias_y", gyro_bias_.value().y - angular_velocity_offset_y_); + stat.add("estimated_gyro_bias_z", gyro_bias_.value().z - angular_velocity_offset_z_); + + // Validation + const bool is_bias_small_enough = + std::abs(gyro_bias_.value().x - angular_velocity_offset_x_) < gyro_bias_threshold_ && + std::abs(gyro_bias_.value().y - angular_velocity_offset_y_) < gyro_bias_threshold_ && + std::abs(gyro_bias_.value().z - angular_velocity_offset_z_) < gyro_bias_threshold_; + + // Update diagnostics + if (is_bias_small_enough) { + stat.add("gyro_bias", "OK"); + stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "OK"); + } else { + stat.add( + "gyro_bias", + "Gyro bias may be incorrect. Please calibrate IMU and reflect the result in " + "imu_corrector. You may also use the output of gyro_bias_estimator."); + stat.summary(diagnostic_msgs::msg::DiagnosticStatus::WARN, "WARN"); + } + } +} + +} // namespace imu_corrector diff --git a/sensing/imu_corrector/src/gyro_bias_estimator.hpp b/sensing/imu_corrector/src/gyro_bias_estimator.hpp new file mode 100644 index 0000000000000..821cba0b213ff --- /dev/null +++ b/sensing/imu_corrector/src/gyro_bias_estimator.hpp @@ -0,0 +1,86 @@ +// Copyright 2023 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 GYRO_BIAS_ESTIMATOR_HPP_ +#define GYRO_BIAS_ESTIMATOR_HPP_ + +#include "gyro_bias_estimation_module.hpp" +#include "tier4_autoware_utils/ros/transform_listener.hpp" + +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace imu_corrector +{ +class GyroBiasEstimator : public rclcpp::Node +{ +private: + using Imu = sensor_msgs::msg::Imu; + using PoseWithCovarianceStamped = geometry_msgs::msg::PoseWithCovarianceStamped; + using Vector3Stamped = geometry_msgs::msg::Vector3Stamped; + using Vector3 = geometry_msgs::msg::Vector3; + using Odometry = nav_msgs::msg::Odometry; + +public: + GyroBiasEstimator(); + +private: + void update_diagnostics(diagnostic_updater::DiagnosticStatusWrapper & stat); + void callback_imu(const Imu::ConstSharedPtr imu_msg_ptr); + void callback_odom(const Odometry::ConstSharedPtr odom_msg_ptr); + void timer_callback(); + + static geometry_msgs::msg::Vector3 transform_vector3( + const geometry_msgs::msg::Vector3 & vec, + const geometry_msgs::msg::TransformStamped & transform); + + const std::string output_frame_ = "base_link"; + + rclcpp::Subscription::SharedPtr imu_sub_; + rclcpp::Subscription::SharedPtr odom_sub_; + rclcpp::Publisher::SharedPtr gyro_bias_pub_; + rclcpp::TimerBase::SharedPtr timer_; + + std::unique_ptr gyro_bias_estimation_module_; + + const double gyro_bias_threshold_; + const double angular_velocity_offset_x_; + const double angular_velocity_offset_y_; + const double angular_velocity_offset_z_; + const double timer_callback_interval_sec_; + const double straight_motion_ang_vel_upper_limit_; + + diagnostic_updater::Updater updater_; + + std::optional gyro_bias_; + + std::shared_ptr transform_listener_; + + std::string imu_frame_; + + std::vector gyro_all_; + std::vector pose_buf_; +}; +} // namespace imu_corrector + +#endif // GYRO_BIAS_ESTIMATOR_HPP_ diff --git a/sensing/imu_corrector/src/gyro_bias_estimator_node.cpp b/sensing/imu_corrector/src/gyro_bias_estimator_node.cpp new file mode 100644 index 0000000000000..a491957bbb57f --- /dev/null +++ b/sensing/imu_corrector/src/gyro_bias_estimator_node.cpp @@ -0,0 +1,26 @@ +// Copyright 2023 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 "gyro_bias_estimator.hpp" + +#include + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} diff --git a/sensing/imu_corrector/src/imu_corrector_core.cpp b/sensing/imu_corrector/src/imu_corrector_core.cpp index edc82f6d56549..a26c64925729c 100644 --- a/sensing/imu_corrector/src/imu_corrector_core.cpp +++ b/sensing/imu_corrector/src/imu_corrector_core.cpp @@ -12,7 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "imu_corrector/imu_corrector_core.hpp" +#include "imu_corrector_core.hpp" + +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif +#include + +#include std::array transformCovariance(const std::array & cov) { @@ -44,9 +53,8 @@ geometry_msgs::msg::Vector3 transformVector3( namespace imu_corrector { -ImuCorrector::ImuCorrector(const rclcpp::NodeOptions & node_options) -: Node("imu_corrector", node_options), - output_frame_(declare_parameter("base_link", "base_link")) +ImuCorrector::ImuCorrector() +: Node("imu_corrector"), output_frame_(declare_parameter("base_link", "base_link")) { transform_listener_ = std::make_shared(this); @@ -115,6 +123,3 @@ void ImuCorrector::callbackImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_m } } // namespace imu_corrector - -#include -RCLCPP_COMPONENTS_REGISTER_NODE(imu_corrector::ImuCorrector) diff --git a/sensing/imu_corrector/include/imu_corrector/imu_corrector_core.hpp b/sensing/imu_corrector/src/imu_corrector_core.hpp similarity index 86% rename from sensing/imu_corrector/include/imu_corrector/imu_corrector_core.hpp rename to sensing/imu_corrector/src/imu_corrector_core.hpp index 1c20dc6977f61..7709c611ab714 100644 --- a/sensing/imu_corrector/include/imu_corrector/imu_corrector_core.hpp +++ b/sensing/imu_corrector/src/imu_corrector_core.hpp @@ -11,11 +11,11 @@ // 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 IMU_CORRECTOR__IMU_CORRECTOR_CORE_HPP_ -#define IMU_CORRECTOR__IMU_CORRECTOR_CORE_HPP_ +#ifndef IMU_CORRECTOR_CORE_HPP_ +#define IMU_CORRECTOR_CORE_HPP_ +#include "tier4_autoware_utils/ros/msg_covariance.hpp" #include "tier4_autoware_utils/ros/transform_listener.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" #include @@ -34,7 +34,7 @@ class ImuCorrector : public rclcpp::Node using COV_IDX = tier4_autoware_utils::xyz_covariance_index::XYZ_COV_IDX; public: - explicit ImuCorrector(const rclcpp::NodeOptions & node_options); + ImuCorrector(); private: void callbackImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg_ptr); @@ -59,4 +59,4 @@ class ImuCorrector : public rclcpp::Node }; } // namespace imu_corrector -#endif // IMU_CORRECTOR__IMU_CORRECTOR_CORE_HPP_ +#endif // IMU_CORRECTOR_CORE_HPP_ diff --git a/common/tier4_autoware_utils/src/tier4_autoware_utils.cpp b/sensing/imu_corrector/src/imu_corrector_node.cpp similarity index 66% rename from common/tier4_autoware_utils/src/tier4_autoware_utils.cpp rename to sensing/imu_corrector/src/imu_corrector_node.cpp index a541d314aa365..c1df5bee28439 100644 --- a/common/tier4_autoware_utils/src/tier4_autoware_utils.cpp +++ b/sensing/imu_corrector/src/imu_corrector_node.cpp @@ -1,4 +1,4 @@ -// Copyright 2020 Tier IV, Inc. +// Copyright 2023 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. @@ -12,4 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" +#include "imu_corrector_core.hpp" + +#include + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} diff --git a/sensing/imu_corrector/test/test_gyro_bias_estimation_module.cpp b/sensing/imu_corrector/test/test_gyro_bias_estimation_module.cpp new file mode 100644 index 0000000000000..78558feeb7936 --- /dev/null +++ b/sensing/imu_corrector/test/test_gyro_bias_estimation_module.cpp @@ -0,0 +1,89 @@ +// Copyright 2023 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 "../src/gyro_bias_estimation_module.hpp" + +#include + +#include + +namespace imu_corrector +{ +class GyroBiasEstimationModuleTest : public ::testing::Test +{ +protected: + GyroBiasEstimationModule module; +}; + +TEST_F(GyroBiasEstimationModuleTest, GetBiasEstimationWhenVehicleStopped) +{ + std::vector pose_list; + std::vector gyro_list; + geometry_msgs::msg::PoseStamped pose1; + pose1.header.stamp = rclcpp::Time(0); + pose1.pose.orientation.w = 1.0; + pose_list.push_back(pose1); + geometry_msgs::msg::PoseStamped pose2; + pose2.header.stamp = rclcpp::Time(1e9); + pose2.pose.orientation.w = 1.0; + pose_list.push_back(pose2); + + geometry_msgs::msg::Vector3Stamped gyro1; + gyro1.header.stamp = rclcpp::Time(0.25 * 1e9); + gyro1.vector.x = 0.1; + gyro1.vector.y = 0.2; + gyro1.vector.z = 0.3; + gyro_list.push_back(gyro1); + geometry_msgs::msg::Vector3Stamped gyro2; + gyro2.header.stamp = rclcpp::Time(0.5 * 1e9); + gyro2.vector.x = 0.1; + gyro2.vector.y = 0.2; + gyro2.vector.z = 0.3; + gyro_list.push_back(gyro2); + + for (size_t i = 0; i < 10; ++i) { + module.update_bias(pose_list, gyro_list); + } + const geometry_msgs::msg::Vector3 result = module.get_bias_base_link(); + ASSERT_DOUBLE_EQ(result.x, 0.1); + ASSERT_DOUBLE_EQ(result.y, 0.2); + ASSERT_DOUBLE_EQ(result.z, 0.3); +} + +TEST_F(GyroBiasEstimationModuleTest, GetInsufficientDataException) +{ + ASSERT_THROW(module.get_bias_base_link(), std::runtime_error); +} + +TEST_F(GyroBiasEstimationModuleTest, GetInsufficientDataExceptionWhenVehicleMoving) +{ + std::vector pose_list; + std::vector gyro_list; + geometry_msgs::msg::PoseStamped pose1; + pose1.header.stamp = rclcpp::Time(0); + pose1.pose.orientation.w = 1.0; + pose_list.push_back(pose1); + + geometry_msgs::msg::Vector3Stamped gyro1; + gyro1.header.stamp = rclcpp::Time(0); + gyro1.vector.x = 0.1; + gyro1.vector.y = 0.2; + gyro1.vector.z = 0.3; + gyro_list.push_back(gyro1); + + for (size_t i = 0; i < 10; ++i) { + ASSERT_THROW(module.update_bias(pose_list, gyro_list), std::runtime_error); + } +} +} // namespace imu_corrector diff --git a/sensing/pointcloud_preprocessor/docs/blockage_diag.md b/sensing/pointcloud_preprocessor/docs/blockage_diag.md index 519bd831fd39c..32e5a5869109d 100644 --- a/sensing/pointcloud_preprocessor/docs/blockage_diag.md +++ b/sensing/pointcloud_preprocessor/docs/blockage_diag.md @@ -2,11 +2,13 @@ ## Purpose -To ensure the performance of LiDAR and safety for autonomous driving, the abnormal condition diagnostics feature is needed. -LiDAR blockage is abnormal condition of LiDAR when some unwanted objects stitch to and block the light pulses and return signal. +To ensure the performance of LiDAR and safety for autonomous driving, the abnormal condition diagnostics feature is +needed. +LiDAR blockage is abnormal condition of LiDAR when some unwanted objects stitch to and block the light pulses and return +signal. This node's purpose is to detect the existing of blockage on LiDAR and its related size and location. -## Inner-workings / Algorithms +## Inner-workings / Algorithms(Blockage detection) This node bases on the no-return region and its location to decide if it is a blockage. @@ -16,6 +18,14 @@ The logic is showed as below ![blockage_diag_flowchart](./image/blockage_diag_flowchart.drawio.svg) +## Inner-workings /Algorithms(Dust detection) + +About dust detection, morphological processing is implemented. +If the lidar's ray cannot be acquired due to dust in the lidar area where the point cloud is considered to return from +the ground, +black pixels appear as noise in the depth image. +The area of noise is found by erosion and dilation these black pixels. + ## Inputs / Outputs This implementation inherits `pointcloud_preprocessor::Filter` class, please refer [README](../README.md). @@ -28,30 +38,41 @@ This implementation inherits `pointcloud_preprocessor::Filter` class, please ref ### Output -| Name | Type | Description | -| ---------------------------------------------------- | --------------------------------------- | -------------------------------------------------- | -| `~/output/blockage_diag/debug/blockage_mask_image` | `sensor_msgs::msg::Image` | The mask image of detected blockage | -| `~/output/blockage_diag/debug/ground_blockage_ratio` | `tier4_debug_msgs::msg::Float32Stamped` | The area ratio of blockage region in ground region | -| `~/output/blockage_diag/debug/sky_blockage_ratio` | `tier4_debug_msgs::msg::Float32Stamped` | The area ratio of blockage region in sky region | -| `~/output/blockage_diag/debug/lidar_depth_map` | `sensor_msgs::msg::Image` | The depth map image of input point cloud | +| Name | Type | Description | +| :-------------------------------------------------------- | :-------------------------------------- | ------------------------------------------------------------------------------------------------ | +| `~/output/blockage_diag/debug/blockage_mask_image` | `sensor_msgs::msg::Image` | The mask image of detected blockage | +| `~/output/blockage_diag/debug/ground_blockage_ratio` | `tier4_debug_msgs::msg::Float32Stamped` | The area ratio of blockage region in ground region | +| `~/output/blockage_diag/debug/sky_blockage_ratio` | `tier4_debug_msgs::msg::Float32Stamped` | The area ratio of blockage region in sky region | +| `~/output/blockage_diag/debug/lidar_depth_map` | `sensor_msgs::msg::Image` | The depth map image of input point cloud | +| `~/output/blockage_diag/debug/single_frame_dust_mask` | `sensor_msgs::msg::Image` | The mask image of detected dusty area in latest single frame | +| `~/output/blockage_diag/debug/multi_frame_dust_mask` | `sensor_msgs::msg::Image` | The mask image of continuous detected dusty area | +| `~/output/blockage_diag/debug/blockage_dust_merged_image` | `sensor_msgs::msg::Image` | The merged image of blockage detection(red) and multi frame dusty area detection(yellow) results | +| `~/output/blockage_diag/debug/ground_dust_ratio` | `tier4_debug_msgs::msg::Float32Stamped` | The ratio of dusty area divided by area where ray usually returns from the ground. | ## Parameters -| Name | Type | Description | -| -------------------------- | ------ | -------------------------------------------------- | -| `blockage_ratio_threshold` | float | The threshold of blockage area ratio | -| `blockage_count_threshold` | float | The threshold of number continuous blockage frames | -| `horizontal_ring_id` | int | The id of horizontal ring of the LiDAR | -| `angle_range` | vector | The effective range of LiDAR | -| `vertical_bins` | int | The LiDAR channel number | -| `model` | string | The LiDAR model | -| `buffering_frames` | uint | The number of buffering [range:1-200] | -| `buffering_interval` | uint | The interval of buffering | +| Name | Type | Description | +| ----------------------------- | ------ | ----------------------------------------------------------------------------------------------------------------------------- | +| `blockage_ratio_threshold` | float | The threshold of blockage area ratio.If the blockage value exceeds this threshold, the diagnostic state will be set to ERROR. | +| `blockage_count_threshold` | float | The threshold of number continuous blockage frames | +| `horizontal_ring_id` | int | The id of horizontal ring of the LiDAR | +| `angle_range` | vector | The effective range of LiDAR | +| `vertical_bins` | int | The LiDAR channel number | +| `model` | string | The LiDAR model | +| `blockage_buffering_frames` | int | The number of buffering about blockage detection [range:1-200] | +| `blockage_buffering_interval` | int | The interval of buffering about blockage detection | +| `dust_ratio_threshold` | float | The threshold of dusty area ratio | +| `dust_count_threshold` | int | The threshold of number continuous frames include dusty area | +| `dust_kernel_size` | int | The kernel size of morphology processing in dusty area detection | +| `dust_buffering_frames` | int | The number of buffering about dusty area detection [range:1-200] | +| `dust_buffering_interval` | int | The interval of buffering about dusty area detection | ## Assumptions / Known limits -1. Only Hesai Pandar40P and Hesai PandarQT were tested. For a new LiDAR, it is necessary to check order of channel id in vertical distribution manually and modify the code. -2. The current method is still limited for dust type of blockage when dust particles are sparsely distributed. +1. Only Hesai Pandar40P and Hesai PandarQT were tested. For a new LiDAR, it is necessary to check order of channel id in + vertical distribution manually and modify the code. +2. About dusty area detection, False positives occur when there are water puddles on the road surface due to rain, etc. + Also, the area of the ray to the sky is currently undetectable. ## (Optional) Error detection and handling diff --git a/sensing/pointcloud_preprocessor/docs/concatenate-data.md b/sensing/pointcloud_preprocessor/docs/concatenate-data.md index a3b5268815f24..74b4f3df17615 100644 --- a/sensing/pointcloud_preprocessor/docs/concatenate-data.md +++ b/sensing/pointcloud_preprocessor/docs/concatenate-data.md @@ -42,6 +42,7 @@ The figure below represents the reception time of each sensor data and how it is | `timeout_sec` | double | 0.1 | tolerance of time to publish next pointcloud [s]
When this time limit is exceeded, the filter concatenates and publishes pointcloud, even if not all the point clouds are subscribed. | | `input_offset` | vector of double | [] | This parameter can control waiting time for each input sensor pointcloud [s]. You must to set the same length of offsets with input pointclouds numbers.
For its tuning, please see [actual usage page](#how-to-tuning-timeout_sec-and-input_offset). | | `publish_synchronized_pointcloud` | bool | false | If true, publish the time synchronized pointclouds. All input pointclouds are transformed and then re-published as message named `_synchronized`. | +| `input_twist_topic_type` | std::string | twist | Topic type for twist. Currently support `twist` or `odom`. | ## Actual Usage diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/blockage_diag/blockage_diag_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/blockage_diag/blockage_diag_nodelet.hpp index 116dd2f832b93..66e14c418b52c 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/blockage_diag/blockage_diag_nodelet.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/blockage_diag/blockage_diag_nodelet.hpp @@ -53,27 +53,42 @@ class BlockageDiagComponent : public pointcloud_preprocessor::Filter rcl_interfaces::msg::SetParametersResult paramCallback(const std::vector & p); image_transport::Publisher lidar_depth_map_pub_; image_transport::Publisher blockage_mask_pub_; + image_transport::Publisher single_frame_dust_mask_pub; + image_transport::Publisher multi_frame_dust_mask_pub; + image_transport::Publisher blockage_dust_merged_pub; rclcpp::Publisher::SharedPtr ground_blockage_ratio_pub_; rclcpp::Publisher::SharedPtr sky_blockage_ratio_pub_; + rclcpp::Publisher::SharedPtr ground_dust_ratio_pub_; + rclcpp::Publisher::SharedPtr blockage_type_pub_; private: void onBlockageChecker(DiagnosticStatusWrapper & stat); + void dustChecker(DiagnosticStatusWrapper & stat); Updater updater_{this}; - uint vertical_bins_; + int vertical_bins_; std::vector angle_range_deg_; - uint horizontal_ring_id_ = 12; + int horizontal_ring_id_; float blockage_ratio_threshold_; + float dust_ratio_threshold_; float ground_blockage_ratio_ = -1.0f; float sky_blockage_ratio_ = -1.0f; + float ground_dust_ratio_ = -1.0f; std::vector ground_blockage_range_deg_ = {0.0f, 0.0f}; std::vector sky_blockage_range_deg_ = {0.0f, 0.0f}; - uint erode_kernel_ = 10; - uint ground_blockage_count_ = 0; - uint sky_blockage_count_ = 0; - uint blockage_count_threshold_; + int blockage_kernel_ = 10; + int blockage_frame_count_ = 0; + int ground_blockage_count_ = 0; + int sky_blockage_count_ = 0; + int blockage_count_threshold_; std::string lidar_model_; - uint buffering_frames_ = 100; - uint buffering_interval_ = 5; + int blockage_buffering_frames_; + int blockage_buffering_interval_; + int dust_kernel_size_; + int dust_buffering_frames_; + int dust_buffering_interval_; + int dust_buffering_frame_counter_ = 0; + int dust_count_threshold_; + int dust_frame_count_ = 0; public: PCL_MAKE_ALIGNED_OPERATOR_NEW diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_nodelet.hpp index f5d55a2ebcbf7..be96aa94382dd 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_nodelet.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_nodelet.hpp @@ -71,6 +71,7 @@ #include #include #include +#include #include #include #include @@ -88,6 +89,7 @@ namespace pointcloud_preprocessor { using autoware_point_types::PointXYZI; using point_cloud_msg_wrapper::PointCloud2Modifier; + /** \brief @b PointCloudConcatenateDataSynchronizerComponent is a special form of data * synchronizer: it listens for a set of input PointCloud messages on the same topic, * checks their timestamps, and concatenates their fields together into a single @@ -131,10 +133,13 @@ class PointCloudConcatenateDataSynchronizerComponent : public rclcpp::Node std::vector::SharedPtr> filters_; rclcpp::Subscription::SharedPtr sub_twist_; + rclcpp::Subscription::SharedPtr sub_odom_; rclcpp::TimerBase::SharedPtr timer_; diagnostic_updater::Updater updater_{this}; + const std::string input_twist_topic_type_; + /** \brief Output TF frame the concatenated points should be transformed to. */ std::string output_frame_; @@ -170,6 +175,7 @@ class PointCloudConcatenateDataSynchronizerComponent : public rclcpp::Node const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_ptr, const std::string & topic_name); void twist_callback(const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr input); + void odom_callback(const nav_msgs::msg::Odometry::ConstSharedPtr input); void timer_callback(); void checkConcatStatus(diagnostic_updater::DiagnosticStatusWrapper & stat); diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/time_synchronizer/time_synchronizer_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/time_synchronizer/time_synchronizer_nodelet.hpp index f30c02a97a6c0..8c3cb98ba266e 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/time_synchronizer/time_synchronizer_nodelet.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/time_synchronizer/time_synchronizer_nodelet.hpp @@ -68,9 +68,10 @@ #include #include -#include #include #include +#include +#include #include #include #include @@ -127,11 +128,14 @@ class PointCloudDataSynchronizerComponent : public rclcpp::Node /** \brief A vector of subscriber. */ std::vector::SharedPtr> filters_; - rclcpp::Subscription::SharedPtr sub_twist_; + rclcpp::Subscription::SharedPtr sub_twist_; + rclcpp::Subscription::SharedPtr sub_odom_; rclcpp::TimerBase::SharedPtr timer_; diagnostic_updater::Updater updater_{this}; + const std::string input_twist_topic_type_; + /** \brief Output TF frame the concatenated points should be transformed to. */ std::string output_frame_; @@ -168,7 +172,8 @@ class PointCloudDataSynchronizerComponent : public rclcpp::Node void cloud_callback( const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_ptr, const std::string & topic_name); - void twist_callback(const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr input); + void twist_callback(const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr input); + void odom_callback(const nav_msgs::msg::Odometry::ConstSharedPtr input); void timer_callback(); void checkSyncStatus(diagnostic_updater::DiagnosticStatusWrapper & stat); diff --git a/sensing/pointcloud_preprocessor/launch/blockage_diag.launch.xml b/sensing/pointcloud_preprocessor/launch/blockage_diag.launch.xml index 47dc1e73eef12..8fa5796e2802d 100644 --- a/sensing/pointcloud_preprocessor/launch/blockage_diag.launch.xml +++ b/sensing/pointcloud_preprocessor/launch/blockage_diag.launch.xml @@ -2,8 +2,36 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/sensing/pointcloud_preprocessor/launch/preprocessor.launch.py b/sensing/pointcloud_preprocessor/launch/preprocessor.launch.py index 6b1fc0f5bd976..d82c23152aee4 100644 --- a/sensing/pointcloud_preprocessor/launch/preprocessor.launch.py +++ b/sensing/pointcloud_preprocessor/launch/preprocessor.launch.py @@ -38,13 +38,17 @@ def launch_setup(context, *args, **kwargs): package=pkg, plugin="pointcloud_preprocessor::PointCloudConcatenateDataSynchronizerComponent", name="sync_and_concatenate_filter", - remappings=[("output", "points_raw/concatenated")], + remappings=[ + ("~/input/twist", "/sensing/vehicle_velocity_converter/twist_with_covariance"), + ("output", "points_raw/concatenated"), + ], parameters=[ { "input_topics": LaunchConfiguration("input_points_raw_list"), "output_frame": LaunchConfiguration("tf_output_frame"), "approximate_sync": True, "publish_synchronized_pointcloud": False, + "input_twist_topic_type": "twist", } ], ) @@ -54,7 +58,10 @@ def launch_setup(context, *args, **kwargs): package=pkg, plugin="pointcloud_preprocessor::PointCloudDataSynchronizerComponent", name="synchronizer_filter", - remappings=[("output", "points_raw/concatenated")], + remappings=[ + ("~/input/twist", "/sensing/vehicle_velocity_converter/twist_with_covariance"), + ("output", "points_raw/concatenated"), + ], parameters=[ { "input_topics": LaunchConfiguration("input_points_raw_list"), diff --git a/sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp b/sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp index 3563bf2b6c408..06da91e61b3bd 100644 --- a/sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp @@ -19,6 +19,7 @@ #include #include +#include namespace pointcloud_preprocessor { @@ -30,34 +31,45 @@ BlockageDiagComponent::BlockageDiagComponent(const rclcpp::NodeOptions & options { { // initialize params: - horizontal_ring_id_ = static_cast(declare_parameter("horizontal_ring_id", 12)); - blockage_ratio_threshold_ = - static_cast(declare_parameter("blockage_ratio_threshold", 0.1)); - vertical_bins_ = static_cast(declare_parameter("vertical_bins", 40)); - angle_range_deg_ = declare_parameter("angle_range", std::vector{0.0, 360.0}); - lidar_model_ = static_cast(declare_parameter("model", "Pandar40P")); - blockage_count_threshold_ = - static_cast(declare_parameter("blockage_count_threshold", 50)); - buffering_frames_ = static_cast(declare_parameter("buffering_frames", 100)); - buffering_interval_ = static_cast(declare_parameter("buffering_interval", 5)); + horizontal_ring_id_ = declare_parameter("horizontal_ring_id"); + blockage_ratio_threshold_ = declare_parameter("blockage_ratio_threshold"); + vertical_bins_ = declare_parameter("vertical_bins"); + angle_range_deg_ = declare_parameter>("angle_range"); + lidar_model_ = declare_parameter("model"); + blockage_count_threshold_ = declare_parameter("blockage_count_threshold"); + blockage_buffering_frames_ = declare_parameter("blockage_buffering_frames"); + blockage_buffering_interval_ = declare_parameter("blockage_buffering_interval"); + dust_ratio_threshold_ = declare_parameter("dust_ratio_threshold"); + dust_count_threshold_ = declare_parameter("dust_count_threshold"); + dust_kernel_size_ = declare_parameter("dust_kernel_size"); + dust_buffering_frames_ = declare_parameter("dust_buffering_frames"); + dust_buffering_interval_ = declare_parameter("dust_buffering_interval"); } updater_.setHardwareID("blockage_diag"); updater_.add( std::string(this->get_namespace()) + ": blockage_validation", this, &BlockageDiagComponent::onBlockageChecker); + updater_.add( + std::string(this->get_namespace()) + ": dust_validation", this, + &BlockageDiagComponent::dustChecker); updater_.setPeriod(0.1); - + single_frame_dust_mask_pub = + image_transport::create_publisher(this, "blockage_diag/debug/single_frame_dust_mask_image"); + multi_frame_dust_mask_pub = + image_transport::create_publisher(this, "blockage_diag/debug/multi_frame_dust_mask_image"); lidar_depth_map_pub_ = image_transport::create_publisher(this, "blockage_diag/debug/lidar_depth_map"); blockage_mask_pub_ = image_transport::create_publisher(this, "blockage_diag/debug/blockage_mask_image"); - + blockage_dust_merged_pub = + image_transport::create_publisher(this, "blockage_diag/debug/blockage_dust_merged_image"); ground_blockage_ratio_pub_ = create_publisher( "blockage_diag/debug/ground_blockage_ratio", rclcpp::SensorDataQoS()); sky_blockage_ratio_pub_ = create_publisher( "blockage_diag/debug/sky_blockage_ratio", rclcpp::SensorDataQoS()); - + ground_dust_ratio_pub_ = create_publisher( + "blockage_diag/debug/ground_dust_ratio", rclcpp::SensorDataQoS()); using std::placeholders::_1; set_param_res_ = this->add_on_set_parameters_callback( std::bind(&BlockageDiagComponent::paramCallback, this, _1)); @@ -105,19 +117,59 @@ void BlockageDiagComponent::onBlockageChecker(DiagnosticStatusWrapper & stat) stat.summary(level, msg); } +void BlockageDiagComponent::dustChecker(diagnostic_updater::DiagnosticStatusWrapper & stat) +{ + stat.add("ground_dust_ratio", std::to_string(ground_dust_ratio_)); + auto level = DiagnosticStatus::OK; + std::string msg; + if (ground_dust_ratio_ < 0.0f) { + level = DiagnosticStatus::STALE; + msg = "STALE"; + } else if ( + (ground_dust_ratio_ > dust_ratio_threshold_) && (dust_frame_count_ > dust_count_threshold_)) { + level = DiagnosticStatus::ERROR; + msg = "ERROR"; + } else if (ground_dust_ratio_ > 0.0f) { + level = DiagnosticStatus::WARN; + msg = "WARN"; + } else { + level = DiagnosticStatus::OK; + msg = "OK"; + } + + if (ground_dust_ratio_ > 0.0f) { + msg = msg + ": LIDAR ground dust"; + } + stat.summary(level, msg); +} + void BlockageDiagComponent::filter( - const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output) + const PointCloud2ConstPtr & input, [[maybe_unused]] const IndicesPtr & indices, + PointCloud2 & output) { std::scoped_lock lock(mutex_); - if (indices) { - RCLCPP_WARN(get_logger(), "Indices are not supported and will be ignored"); + int vertical_bins = vertical_bins_; + int ideal_horizontal_bins; + float distance_coefficient = 327.67f; + float horizontal_resolution_ = 0.4f; + if (lidar_model_ == "Pandar40P") { + distance_coefficient = 327.67f; + horizontal_resolution_ = 0.4f; + } else if (lidar_model_ == "PandarQT") { + distance_coefficient = 3276.75f; + horizontal_resolution_ = 0.6f; } - uint horizontal_bins = static_cast((angle_range_deg_[1] - angle_range_deg_[0])); - uint vertical_bins = vertical_bins_; + ideal_horizontal_bins = + static_cast((angle_range_deg_[1] - angle_range_deg_[0]) / horizontal_resolution_); pcl::PointCloud::Ptr pcl_input(new pcl::PointCloud); pcl::fromROSMsg(*input, *pcl_input); - cv::Mat lidar_depth_map(cv::Size(horizontal_bins, vertical_bins), CV_16UC1, cv::Scalar(0)); - cv::Mat lidar_depth_map_8u(cv::Size(horizontal_bins, vertical_bins), CV_8UC1, cv::Scalar(0)); + std::vector horizontal_bin_reference(ideal_horizontal_bins); + std::vector> each_ring_pointcloud(vertical_bins); + cv::Mat full_size_depth_map( + cv::Size(ideal_horizontal_bins, vertical_bins), CV_16UC1, cv::Scalar(0)); + cv::Mat lidar_depth_map(cv::Size(ideal_horizontal_bins, vertical_bins), CV_16UC1, cv::Scalar(0)); + cv::Mat lidar_depth_map_8u( + cv::Size(ideal_horizontal_bins, vertical_bins), CV_8UC1, cv::Scalar(0)); if (pcl_input->points.empty()) { ground_blockage_ratio_ = 1.0f; sky_blockage_ratio_ = 1.0f; @@ -132,114 +184,186 @@ void BlockageDiagComponent::filter( sky_blockage_range_deg_[0] = angle_range_deg_[0]; sky_blockage_range_deg_[1] = angle_range_deg_[1]; } else { - for (const auto & p : pcl_input->points) { - if ((p.azimuth / 100.0 > angle_range_deg_[0]) && (p.azimuth / 100.0 < angle_range_deg_[1])) { - if (lidar_model_ == "Pandar40P") { - lidar_depth_map.at( - p.ring, static_cast((p.azimuth / 100.0 - angle_range_deg_[0]))) += - static_cast(6250.0 / p.distance); // make image clearly - lidar_depth_map_8u.at( - p.ring, static_cast((p.azimuth / 100.0 - angle_range_deg_[0]))) = 255; - } else if (lidar_model_ == "PandarQT") { - lidar_depth_map.at( - vertical_bins - p.ring - 1, - static_cast((p.azimuth / 100.0 - angle_range_deg_[0]))) += - static_cast(6250.0 / p.distance); - lidar_depth_map_8u.at( - vertical_bins - p.ring - 1, - static_cast((p.azimuth / 100.0 - angle_range_deg_[0]))) = 255; + for (int i = 0; i < ideal_horizontal_bins; ++i) { + horizontal_bin_reference.at(i) = angle_range_deg_[0] + i * horizontal_resolution_; + } + for (const auto p : pcl_input->points) { + for (int horizontal_bin = 0; + horizontal_bin < static_cast(horizontal_bin_reference.size()); horizontal_bin++) { + if ( + (p.azimuth / 100 > + (horizontal_bin_reference.at(horizontal_bin) - horizontal_resolution_ / 2)) && + (p.azimuth / 100 <= + (horizontal_bin_reference.at(horizontal_bin) + horizontal_resolution_ / 2))) { + if (lidar_model_ == "Pandar40P") { + full_size_depth_map.at(p.ring, horizontal_bin) = + UINT16_MAX - distance_coefficient * p.distance; + } else if (lidar_model_ == "PandarQT") { + full_size_depth_map.at(vertical_bins - p.ring - 1, horizontal_bin) = + UINT16_MAX - distance_coefficient * p.distance; + } } } } - lidar_depth_map.convertTo(lidar_depth_map, CV_8UC1, 1.0 / 100.0); - cv::Mat no_return_mask; - cv::inRange(lidar_depth_map_8u, 0, 1, no_return_mask); - - cv::Mat erosion_dst; - cv::Mat element = cv::getStructuringElement( - cv::MORPH_RECT, cv::Size(2 * erode_kernel_ + 1, 2 * erode_kernel_ + 1), - cv::Point(erode_kernel_, erode_kernel_)); - cv::erode(no_return_mask, erosion_dst, element); - cv::dilate(erosion_dst, no_return_mask, element); - - static boost::circular_buffer no_return_mask_buffer(buffering_frames_); - - cv::Mat no_return_mask_result(cv::Size(horizontal_bins, vertical_bins), CV_8UC1, cv::Scalar(0)); - cv::Mat time_series_blockage_mask( - cv::Size(horizontal_bins, vertical_bins), CV_8UC1, cv::Scalar(0)); - cv::Mat no_return_mask_binarized( - cv::Size(horizontal_bins, vertical_bins), CV_8UC1, cv::Scalar(0)); + } + full_size_depth_map.convertTo(lidar_depth_map_8u, CV_8UC1, 1.0 / 300); + cv::Mat no_return_mask(cv::Size(ideal_horizontal_bins, vertical_bins), CV_8UC1, cv::Scalar(0)); + cv::inRange(lidar_depth_map_8u, 0, 1, no_return_mask); + cv::Mat erosion_dst; + cv::Mat blockage_element = cv::getStructuringElement( + cv::MORPH_RECT, cv::Size(2 * blockage_kernel_ + 1, 2 * blockage_kernel_ + 1), + cv::Point(blockage_kernel_, blockage_kernel_)); + cv::erode(no_return_mask, erosion_dst, blockage_element); + cv::dilate(erosion_dst, no_return_mask, blockage_element); + static boost::circular_buffer no_return_mask_buffer(blockage_buffering_frames_); + cv::Mat time_series_blockage_result( + cv::Size(ideal_horizontal_bins, vertical_bins), CV_8UC1, cv::Scalar(0)); + cv::Mat time_series_blockage_mask( + cv::Size(ideal_horizontal_bins, vertical_bins), CV_8UC1, cv::Scalar(0)); + cv::Mat no_return_mask_binarized( + cv::Size(ideal_horizontal_bins, vertical_bins), CV_8UC1, cv::Scalar(0)); - static uint frame_count; - frame_count++; - if (buffering_interval_ != 0) { - no_return_mask_binarized = no_return_mask / 255; - if (frame_count == buffering_interval_) { - no_return_mask_buffer.push_back(no_return_mask_binarized); - frame_count = 0; - } - for (const auto & binary_mask : no_return_mask_buffer) { - time_series_blockage_mask += binary_mask; - } - cv::inRange( - time_series_blockage_mask, no_return_mask_buffer.size() - 1, no_return_mask_buffer.size(), - no_return_mask_result); + if (blockage_buffering_interval_ == 0) { + no_return_mask.copyTo(time_series_blockage_result); + } else { + no_return_mask_binarized = no_return_mask / 255; + if (blockage_frame_count_ >= blockage_buffering_interval_) { + no_return_mask_buffer.push_back(no_return_mask_binarized); + blockage_frame_count_ = 0; } else { - no_return_mask.copyTo(no_return_mask_result); + blockage_frame_count_++; } - cv::Mat ground_no_return_mask; - cv::Mat sky_no_return_mask; - no_return_mask_result(cv::Rect(0, 0, horizontal_bins, horizontal_ring_id_)) - .copyTo(sky_no_return_mask); - no_return_mask_result( - cv::Rect(0, horizontal_ring_id_, horizontal_bins, vertical_bins - horizontal_ring_id_)) - .copyTo(ground_no_return_mask); - ground_blockage_ratio_ = - static_cast(cv::countNonZero(ground_no_return_mask)) / - static_cast(horizontal_bins * (vertical_bins - horizontal_ring_id_)); - sky_blockage_ratio_ = static_cast(cv::countNonZero(sky_no_return_mask)) / - static_cast(horizontal_bins * horizontal_ring_id_); - - if (ground_blockage_ratio_ > blockage_ratio_threshold_) { - cv::Rect ground_blockage_bb = cv::boundingRect(ground_no_return_mask); - ground_blockage_range_deg_[0] = - static_cast(ground_blockage_bb.x) + angle_range_deg_[0]; - ground_blockage_range_deg_[1] = - static_cast(ground_blockage_bb.x + ground_blockage_bb.width) + angle_range_deg_[0]; + for (const auto & binary_mask : no_return_mask_buffer) { + time_series_blockage_mask += binary_mask; + } + cv::inRange( + time_series_blockage_mask, no_return_mask_buffer.size() - 1, no_return_mask_buffer.size(), + time_series_blockage_result); + } + cv::Mat ground_no_return_mask; + cv::Mat sky_no_return_mask; + no_return_mask(cv::Rect(0, 0, ideal_horizontal_bins, horizontal_ring_id_)) + .copyTo(sky_no_return_mask); + no_return_mask( + cv::Rect(0, horizontal_ring_id_, ideal_horizontal_bins, vertical_bins - horizontal_ring_id_)) + .copyTo(ground_no_return_mask); + ground_blockage_ratio_ = + static_cast(cv::countNonZero(ground_no_return_mask)) / + static_cast(ideal_horizontal_bins * (vertical_bins - horizontal_ring_id_)); + sky_blockage_ratio_ = static_cast(cv::countNonZero(sky_no_return_mask)) / + static_cast(ideal_horizontal_bins * horizontal_ring_id_); - if (ground_blockage_count_ <= 2 * blockage_count_threshold_) { - ground_blockage_count_ += 1; - } - } else { - ground_blockage_count_ = 0; + if (ground_blockage_ratio_ > blockage_ratio_threshold_) { + cv::Rect ground_blockage_bb = cv::boundingRect(ground_no_return_mask); + ground_blockage_range_deg_[0] = static_cast(ground_blockage_bb.x) + angle_range_deg_[0]; + ground_blockage_range_deg_[1] = + static_cast(ground_blockage_bb.x + ground_blockage_bb.width) + angle_range_deg_[0]; + if (ground_blockage_count_ <= 2 * blockage_count_threshold_) { + ground_blockage_count_ += 1; } + } else { + ground_blockage_count_ = 0; + } + if (sky_blockage_ratio_ > blockage_ratio_threshold_) { + cv::Rect sky_blockage_bx = cv::boundingRect(sky_no_return_mask); + sky_blockage_range_deg_[0] = static_cast(sky_blockage_bx.x) + angle_range_deg_[0]; + sky_blockage_range_deg_[1] = + static_cast(sky_blockage_bx.x + sky_blockage_bx.width) + angle_range_deg_[0]; + if (sky_blockage_count_ <= 2 * blockage_count_threshold_) { + sky_blockage_count_ += 1; + } + } else { + sky_blockage_count_ = 0; + } + // dust + cv::Mat ground_depth_map = lidar_depth_map_8u( + cv::Rect(0, horizontal_ring_id_, ideal_horizontal_bins, vertical_bins - horizontal_ring_id_)); + cv::Mat sky_blank(horizontal_ring_id_, ideal_horizontal_bins, CV_8UC1, cv::Scalar(0)); + cv::Mat ground_blank( + vertical_bins - horizontal_ring_id_, ideal_horizontal_bins, CV_8UC1, cv::Scalar(0)); + cv::Mat single_dust_img( + cv::Size(lidar_depth_map_8u.rows, lidar_depth_map_8u.cols), CV_8UC1, cv::Scalar(0)); + cv::Mat single_dust_ground_img = ground_depth_map.clone(); + cv::inRange(single_dust_ground_img, 0, 1, single_dust_ground_img); + cv::Mat dust_element = getStructuringElement( + cv::MORPH_RECT, cv::Size(2 * dust_kernel_size_ + 1, 2 * dust_kernel_size_ + 1), + cv::Point(-1, -1)); + cv::dilate(single_dust_ground_img, single_dust_ground_img, dust_element); + cv::erode(single_dust_ground_img, single_dust_ground_img, dust_element); + cv::inRange(single_dust_ground_img, 254, 255, single_dust_ground_img); + cv::Mat ground_mask(cv::Size(ideal_horizontal_bins, horizontal_ring_id_), CV_8UC1); + cv::vconcat(sky_blank, single_dust_ground_img, single_dust_img); + static boost::circular_buffer dust_mask_buffer(dust_buffering_frames_); + cv::Mat binarized_dust_mask_( + cv::Size(ideal_horizontal_bins, vertical_bins), CV_8UC1, cv::Scalar(0)); + cv::Mat multi_frame_dust_mask( + cv::Size(ideal_horizontal_bins, vertical_bins), CV_8UC1, cv::Scalar(0)); + cv::Mat multi_frame_ground_dust_result( + cv::Size(ideal_horizontal_bins, vertical_bins), CV_8UC1, cv::Scalar(0)); - if (sky_blockage_ratio_ > blockage_ratio_threshold_) { - cv::Rect sky_blockage_bx = cv::boundingRect(sky_no_return_mask); - sky_blockage_range_deg_[0] = static_cast(sky_blockage_bx.x) + angle_range_deg_[0]; - sky_blockage_range_deg_[1] = - static_cast(sky_blockage_bx.x + sky_blockage_bx.width) + angle_range_deg_[0]; - if (sky_blockage_count_ <= 2 * blockage_count_threshold_) { - sky_blockage_count_ += 1; - } + if (dust_buffering_interval_ == 0) { + single_dust_img.copyTo(multi_frame_ground_dust_result); + dust_buffering_frame_counter_ = 0; + } else { + binarized_dust_mask_ = single_dust_img / 255; + if (dust_buffering_frame_counter_ >= dust_buffering_interval_) { + dust_mask_buffer.push_back(binarized_dust_mask_); + dust_buffering_frame_counter_ = 0; } else { - sky_blockage_count_ = 0; + dust_buffering_frame_counter_++; } - - cv::Mat lidar_depth_colorized; - cv::applyColorMap(lidar_depth_map, lidar_depth_colorized, cv::COLORMAP_JET); - sensor_msgs::msg::Image::SharedPtr lidar_depth_msg = - cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", lidar_depth_colorized).toImageMsg(); - lidar_depth_msg->header = input->header; - lidar_depth_map_pub_.publish(lidar_depth_msg); - - cv::Mat blockage_mask_colorized; - cv::applyColorMap(no_return_mask_result, blockage_mask_colorized, cv::COLORMAP_JET); - sensor_msgs::msg::Image::SharedPtr blockage_mask_msg = - cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", blockage_mask_colorized).toImageMsg(); - blockage_mask_msg->header = input->header; - blockage_mask_pub_.publish(blockage_mask_msg); + for (const auto & binarized_dust_mask : dust_mask_buffer) { + multi_frame_dust_mask += binarized_dust_mask; + } + cv::inRange( + multi_frame_dust_mask, dust_mask_buffer.size() - 1, dust_mask_buffer.size(), + multi_frame_ground_dust_result); + } + cv::Mat single_frame_ground_dust_colorized( + cv::Size(ideal_horizontal_bins, vertical_bins), CV_8UC3, cv::Scalar(0, 0, 0)); + cv::applyColorMap(single_dust_img, single_frame_ground_dust_colorized, cv::COLORMAP_JET); + cv::Mat multi_frame_ground_dust_colorized; + cv::Mat blockage_dust_merged_img( + cv::Size(ideal_horizontal_bins, vertical_bins), CV_8UC3, cv::Scalar(0, 0, 0)); + cv::Mat blockage_dust_merged_mask( + cv::Size(ideal_horizontal_bins, vertical_bins), CV_8UC1, cv::Scalar(0)); + blockage_dust_merged_img.setTo( + cv::Vec3b(0, 0, 255), time_series_blockage_result); // red:blockage + blockage_dust_merged_img.setTo( + cv::Vec3b(0, 255, 255), multi_frame_ground_dust_result); // yellow:dust + sensor_msgs::msg::Image::SharedPtr single_frame_dust_mask_msg = + cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", single_frame_ground_dust_colorized) + .toImageMsg(); + single_frame_dust_mask_pub.publish(single_frame_dust_mask_msg); + sensor_msgs::msg::Image::SharedPtr multi_frame_dust_mask_msg = + cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", multi_frame_ground_dust_colorized) + .toImageMsg(); + multi_frame_dust_mask_pub.publish(multi_frame_dust_mask_msg); + sensor_msgs::msg::Image::SharedPtr lidar_depth_map_msg = + cv_bridge::CvImage(std_msgs::msg::Header(), "mono16", full_size_depth_map).toImageMsg(); + lidar_depth_map_msg->header = input->header; + lidar_depth_map_pub_.publish(lidar_depth_map_msg); + cv::Mat blockage_mask_colorized; + cv::applyColorMap(time_series_blockage_result, blockage_mask_colorized, cv::COLORMAP_JET); + sensor_msgs::msg::Image::SharedPtr blockage_mask_msg = + cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", blockage_mask_colorized).toImageMsg(); + blockage_mask_msg->header = input->header; + blockage_mask_pub_.publish(blockage_mask_msg); + cv::Mat blockage_dust_merged_colorized( + cv::Size(ideal_horizontal_bins, vertical_bins), CV_8UC3, cv::Scalar(0, 0, 0)); + blockage_dust_merged_img.copyTo(blockage_dust_merged_colorized); + sensor_msgs::msg::Image::SharedPtr blockage_dust_merged_msg = + cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", blockage_dust_merged_colorized) + .toImageMsg(); + if (ground_dust_ratio_ > dust_ratio_threshold_) { + if (dust_frame_count_ < 2 * dust_count_threshold_) { + dust_frame_count_++; + } + } else { + dust_frame_count_ = 0; } + blockage_dust_merged_msg->header = input->header; + blockage_dust_merged_pub.publish(blockage_dust_merged_msg); tier4_debug_msgs::msg::Float32Stamped ground_blockage_ratio_msg; ground_blockage_ratio_msg.data = ground_blockage_ratio_; @@ -250,7 +374,14 @@ void BlockageDiagComponent::filter( sky_blockage_ratio_msg.data = sky_blockage_ratio_; sky_blockage_ratio_msg.stamp = now(); sky_blockage_ratio_pub_->publish(sky_blockage_ratio_msg); + tier4_debug_msgs::msg::Float32Stamped blockage_ratio_msg; + tier4_debug_msgs::msg::Float32Stamped ground_dust_ratio_msg; + ground_dust_ratio_ = static_cast(cv::countNonZero(single_dust_ground_img)) / + (single_dust_ground_img.cols * single_dust_ground_img.rows); + ground_dust_ratio_msg.data = ground_dust_ratio_; + ground_dust_ratio_msg.stamp = now(); + ground_dust_ratio_pub_->publish(ground_dust_ratio_msg); pcl::toROSMsg(*pcl_input, output); output.header = input->header; } @@ -280,11 +411,29 @@ rcl_interfaces::msg::SetParametersResult BlockageDiagComponent::paramCallback( get_logger(), " Setting new angle_range to: [%f , %f].", angle_range_deg_[0], angle_range_deg_[1]); } - if (get_param(p, "buffering_frames", buffering_frames_)) { - RCLCPP_DEBUG(get_logger(), "Setting new buffering_frames to: %d.", buffering_frames_); + if (get_param(p, "blockage_buffering_frames", blockage_buffering_frames_)) { + RCLCPP_DEBUG( + get_logger(), "Setting new blockage_buffering_frames_ to: %d.", blockage_buffering_frames_); + } + if (get_param(p, "blockage_buffering_interval", blockage_buffering_interval_)) { + RCLCPP_DEBUG( + get_logger(), "Setting new blockage_buffering_interval_ to: %d.", + blockage_buffering_interval_); + } + if (get_param(p, "dust_kernel_size", dust_kernel_size_)) { + RCLCPP_DEBUG(get_logger(), "Setting new dust_kernel_size_ to: %d.", dust_kernel_size_); } - if (get_param(p, "buffering_interval", buffering_interval_)) { - RCLCPP_DEBUG(get_logger(), "Setting new buffering_interval to: %d.", buffering_interval_); + if (get_param(p, "dust_buffering_frames", dust_buffering_frames_)) { + RCLCPP_DEBUG( + get_logger(), "Setting new dust_buffering_frames_ to: %d.", dust_buffering_frames_); + // note:NOT affects to actual variable. + // if you want change this param/variable, change the parameter called at launch this + // node(aip_launcher). + } + if (get_param(p, "dust_buffering_interval", dust_buffering_interval_)) { + RCLCPP_DEBUG( + get_logger(), "Setting new dust_buffering_interval_ to: %d.", dust_buffering_interval_); + dust_buffering_frame_counter_ = 0; } rcl_interfaces::msg::SetParametersResult result; result.successful = true; diff --git a/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp b/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp index d88665a4c4c98..ff72e433e9602 100644 --- a/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp @@ -69,7 +69,8 @@ namespace pointcloud_preprocessor { PointCloudConcatenateDataSynchronizerComponent::PointCloudConcatenateDataSynchronizerComponent( const rclcpp::NodeOptions & node_options) -: Node("point_cloud_concatenator_component", node_options) +: Node("point_cloud_concatenator_component", node_options), + input_twist_topic_type_(declare_parameter("input_twist_topic_type", "twist")) { // initialize debug tool { @@ -164,10 +165,24 @@ PointCloudConcatenateDataSynchronizerComponent::PointCloudConcatenateDataSynchro filters_[d] = this->create_subscription( input_topics_[d], rclcpp::SensorDataQoS().keep_last(maximum_queue_size_), cb); } - auto twist_cb = std::bind( - &PointCloudConcatenateDataSynchronizerComponent::twist_callback, this, std::placeholders::_1); - sub_twist_ = this->create_subscription( - "~/input/twist", rclcpp::QoS{100}, twist_cb); + + if (input_twist_topic_type_ == "twist") { + auto twist_cb = std::bind( + &PointCloudConcatenateDataSynchronizerComponent::twist_callback, this, + std::placeholders::_1); + sub_twist_ = this->create_subscription( + "~/input/twist", rclcpp::QoS{100}, twist_cb); + } else if (input_twist_topic_type_ == "odom") { + auto odom_cb = std::bind( + &PointCloudConcatenateDataSynchronizerComponent::odom_callback, this, + std::placeholders::_1); + sub_odom_ = this->create_subscription( + "~/input/odom", rclcpp::QoS{100}, odom_cb); + } else { + RCLCPP_ERROR_STREAM( + get_logger(), "input_twist_topic_type is invalid: " << input_twist_topic_type_); + throw std::runtime_error("input_twist_topic_type is invalid: " + input_twist_topic_type_); + } } // Transformed Raw PointCloud2 Publisher @@ -227,8 +242,19 @@ Eigen::Matrix4f PointCloudConcatenateDataSynchronizerComponent::computeTransformToAdjustForOldTimestamp( const rclcpp::Time & old_stamp, const rclcpp::Time & new_stamp) { - // return identity if no twist is available or old_stamp is newer than new_stamp - if (twist_ptr_queue_.empty() || old_stamp > new_stamp) { + // return identity if no twist is available + if (twist_ptr_queue_.empty()) { + RCLCPP_WARN_STREAM_THROTTLE( + get_logger(), *get_clock(), std::chrono::milliseconds(10000).count(), + "No twist is available. Please confirm twist topic and timestamp"); + return Eigen::Matrix4f::Identity(); + } + + // return identity if old_stamp is newer than new_stamp + if (old_stamp > new_stamp) { + RCLCPP_WARN_STREAM_THROTTLE( + get_logger(), *get_clock(), std::chrono::milliseconds(10000).count(), + "old_stamp is newer than new_stamp,"); return Eigen::Matrix4f::Identity(); } @@ -400,12 +426,6 @@ void PointCloudConcatenateDataSynchronizerComponent::convertToXYZICloud( { output_ptr->header = input_ptr->header; - if (input_ptr->data.empty()) { - RCLCPP_WARN_STREAM_THROTTLE( - this->get_logger(), *this->get_clock(), 1000, "Empty sensor points!"); - return; - } - PointCloud2Modifier output_modifier{*output_ptr, input_ptr->header.frame_id}; output_modifier.reserve(input_ptr->width); @@ -460,6 +480,12 @@ void PointCloudConcatenateDataSynchronizerComponent::cloud_callback( { std::lock_guard lock(mutex_); auto input = std::make_shared(*input_ptr); + if (input->data.empty()) { + RCLCPP_WARN_STREAM_THROTTLE( + this->get_logger(), *this->get_clock(), 1000, "Empty sensor points!"); + return; + } + sensor_msgs::msg::PointCloud2::SharedPtr xyzi_input_ptr(new sensor_msgs::msg::PointCloud2()); convertToXYZICloud(input, xyzi_input_ptr); @@ -558,6 +584,32 @@ void PointCloudConcatenateDataSynchronizerComponent::twist_callback( twist_ptr_queue_.push_back(twist_ptr); } +void PointCloudConcatenateDataSynchronizerComponent::odom_callback( + const nav_msgs::msg::Odometry::ConstSharedPtr input) +{ + // if rosbag restart, clear buffer + if (!twist_ptr_queue_.empty()) { + if (rclcpp::Time(twist_ptr_queue_.front()->header.stamp) > rclcpp::Time(input->header.stamp)) { + twist_ptr_queue_.clear(); + } + } + + // pop old data + while (!twist_ptr_queue_.empty()) { + if ( + rclcpp::Time(twist_ptr_queue_.front()->header.stamp) + rclcpp::Duration::from_seconds(1.0) > + rclcpp::Time(input->header.stamp)) { + break; + } + twist_ptr_queue_.pop_front(); + } + + auto twist_ptr = std::make_shared(); + twist_ptr->header = input->header; + twist_ptr->twist = input->twist.twist; + twist_ptr_queue_.push_back(twist_ptr); +} + void PointCloudConcatenateDataSynchronizerComponent::checkConcatStatus( diagnostic_updater::DiagnosticStatusWrapper & stat) { diff --git a/sensing/pointcloud_preprocessor/src/crop_box_filter/crop_box_filter_nodelet.cpp b/sensing/pointcloud_preprocessor/src/crop_box_filter/crop_box_filter_nodelet.cpp index 0cae2c90c2f16..20d1f5c8b3d6d 100644 --- a/sensing/pointcloud_preprocessor/src/crop_box_filter/crop_box_filter_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/crop_box_filter/crop_box_filter_nodelet.cpp @@ -119,7 +119,8 @@ void CropBoxFilterComponent::faster_filter( stop_watch_ptr_->toc("processing_time", true); if (indices) { - RCLCPP_WARN(get_logger(), "Indices are not supported and will be ignored"); + RCLCPP_WARN_THROTTLE( + get_logger(), *get_clock(), 1000, "Indices are not supported and will be ignored"); } int x_offset = input->fields[pcl::getFieldIndex(*input, "x")].offset; @@ -129,6 +130,8 @@ void CropBoxFilterComponent::faster_filter( output.data.resize(input->data.size()); size_t output_size = 0; + int skipped_count = 0; + for (size_t global_offset = 0; global_offset + input->point_step <= input->data.size(); global_offset += input->point_step) { Eigen::Vector4f point; @@ -138,7 +141,7 @@ void CropBoxFilterComponent::faster_filter( point[3] = 1; if (!std::isfinite(point[0]) || !std::isfinite(point[1]) || !std::isfinite(point[2])) { - RCLCPP_WARN(this->get_logger(), "Ignoring point containing NaN values"); + skipped_count++; continue; } @@ -162,6 +165,12 @@ void CropBoxFilterComponent::faster_filter( } } + if (skipped_count > 0) { + RCLCPP_WARN_THROTTLE( + get_logger(), *get_clock(), 1000, "%d points contained NaN values and have been ignored", + skipped_count); + } + output.data.resize(output_size); // Note that tf_input_orig_frame_ is the input frame, while tf_input_frame_ is the frame of the diff --git a/sensing/pointcloud_preprocessor/src/downsample_filter/faster_voxel_grid_downsample_filter.cpp b/sensing/pointcloud_preprocessor/src/downsample_filter/faster_voxel_grid_downsample_filter.cpp index 7c302eec20431..5829277335433 100644 --- a/sensing/pointcloud_preprocessor/src/downsample_filter/faster_voxel_grid_downsample_filter.cpp +++ b/sensing/pointcloud_preprocessor/src/downsample_filter/faster_voxel_grid_downsample_filter.cpp @@ -36,7 +36,12 @@ void FasterVoxelGridDownsampleFilter::set_field_offsets(const PointCloud2ConstPt x_offset_ = input->fields[pcl::getFieldIndex(*input, "x")].offset; y_offset_ = input->fields[pcl::getFieldIndex(*input, "y")].offset; z_offset_ = input->fields[pcl::getFieldIndex(*input, "z")].offset; - intensity_offset_ = input->fields[pcl::getFieldIndex(*input, "intensity")].offset; + int intensity_index = pcl::getFieldIndex(*input, "intensity"); + if (intensity_index != -1) { + intensity_offset_ = input->fields[intensity_index].offset; + } else { + intensity_offset_ = z_offset_ + sizeof(float); + } offset_initialized_ = true; } diff --git a/sensing/pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_nodelet.cpp b/sensing/pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_nodelet.cpp index a6bf64b835f61..5d261f42391e9 100644 --- a/sensing/pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_nodelet.cpp @@ -41,7 +41,8 @@ namespace pointcloud_preprocessor { PointCloudDataSynchronizerComponent::PointCloudDataSynchronizerComponent( const rclcpp::NodeOptions & node_options) -: Node("point_cloud_time_synchronizer_component", node_options) +: Node("point_cloud_time_synchronizer_component", node_options), + input_twist_topic_type_(declare_parameter("input_twist_topic_type", "twist")) { // initialize debug tool { @@ -129,10 +130,21 @@ PointCloudDataSynchronizerComponent::PointCloudDataSynchronizerComponent( } // Subscribe to the twist - auto twist_cb = - std::bind(&PointCloudDataSynchronizerComponent::twist_callback, this, std::placeholders::_1); - sub_twist_ = this->create_subscription( - "/vehicle/status/velocity_status", rclcpp::QoS{100}, twist_cb); + if (input_twist_topic_type_ == "twist") { + auto twist_cb = std::bind( + &PointCloudDataSynchronizerComponent::twist_callback, this, std::placeholders::_1); + sub_twist_ = this->create_subscription( + "~/input/twist", rclcpp::QoS{100}, twist_cb); + } else if (input_twist_topic_type_ == "odom") { + auto odom_cb = + std::bind(&PointCloudDataSynchronizerComponent::odom_callback, this, std::placeholders::_1); + sub_odom_ = this->create_subscription( + "~/input/odom", rclcpp::QoS{100}, odom_cb); + } else { + RCLCPP_ERROR_STREAM( + get_logger(), "input_twist_topic_type is invalid: " << input_twist_topic_type_); + throw std::runtime_error("input_twist_topic_type is invalid: " + input_twist_topic_type_); + } } // Transformed Raw PointCloud2 Publisher @@ -474,7 +486,33 @@ void PointCloudDataSynchronizerComponent::timer_callback() } void PointCloudDataSynchronizerComponent::twist_callback( - const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr input) + const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr input) +{ + // if rosbag restart, clear buffer + if (!twist_ptr_queue_.empty()) { + if (rclcpp::Time(twist_ptr_queue_.front()->header.stamp) > rclcpp::Time(input->header.stamp)) { + twist_ptr_queue_.clear(); + } + } + + // pop old data + while (!twist_ptr_queue_.empty()) { + if ( + rclcpp::Time(twist_ptr_queue_.front()->header.stamp) + rclcpp::Duration::from_seconds(1.0) > + rclcpp::Time(input->header.stamp)) { + break; + } + twist_ptr_queue_.pop_front(); + } + + auto twist_ptr = std::make_shared(); + twist_ptr->header.stamp = input->header.stamp; + twist_ptr->twist = input->twist.twist; + twist_ptr_queue_.push_back(twist_ptr); +} + +void PointCloudDataSynchronizerComponent::odom_callback( + const nav_msgs::msg::Odometry::ConstSharedPtr input) { // if rosbag restart, clear buffer if (!twist_ptr_queue_.empty()) { @@ -495,9 +533,7 @@ void PointCloudDataSynchronizerComponent::twist_callback( auto twist_ptr = std::make_shared(); twist_ptr->header.stamp = input->header.stamp; - twist_ptr->twist.linear.x = input->longitudinal_velocity; - twist_ptr->twist.linear.y = input->lateral_velocity; - twist_ptr->twist.angular.z = input->heading_rate; + twist_ptr->twist = input->twist.twist; twist_ptr_queue_.push_back(twist_ptr); } diff --git a/sensing/radar_scan_to_pointcloud2/CMakeLists.txt b/sensing/radar_scan_to_pointcloud2/CMakeLists.txt index 1fb63df6594bf..6ec1759fc7d4d 100644 --- a/sensing/radar_scan_to_pointcloud2/CMakeLists.txt +++ b/sensing/radar_scan_to_pointcloud2/CMakeLists.txt @@ -27,4 +27,5 @@ endif() ament_auto_package( INSTALL_TO_SHARE launch + config ) diff --git a/sensing/radar_scan_to_pointcloud2/config/radar_scan_to_pointcloud2.param.yaml b/sensing/radar_scan_to_pointcloud2/config/radar_scan_to_pointcloud2.param.yaml new file mode 100644 index 0000000000000..381977af218be --- /dev/null +++ b/sensing/radar_scan_to_pointcloud2/config/radar_scan_to_pointcloud2.param.yaml @@ -0,0 +1,4 @@ +/**: + ros__parameters: + publish_amplitude_pointcloud: true + publish_doppler_pointcloud: false diff --git a/sensing/radar_scan_to_pointcloud2/launch/radar_scan_to_pointcloud2.launch.xml b/sensing/radar_scan_to_pointcloud2/launch/radar_scan_to_pointcloud2.launch.xml index f94b0bfd91ea8..45853c409fced 100644 --- a/sensing/radar_scan_to_pointcloud2/launch/radar_scan_to_pointcloud2.launch.xml +++ b/sensing/radar_scan_to_pointcloud2/launch/radar_scan_to_pointcloud2.launch.xml @@ -2,14 +2,13 @@ - - + + - - + diff --git a/sensing/radar_scan_to_pointcloud2/schema/radar_scan_to_pointcloud2.schema.json b/sensing/radar_scan_to_pointcloud2/schema/radar_scan_to_pointcloud2.schema.json new file mode 100644 index 0000000000000..3d60c3719af8f --- /dev/null +++ b/sensing/radar_scan_to_pointcloud2/schema/radar_scan_to_pointcloud2.schema.json @@ -0,0 +1,35 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Radar Scan to PointCloud2 Node", + "type": "object", + "definitions": { + "radar_scan_to_pointcloud2": { + "type": "object", + "properties": { + "publish_amplitude_pointcloud": { + "type": "boolean", + "description": "Whether publish radar pointcloud whose intensity is amplitude.", + "default": "true" + }, + "publish_doppler_pointcloud": { + "type": "boolean", + "description": "Whether publish radar pointcloud whose intensity is doppler velocity.", + "default": "false" + } + }, + "required": ["publish_amplitude_pointcloud", "publish_doppler_pointcloud"] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/radar_scan_to_pointcloud2" + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] +} diff --git a/sensing/radar_scan_to_pointcloud2/src/radar_scan_to_pointcloud2_node/radar_scan_to_pointcloud2_node.cpp b/sensing/radar_scan_to_pointcloud2/src/radar_scan_to_pointcloud2_node/radar_scan_to_pointcloud2_node.cpp index de4ac74db6429..b182147468262 100644 --- a/sensing/radar_scan_to_pointcloud2/src/radar_scan_to_pointcloud2_node/radar_scan_to_pointcloud2_node.cpp +++ b/sensing/radar_scan_to_pointcloud2/src/radar_scan_to_pointcloud2_node/radar_scan_to_pointcloud2_node.cpp @@ -104,9 +104,8 @@ RadarScanToPointcloud2Node::RadarScanToPointcloud2Node(const rclcpp::NodeOptions // Node Parameter node_param_.publish_amplitude_pointcloud = - declare_parameter("publish_amplitude_pointcloud", true); - node_param_.publish_doppler_pointcloud = - declare_parameter("publish_doppler_pointcloud", false); + declare_parameter("publish_amplitude_pointcloud"); + node_param_.publish_doppler_pointcloud = declare_parameter("publish_doppler_pointcloud"); // Subscriber sub_radar_ = create_subscription( diff --git a/sensing/radar_static_pointcloud_filter/CMakeLists.txt b/sensing/radar_static_pointcloud_filter/CMakeLists.txt index 1f08f7842cfdf..ecd33d3166b19 100644 --- a/sensing/radar_static_pointcloud_filter/CMakeLists.txt +++ b/sensing/radar_static_pointcloud_filter/CMakeLists.txt @@ -27,4 +27,5 @@ endif() ament_auto_package( INSTALL_TO_SHARE launch + config ) diff --git a/sensing/radar_static_pointcloud_filter/config/radar_static_pointcloud_filter.param.yaml b/sensing/radar_static_pointcloud_filter/config/radar_static_pointcloud_filter.param.yaml new file mode 100644 index 0000000000000..a429f7a065ffe --- /dev/null +++ b/sensing/radar_static_pointcloud_filter/config/radar_static_pointcloud_filter.param.yaml @@ -0,0 +1,17 @@ +# Copyright 2023 Foxconn, 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. + +/**: + ros__parameters: + doppler_velocity_sd: 4.0 diff --git a/sensing/radar_static_pointcloud_filter/include/radar_static_pointcloud_filter/radar_static_pointcloud_filter_node.hpp b/sensing/radar_static_pointcloud_filter/include/radar_static_pointcloud_filter/radar_static_pointcloud_filter_node.hpp index e97e8fa5ca500..f1ab9344d58f3 100644 --- a/sensing/radar_static_pointcloud_filter/include/radar_static_pointcloud_filter/radar_static_pointcloud_filter_node.hpp +++ b/sensing/radar_static_pointcloud_filter/include/radar_static_pointcloud_filter/radar_static_pointcloud_filter_node.hpp @@ -15,7 +15,7 @@ #ifndef RADAR_STATIC_POINTCLOUD_FILTER__RADAR_STATIC_POINTCLOUD_FILTER_NODE_HPP_ #define RADAR_STATIC_POINTCLOUD_FILTER__RADAR_STATIC_POINTCLOUD_FILTER_NODE_HPP_ -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" +#include "tier4_autoware_utils/ros/transform_listener.hpp" #include diff --git a/sensing/radar_static_pointcloud_filter/launch/radar_static_pointcloud_filter.launch.xml b/sensing/radar_static_pointcloud_filter/launch/radar_static_pointcloud_filter.launch.xml index 08fdeee74f15a..b9491227837c5 100644 --- a/sensing/radar_static_pointcloud_filter/launch/radar_static_pointcloud_filter.launch.xml +++ b/sensing/radar_static_pointcloud_filter/launch/radar_static_pointcloud_filter.launch.xml @@ -3,13 +3,14 @@ - + + - + diff --git a/sensing/radar_static_pointcloud_filter/schema/radar_static_pointcloud_filter.schema.json b/sensing/radar_static_pointcloud_filter/schema/radar_static_pointcloud_filter.schema.json new file mode 100644 index 0000000000000..65340d889f62e --- /dev/null +++ b/sensing/radar_static_pointcloud_filter/schema/radar_static_pointcloud_filter.schema.json @@ -0,0 +1,31 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Radar Static Pointcloud Filter Node", + "type": "object", + "definitions": { + "radar_static_pointcloud_filter": { + "type": "object", + "properties": { + "doppler_velocity_sd": { + "type": "number", + "default": "4.0", + "minimum": 0.0, + "description": "Standard deviation for radar doppler velocity. [m/s]" + } + }, + "required": ["doppler_velocity_sd"] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/radar_static_pointcloud_filter" + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] +} diff --git a/sensing/radar_static_pointcloud_filter/src/radar_static_pointcloud_filter_node/radar_static_pointcloud_filter_node.cpp b/sensing/radar_static_pointcloud_filter/src/radar_static_pointcloud_filter_node/radar_static_pointcloud_filter_node.cpp index 812a96e7cdcd5..772dcef753c11 100644 --- a/sensing/radar_static_pointcloud_filter/src/radar_static_pointcloud_filter_node/radar_static_pointcloud_filter_node.cpp +++ b/sensing/radar_static_pointcloud_filter/src/radar_static_pointcloud_filter_node/radar_static_pointcloud_filter_node.cpp @@ -1,4 +1,4 @@ -// Copyright 2022 TIER IV, Inc. +// Copyright 2022-2023 Foxconn, 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. @@ -16,6 +16,7 @@ #include #include +#include #include #include @@ -92,7 +93,7 @@ RadarStaticPointcloudFilterNode::RadarStaticPointcloudFilterNode( std::bind(&RadarStaticPointcloudFilterNode::onSetParam, this, _1)); // Node Parameter - node_param_.doppler_velocity_sd = declare_parameter("doppler_velocity_sd", 2.0); + node_param_.doppler_velocity_sd = declare_parameter("doppler_velocity_sd"); // Subscriber transform_listener_ = std::make_shared(this); diff --git a/simulator/dummy_perception_publisher/src/node.cpp b/simulator/dummy_perception_publisher/src/node.cpp index 6036e760fed47..f4cb6063a9bf6 100644 --- a/simulator/dummy_perception_publisher/src/node.cpp +++ b/simulator/dummy_perception_publisher/src/node.cpp @@ -14,7 +14,7 @@ #include "dummy_perception_publisher/node.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" #include #include diff --git a/simulator/simple_planning_simulator/README.md b/simulator/simple_planning_simulator/README.md index d70aca794dc07..ceecacfe8cd8c 100644 --- a/simulator/simple_planning_simulator/README.md +++ b/simulator/simple_planning_simulator/README.md @@ -89,6 +89,12 @@ _Note_: The steering/velocity/acceleration dynamics is modeled by a first order Since the vehicle outputs `odom`->`base_link` tf, this simulator outputs the tf with the same frame_id configuration. In the simple_planning_simulator.launch.py, the node that outputs the `map`->`odom` tf, that usually estimated by the localization module (e.g. NDT), will be launched as well. Since the tf output by this simulator module is an ideal value, `odom`->`map` will always be 0. +### (Caveat) Pitch calculation + +Ego vehicle pitch angle is calculated in the following manner. + +![pitch calculation](./media/pitch-calculation.drawio.svg) + ## Error detection and handling The only validation on inputs being done is testing for a valid vehicle model type. diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp index 742f87411b7d8..4b25dba8b16ee 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp @@ -22,6 +22,7 @@ #include "autoware_auto_control_msgs/msg/ackermann_control_command.hpp" #include "autoware_auto_geometry_msgs/msg/complex32.hpp" +#include "autoware_auto_mapping_msgs/msg/had_map_bin.hpp" #include "autoware_auto_planning_msgs/msg/trajectory.hpp" #include "autoware_auto_vehicle_msgs/msg/control_mode_command.hpp" #include "autoware_auto_vehicle_msgs/msg/control_mode_report.hpp" @@ -47,6 +48,7 @@ #include "sensor_msgs/msg/imu.hpp" #include "tier4_external_api_msgs/srv/initialize_pose.hpp" +#include #include #include @@ -62,6 +64,7 @@ namespace simple_planning_simulator using autoware_auto_control_msgs::msg::AckermannControlCommand; using autoware_auto_geometry_msgs::msg::Complex32; +using autoware_auto_mapping_msgs::msg::HADMapBin; using autoware_auto_planning_msgs::msg::Trajectory; using autoware_auto_vehicle_msgs::msg::ControlModeReport; using autoware_auto_vehicle_msgs::msg::Engage; @@ -143,6 +146,7 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node rclcpp::Subscription::SharedPtr sub_vehicle_cmd_; rclcpp::Subscription::SharedPtr sub_ackermann_cmd_; rclcpp::Subscription::SharedPtr sub_manual_ackermann_cmd_; + rclcpp::Subscription::SharedPtr sub_map_; rclcpp::Subscription::SharedPtr sub_init_pose_; rclcpp::Subscription::SharedPtr sub_init_twist_; rclcpp::Subscription::SharedPtr sub_trajectory_; @@ -160,6 +164,8 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node rcl_interfaces::msg::SetParametersResult on_parameter( const std::vector & parameters); + lanelet::ConstLanelets road_lanelets_; + /* tf */ tf2_ros::Buffer tf_buffer_; tf2_ros::TransformListener tf_listener_; @@ -179,6 +185,7 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node Trajectory::ConstSharedPtr current_trajectory_ptr_; bool simulate_motion_; //!< stop vehicle motion simulation if false ControlModeReport current_control_mode_; + bool enable_road_slope_simulation_; /* frame_id */ std::string simulated_frame_id_; //!< @brief simulated vehicle frame id @@ -214,7 +221,7 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node /** * @brief set input steering, velocity, and acceleration of the vehicle model */ - void set_input(const AckermannControlCommand & cmd); + void set_input(const AckermannControlCommand & cmd, const double acc_by_slope); /** * @brief set current_vehicle_state_ with received message @@ -226,6 +233,11 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node */ void on_hazard_lights_cmd(const HazardLightsCommand::ConstSharedPtr msg); + /** + * @brief subscribe lanelet map + */ + void on_map(const HADMapBin::ConstSharedPtr msg); + /** * @brief set initial pose for simulation with received message */ @@ -257,7 +269,7 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node * @brief ControlModeRequest server */ void on_control_mode_request( - const ControlModeCommand::Request::SharedPtr request, + const ControlModeCommand::Request::ConstSharedPtr request, const ControlModeCommand::Response::SharedPtr response); /** @@ -276,6 +288,12 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node */ TransformStamped get_transform_msg(const std::string parent_frame, const std::string child_frame); + /** + * @brief calculate ego pitch angle from trajectory + * @return ego pitch angle + */ + double calculate_ego_pitch() const; + /** * @brief timer callback for simulation with loop_rate */ diff --git a/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py b/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py index 7837c61ec6593..2f9c2cfe333f4 100644 --- a/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py +++ b/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py @@ -53,6 +53,7 @@ def launch_setup(context, *args, **kwargs): }, ], remappings=[ + ("input/vector_map", "/map/vector_map"), ("input/initialpose", "/initialpose3d"), ("input/ackermann_control_command", "/control/command/control_cmd"), ("input/manual_ackermann_control_command", "/vehicle/command/manual_control_cmd"), diff --git a/simulator/simple_planning_simulator/media/pitch-calculation.drawio.svg b/simulator/simple_planning_simulator/media/pitch-calculation.drawio.svg new file mode 100644 index 0000000000000..d022b29d2a730 --- /dev/null +++ b/simulator/simple_planning_simulator/media/pitch-calculation.drawio.svg @@ -0,0 +1,329 @@ + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + forward
$diff_xy > 0
 + +
+
+
+ + forward... + + + + + +
+
+
+ + + ego +
+ direction +
+ + +
+
+
+ + ego... + + + + + +
+
+
+ + reverse
$diff_xy < 0
 + +
+
+
+ + reverse... + + + + + +
+
+
+ + + lane +
+ direction +
+ + +
+
+
+ + lane... + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + $diff_z > 0
 + +
+
+
+ + $diff_z > 0 + + + + + +
+
+
+ + $diff_z < 0
 + +
+
+
+ + $diff_z < 0 + + + + + +
+
+
+ + pitch < 0
 + +
+
+
+ + pitch < 0 + + + + + +
+
+
+ + pitch > 0
 + +
+
+
+ + pitch > 0 + + + + + +
+
+
+ + pitch < 0
 + +
+
+
+ + pitch < 0 + + + + + +
+
+
+ + pitch > 0
 + +
+
+
+ + pitch > 0 + + + + + + + + Text is not SVG - cannot display + + + diff --git a/simulator/simple_planning_simulator/package.xml b/simulator/simple_planning_simulator/package.xml index c83433d35bb1b..5652be138b18f 100644 --- a/simulator/simple_planning_simulator/package.xml +++ b/simulator/simple_planning_simulator/package.xml @@ -12,10 +12,14 @@ autoware_cmake autoware_auto_control_msgs + autoware_auto_mapping_msgs autoware_auto_planning_msgs autoware_auto_tf2 autoware_auto_vehicle_msgs geometry_msgs + lanelet2_core + lanelet2_extension + motion_utils nav_msgs rclcpp rclcpp_components diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp index cfc58cdaeed63..fb6a5457e8f05 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp @@ -15,11 +15,19 @@ #include "simple_planning_simulator/simple_planning_simulator_core.hpp" #include "autoware_auto_tf2/tf2_autoware_auto_msgs.hpp" +#include "motion_utils/trajectory/trajectory.hpp" #include "rclcpp_components/register_node_macro.hpp" #include "simple_planning_simulator/vehicle_model/sim_model.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" +#include "tier4_autoware_utils/ros/msg_covariance.hpp" +#include "tier4_autoware_utils/ros/update_param.hpp" #include "vehicle_info_util/vehicle_info_util.hpp" +#include +#include + +#include +#include #include #include @@ -45,13 +53,14 @@ autoware_auto_vehicle_msgs::msg::VelocityReport to_velocity_report( return velocity; } -nav_msgs::msg::Odometry to_odometry(const std::shared_ptr vehicle_model_ptr) +nav_msgs::msg::Odometry to_odometry( + const std::shared_ptr vehicle_model_ptr, const double ego_pitch_angle) { nav_msgs::msg::Odometry odometry; odometry.pose.pose.position.x = vehicle_model_ptr->getX(); odometry.pose.pose.position.y = vehicle_model_ptr->getY(); - odometry.pose.pose.orientation = - tier4_autoware_utils::createQuaternionFromYaw(vehicle_model_ptr->getYaw()); + odometry.pose.pose.orientation = tier4_autoware_utils::createQuaternionFromRPY( + 0.0, ego_pitch_angle, vehicle_model_ptr->getYaw()); odometry.twist.twist.linear.x = vehicle_model_ptr->getVx(); odometry.twist.twist.angular.z = vehicle_model_ptr->getWz(); @@ -66,6 +75,19 @@ autoware_auto_vehicle_msgs::msg::SteeringReport to_steering_report( return steer; } +std::vector convert_centerline_to_points( + const lanelet::Lanelet & lanelet) +{ + std::vector centerline_points; + for (const auto & point : lanelet.centerline()) { + geometry_msgs::msg::Point center_point; + center_point.x = point.basicPoint().x(); + center_point.y = point.basicPoint().y(); + center_point.z = point.basicPoint().z(); + centerline_points.push_back(center_point); + } + return centerline_points; +} } // namespace namespace simulation @@ -80,27 +102,33 @@ SimplePlanningSimulator::SimplePlanningSimulator(const rclcpp::NodeOptions & opt origin_frame_id_ = declare_parameter("origin_frame_id", "odom"); add_measurement_noise_ = declare_parameter("add_measurement_noise", false); simulate_motion_ = declare_parameter("initial_engage_state"); + enable_road_slope_simulation_ = declare_parameter("enable_road_slope_simulation", false); using rclcpp::QoS; using std::placeholders::_1; using std::placeholders::_2; + sub_map_ = create_subscription( + "input/vector_map", rclcpp::QoS(10).transient_local(), + std::bind(&SimplePlanningSimulator::on_map, this, _1)); sub_init_pose_ = create_subscription( "input/initialpose", QoS{1}, std::bind(&SimplePlanningSimulator::on_initialpose, this, _1)); sub_init_twist_ = create_subscription( "input/initialtwist", QoS{1}, std::bind(&SimplePlanningSimulator::on_initialtwist, this, _1)); sub_ackermann_cmd_ = create_subscription( "input/ackermann_control_command", QoS{1}, - [this](const AckermannControlCommand::SharedPtr msg) { current_ackermann_cmd_ = *msg; }); + [this](const AckermannControlCommand::ConstSharedPtr msg) { current_ackermann_cmd_ = *msg; }); sub_manual_ackermann_cmd_ = create_subscription( "input/manual_ackermann_control_command", QoS{1}, - [this](const AckermannControlCommand::SharedPtr msg) { current_manual_ackermann_cmd_ = *msg; }); + [this](const AckermannControlCommand::ConstSharedPtr msg) { + current_manual_ackermann_cmd_ = *msg; + }); sub_gear_cmd_ = create_subscription( "input/gear_command", QoS{1}, - [this](const GearCommand::SharedPtr msg) { current_gear_cmd_ = *msg; }); + [this](const GearCommand::ConstSharedPtr msg) { current_gear_cmd_ = *msg; }); sub_manual_gear_cmd_ = create_subscription( "input/manual_gear_command", QoS{1}, - [this](const GearCommand::SharedPtr msg) { current_manual_gear_cmd_ = *msg; }); + [this](const GearCommand::ConstSharedPtr msg) { current_manual_gear_cmd_ = *msg; }); sub_turn_indicators_cmd_ = create_subscription( "input/turn_indicators_command", QoS{1}, std::bind(&SimplePlanningSimulator::on_turn_indicators_cmd, this, _1)); @@ -251,6 +279,44 @@ rcl_interfaces::msg::SetParametersResult SimplePlanningSimulator::on_parameter( return result; } +double SimplePlanningSimulator::calculate_ego_pitch() const +{ + const double ego_x = vehicle_model_ptr_->getX(); + const double ego_y = vehicle_model_ptr_->getY(); + const double ego_yaw = vehicle_model_ptr_->getYaw(); + + geometry_msgs::msg::Pose ego_pose; + ego_pose.position.x = ego_x; + ego_pose.position.y = ego_y; + ego_pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(ego_yaw); + + // calculate prev/next point of lanelet centerline nearest to ego pose. + lanelet::Lanelet ego_lanelet; + if (!lanelet::utils::query::getClosestLaneletWithConstrains( + road_lanelets_, ego_pose, &ego_lanelet, 2.0, std::numeric_limits::max())) { + return 0.0; + } + const auto centerline_points = convert_centerline_to_points(ego_lanelet); + const size_t ego_seg_idx = + motion_utils::findNearestSegmentIndex(centerline_points, ego_pose.position); + + const auto & prev_point = centerline_points.at(ego_seg_idx); + const auto & next_point = centerline_points.at(ego_seg_idx + 1); + + // calculate ego yaw angle on lanelet coordinates + const double lanelet_yaw = std::atan2(next_point.y - prev_point.y, next_point.x - prev_point.x); + const double ego_yaw_against_lanelet = ego_yaw - lanelet_yaw; + + // calculate ego pitch angle considering ego yaw. + const double diff_z = next_point.z - prev_point.z; + const double diff_xy = std::hypot(next_point.x - prev_point.x, next_point.y - prev_point.y) / + std::cos(ego_yaw_against_lanelet); + const bool reverse_sign = std::cos(ego_yaw_against_lanelet) < 0.0; + const double ego_pitch_angle = + reverse_sign ? -std::atan2(-diff_z, -diff_xy) : -std::atan2(diff_z, diff_xy); + return ego_pitch_angle; +} + void SimplePlanningSimulator::on_timer() { if (!is_initialized_) { @@ -258,16 +324,21 @@ void SimplePlanningSimulator::on_timer() return; } + // calculate longitudinal acceleration by slope + const double ego_pitch_angle = calculate_ego_pitch(); + const double acc_by_slope = + enable_road_slope_simulation_ ? -9.81 * std::sin(ego_pitch_angle) : 0.0; + // update vehicle dynamics { const double dt = delta_time_.get_dt(get_clock()->now()); if (current_control_mode_.mode == ControlModeReport::AUTONOMOUS) { vehicle_model_ptr_->setGear(current_gear_cmd_.command); - set_input(current_ackermann_cmd_); + set_input(current_ackermann_cmd_, acc_by_slope); } else { vehicle_model_ptr_->setGear(current_manual_gear_cmd_.command); - set_input(current_manual_ackermann_cmd_); + set_input(current_manual_ackermann_cmd_, acc_by_slope); } if (simulate_motion_) { @@ -276,7 +347,7 @@ void SimplePlanningSimulator::on_timer() } // set current state - current_odometry_ = to_odometry(vehicle_model_ptr_); + current_odometry_ = to_odometry(vehicle_model_ptr_, ego_pitch_angle); current_odometry_.pose.pose.position.z = get_z_pose_from_trajectory( current_odometry_.pose.pose.position.x, current_odometry_.pose.pose.position.y); @@ -308,6 +379,19 @@ void SimplePlanningSimulator::on_timer() publish_tf(current_odometry_); } +void SimplePlanningSimulator::on_map(const HADMapBin::ConstSharedPtr msg) +{ + auto lanelet_map_ptr = std::make_shared(); + + lanelet::routing::RoutingGraphPtr routing_graph_ptr; + lanelet::traffic_rules::TrafficRulesPtr traffic_rules_ptr; + lanelet::utils::conversion::fromBinMsg( + *msg, lanelet_map_ptr, &traffic_rules_ptr, &routing_graph_ptr); + + lanelet::ConstLanelets all_lanelets = lanelet::utils::query::laneletLayer(lanelet_map_ptr); + road_lanelets_ = lanelet::utils::query::roadLanelets(all_lanelets); +} + void SimplePlanningSimulator::on_initialpose(const PoseWithCovarianceStamped::ConstSharedPtr msg) { // save initial pose @@ -344,7 +428,8 @@ void SimplePlanningSimulator::on_set_pose( response->status = tier4_api_utils::response_success(); } -void SimplePlanningSimulator::set_input(const AckermannControlCommand & cmd) +void SimplePlanningSimulator::set_input( + const AckermannControlCommand & cmd, const double acc_by_slope) { const auto steer = cmd.lateral.steering_tire_angle; const auto vel = cmd.longitudinal.speed; @@ -356,11 +441,11 @@ void SimplePlanningSimulator::set_input(const AckermannControlCommand & cmd) // TODO(Watanabe): The definition of the sign of acceleration in REVERSE mode is different // between .auto and proposal.iv, and will be discussed later. - float acc = accel; + float acc = accel + acc_by_slope; if (gear == GearCommand::NONE) { acc = 0.0; } else if (gear == GearCommand::REVERSE || gear == GearCommand::REVERSE_2) { - acc = -accel; + acc = -accel - acc_by_slope; } if ( @@ -401,7 +486,7 @@ void SimplePlanningSimulator::on_engage(const Engage::ConstSharedPtr msg) } void SimplePlanningSimulator::on_control_mode_request( - const ControlModeCommand::Request::SharedPtr request, + const ControlModeCommand::Request::ConstSharedPtr request, const ControlModeCommand::Response::SharedPtr response) { const auto m = request->mode; @@ -553,6 +638,7 @@ void SimplePlanningSimulator::publish_acceleration() msg.header.frame_id = "/base_link"; msg.header.stamp = get_clock()->now(); msg.accel.accel.linear.x = vehicle_model_ptr_->getAx(); + msg.accel.accel.linear.y = vehicle_model_ptr_->getWz() * vehicle_model_ptr_->getVx(); using COV_IDX = tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; constexpr auto COV = 0.001; @@ -573,6 +659,7 @@ void SimplePlanningSimulator::publish_imu() imu.header.frame_id = "base_link"; imu.header.stamp = now(); imu.linear_acceleration.x = vehicle_model_ptr_->getAx(); + imu.linear_acceleration.y = vehicle_model_ptr_->getWz() * vehicle_model_ptr_->getVx(); constexpr auto COV = 0.001; imu.linear_acceleration_covariance.at(COV_IDX::X_X) = COV; imu.linear_acceleration_covariance.at(COV_IDX::Y_Y) = COV; diff --git a/simulator/simple_planning_simulator/test/test_simple_planning_simulator.cpp b/simulator/simple_planning_simulator/test/test_simple_planning_simulator.cpp index e57a3683b97a9..cedfec395110e 100644 --- a/simulator/simple_planning_simulator/test/test_simple_planning_simulator.cpp +++ b/simulator/simple_planning_simulator/test/test_simple_planning_simulator.cpp @@ -49,7 +49,7 @@ class PubSubNode : public rclcpp::Node { current_odom_sub_ = create_subscription( "output/odometry", rclcpp::QoS{1}, - [this](const Odometry::SharedPtr msg) { current_odom_ = msg; }); + [this](const Odometry::ConstSharedPtr msg) { current_odom_ = msg; }); pub_ackermann_command_ = create_publisher("input/ackermann_control_command", rclcpp::QoS{1}); pub_initialpose_ = @@ -62,7 +62,7 @@ class PubSubNode : public rclcpp::Node rclcpp::Publisher::SharedPtr pub_gear_cmd_; rclcpp::Publisher::SharedPtr pub_initialpose_; - Odometry::SharedPtr current_odom_; + Odometry::ConstSharedPtr current_odom_; }; /** diff --git a/system/autoware_auto_msgs_adapter/CMakeLists.txt b/system/autoware_auto_msgs_adapter/CMakeLists.txt index 9b48112227cbe..8b1d31ff2d01b 100644 --- a/system/autoware_auto_msgs_adapter/CMakeLists.txt +++ b/system/autoware_auto_msgs_adapter/CMakeLists.txt @@ -12,6 +12,8 @@ ament_auto_add_library(${NODE_NAME} src/include/adapter_base.hpp src/include/adapter_base_interface.hpp src/include/adapter_control.hpp + src/include/adapter_map.hpp + src/include/adapter_perception.hpp src/include/autoware_auto_msgs_adapter_core.hpp src/autoware_auto_msgs_adapter_core.cpp) diff --git a/system/autoware_auto_msgs_adapter/config/adapter_map.param.yaml b/system/autoware_auto_msgs_adapter/config/adapter_map.param.yaml new file mode 100644 index 0000000000000..dcaa49e089b44 --- /dev/null +++ b/system/autoware_auto_msgs_adapter/config/adapter_map.param.yaml @@ -0,0 +1,5 @@ +/**: + ros__parameters: + msg_type_target: "autoware_auto_mapping_msgs/msg/HADMapBin" + topic_name_source: "/map/vector_map" + topic_name_target: "/map/vector_map_auto" diff --git a/system/autoware_auto_msgs_adapter/config/adapter_perception.param.yaml b/system/autoware_auto_msgs_adapter/config/adapter_perception.param.yaml new file mode 100644 index 0000000000000..d8b6e31543fad --- /dev/null +++ b/system/autoware_auto_msgs_adapter/config/adapter_perception.param.yaml @@ -0,0 +1,5 @@ +/**: + ros__parameters: + msg_type_target: "autoware_auto_perception_msgs/msg/PredictedObjects" + topic_name_source: "/perception/object_recognition/objects" + topic_name_target: "/perception/object_recognition/objects_auto" diff --git a/system/autoware_auto_msgs_adapter/launch/autoware_auto_msgs_adapter.launch.xml b/system/autoware_auto_msgs_adapter/launch/autoware_auto_msgs_adapter.launch.xml index 89b58e2d60c28..d80c1bd8cbedf 100755 --- a/system/autoware_auto_msgs_adapter/launch/autoware_auto_msgs_adapter.launch.xml +++ b/system/autoware_auto_msgs_adapter/launch/autoware_auto_msgs_adapter.launch.xml @@ -1,8 +1,16 @@ - + + + - - + + + + + + + + diff --git a/system/autoware_auto_msgs_adapter/package.xml b/system/autoware_auto_msgs_adapter/package.xml index 0deba020ad7a3..99a94fa043565 100644 --- a/system/autoware_auto_msgs_adapter/package.xml +++ b/system/autoware_auto_msgs_adapter/package.xml @@ -10,8 +10,6 @@ Apache License 2.0 - M. Fatih Cırıt - ament_cmake_auto autoware_cmake @@ -22,7 +20,11 @@ autoware_lint_common autoware_auto_control_msgs + autoware_auto_mapping_msgs + autoware_auto_perception_msgs autoware_control_msgs + autoware_map_msgs + autoware_perception_msgs rclcpp rclcpp_components diff --git a/system/autoware_auto_msgs_adapter/schema/autoware_auto_msgs_adapter.schema.json b/system/autoware_auto_msgs_adapter/schema/autoware_auto_msgs_adapter.schema.json index cf853ee1da130..f6aa87f774528 100644 --- a/system/autoware_auto_msgs_adapter/schema/autoware_auto_msgs_adapter.schema.json +++ b/system/autoware_auto_msgs_adapter/schema/autoware_auto_msgs_adapter.schema.json @@ -9,7 +9,11 @@ "msg_type_target": { "type": "string", "description": "Target message type", - "enum": ["autoware_auto_control_msgs/msg/AckermannControlCommand"], + "enum": [ + "autoware_auto_control_msgs/msg/AckermannControlCommand", + "autoware_auto_mapping_msgs/msg/HADMapBin", + "autoware_auto_perception_msgs/msg/PredictedObjects" + ], "default": "autoware_auto_control_msgs/msg/AckermannControlCommand" }, "topic_name_source": { diff --git a/system/autoware_auto_msgs_adapter/src/autoware_auto_msgs_adapter_core.cpp b/system/autoware_auto_msgs_adapter/src/autoware_auto_msgs_adapter_core.cpp index 98fe916903a60..15e3c90d227dd 100644 --- a/system/autoware_auto_msgs_adapter/src/autoware_auto_msgs_adapter_core.cpp +++ b/system/autoware_auto_msgs_adapter/src/autoware_auto_msgs_adapter_core.cpp @@ -57,6 +57,16 @@ MapStringAdapter AutowareAutoMsgsAdapterNode::create_adapter_map( return std::static_pointer_cast( std::make_shared(*this, topic_name_source, topic_name_target)); }}, + {"autoware_auto_mapping_msgs/msg/HADMapBin", + [&] { + return std::static_pointer_cast( + std::make_shared(*this, topic_name_source, topic_name_target)); + }}, + {"autoware_auto_perception_msgs/msg/PredictedObjects", + [&] { + return std::static_pointer_cast( + std::make_shared(*this, topic_name_source, topic_name_target)); + }}, }; } diff --git a/system/autoware_auto_msgs_adapter/src/include/adapter_map.hpp b/system/autoware_auto_msgs_adapter/src/include/adapter_map.hpp new file mode 100644 index 0000000000000..8150b7d7dba08 --- /dev/null +++ b/system/autoware_auto_msgs_adapter/src/include/adapter_map.hpp @@ -0,0 +1,59 @@ +// Copyright 2023 The 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 ADAPTER_MAP_HPP_ +#define ADAPTER_MAP_HPP_ + +#include "adapter_base.hpp" + +#include + +#include +#include + +#include + +namespace autoware_auto_msgs_adapter +{ +using autoware_auto_mapping_msgs::msg::HADMapBin; +using autoware_map_msgs::msg::LaneletMapBin; + +class AdapterMap : public autoware_auto_msgs_adapter::AdapterBase +{ +public: + AdapterMap( + rclcpp::Node & node, const std::string & topic_name_source, + const std::string & topic_name_target, const rclcpp::QoS & qos = rclcpp::QoS{1}) + : AdapterBase(node, topic_name_source, topic_name_target, qos) + { + RCLCPP_DEBUG( + node.get_logger(), "AdapterMap is initialized to convert: %s -> %s", + topic_name_source.c_str(), topic_name_target.c_str()); + } + +protected: + HADMapBin convert(const LaneletMapBin & msg_source) override + { + autoware_auto_mapping_msgs::msg::HADMapBin msg_auto; + msg_auto.header = msg_source.header; + msg_auto.format_version = msg_source.version_map_format; + msg_auto.map_version = msg_source.version_map; + msg_auto.map_format = 0; + msg_auto.data = msg_source.data; + + return msg_auto; + } +}; +} // namespace autoware_auto_msgs_adapter + +#endif // ADAPTER_MAP_HPP_ diff --git a/system/autoware_auto_msgs_adapter/src/include/adapter_perception.hpp b/system/autoware_auto_msgs_adapter/src/include/adapter_perception.hpp new file mode 100644 index 0000000000000..3821bbad8ce38 --- /dev/null +++ b/system/autoware_auto_msgs_adapter/src/include/adapter_perception.hpp @@ -0,0 +1,97 @@ +// Copyright 2023 The 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 ADAPTER_PERCEPTION_HPP_ +#define ADAPTER_PERCEPTION_HPP_ + +#include "adapter_base.hpp" + +#include + +#include +#include + +#include + +namespace autoware_auto_msgs_adapter +{ +using Objects_auto = autoware_auto_perception_msgs::msg::PredictedObjects; +using Objects = autoware_perception_msgs::msg::PredictedObjects; + +class AdapterPerception : public autoware_auto_msgs_adapter::AdapterBase +{ +public: + AdapterPerception( + rclcpp::Node & node, const std::string & topic_name_source, + const std::string & topic_name_target, const rclcpp::QoS & qos = rclcpp::QoS{1}) + : AdapterBase(node, topic_name_source, topic_name_target, qos) + { + RCLCPP_DEBUG( + node.get_logger(), "AdapterPerception is initialized to convert: %s -> %s", + topic_name_source.c_str(), topic_name_target.c_str()); + } + +protected: + Objects_auto convert(const Objects & msg_source) override + { + Objects_auto msg_auto; + msg_auto.header = msg_source.header; + + autoware_auto_perception_msgs::msg::PredictedObject object_auto; + for (size_t it_of_objects = 0; it_of_objects < msg_source.objects.size(); it_of_objects++) { + // convert id and probability + object_auto.object_id = msg_source.objects[it_of_objects].object_id; + object_auto.existence_probability = msg_source.objects[it_of_objects].existence_probability; + // convert classification + autoware_auto_perception_msgs::msg::ObjectClassification classification; + for (size_t i = 0; i < msg_source.objects[it_of_objects].classification.size(); i++) { + classification.label = msg_source.objects[it_of_objects].classification[i].label; + classification.probability = + msg_source.objects[it_of_objects].classification[i].probability; + object_auto.classification.push_back(classification); + } + // convert kinematics + object_auto.kinematics.initial_pose_with_covariance = + msg_source.objects[it_of_objects].kinematics.initial_pose_with_covariance; + object_auto.kinematics.initial_twist_with_covariance = + msg_source.objects[it_of_objects].kinematics.initial_twist_with_covariance; + object_auto.kinematics.initial_acceleration_with_covariance = + msg_source.objects[it_of_objects].kinematics.initial_acceleration_with_covariance; + for (size_t j = 0; j < msg_source.objects[it_of_objects].kinematics.predicted_paths.size(); + j++) { + autoware_auto_perception_msgs::msg::PredictedPath predicted_path; + for (size_t k = 0; + k < msg_source.objects[it_of_objects].kinematics.predicted_paths[j].path.size(); k++) { + predicted_path.path.push_back( + msg_source.objects[it_of_objects].kinematics.predicted_paths[j].path[k]); + } + predicted_path.time_step = + msg_source.objects[it_of_objects].kinematics.predicted_paths[j].time_step; + predicted_path.confidence = + msg_source.objects[it_of_objects].kinematics.predicted_paths[j].confidence; + object_auto.kinematics.predicted_paths.push_back(predicted_path); + } + // convert shape + object_auto.shape.type = msg_source.objects[it_of_objects].shape.type; + object_auto.shape.footprint = msg_source.objects[it_of_objects].shape.footprint; + object_auto.shape.dimensions = msg_source.objects[it_of_objects].shape.dimensions; + + // add to objects list + msg_auto.objects.push_back(object_auto); + } + return msg_auto; + } +}; +} // namespace autoware_auto_msgs_adapter + +#endif // ADAPTER_PERCEPTION_HPP_ diff --git a/system/autoware_auto_msgs_adapter/src/include/autoware_auto_msgs_adapter_core.hpp b/system/autoware_auto_msgs_adapter/src/include/autoware_auto_msgs_adapter_core.hpp index a33a88c68618f..f0e28f48aacd8 100644 --- a/system/autoware_auto_msgs_adapter/src/include/autoware_auto_msgs_adapter_core.hpp +++ b/system/autoware_auto_msgs_adapter/src/include/autoware_auto_msgs_adapter_core.hpp @@ -15,6 +15,8 @@ #define AUTOWARE_AUTO_MSGS_ADAPTER_CORE_HPP_ #include "adapter_control.hpp" +#include "adapter_map.hpp" +#include "adapter_perception.hpp" #include diff --git a/system/autoware_auto_msgs_adapter/test/test_msg_ackermann_control_command.cpp b/system/autoware_auto_msgs_adapter/test/test_msg_ackermann_control_command.cpp index e33ae07e6aca1..430b9a26b60e1 100644 --- a/system/autoware_auto_msgs_adapter/test/test_msg_ackermann_control_command.cpp +++ b/system/autoware_auto_msgs_adapter/test/test_msg_ackermann_control_command.cpp @@ -112,5 +112,5 @@ TEST(AutowareAutoMsgsAdapter, TestMsgAckermannControlCommand) // NOLINT for gte EXPECT_TRUE(test_completed); EXPECT_FALSE(timed_out); - rclcpp::shutdown(); + // rclcpp::shutdown(); } diff --git a/system/autoware_auto_msgs_adapter/test/test_msg_had_map_bin.cpp b/system/autoware_auto_msgs_adapter/test/test_msg_had_map_bin.cpp new file mode 100644 index 0000000000000..b312bd144f6f3 --- /dev/null +++ b/system/autoware_auto_msgs_adapter/test/test_msg_had_map_bin.cpp @@ -0,0 +1,104 @@ +// Copyright 2023 The 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 + +#include + +#include + +autoware_map_msgs::msg::LaneletMapBin generate_map_msg() +{ + // generate deterministic random int + std::mt19937 gen(0); + std::uniform_int_distribution<> dis_int(0, 1000000); + auto rand_int = [&dis_int, &gen]() { return dis_int(gen); }; + + autoware_map_msgs::msg::LaneletMapBin msg_map; + msg_map.header.stamp = rclcpp::Time(rand_int()); + msg_map.header.frame_id = "test_frame"; + + msg_map.version_map_format = "1.1.1"; + msg_map.version_map = "1.0.0"; + msg_map.name_map = "florence-prato-city-center"; + msg_map.data.push_back(rand_int()); + + return msg_map; +} + +TEST(AutowareAutoMsgsAdapter, TestHADMapBin) // NOLINT for gtest +{ + const std::string msg_type_target = "autoware_auto_mapping_msgs/msg/HADMapBin"; + const std::string topic_name_source = "topic_name_source"; + const std::string topic_name_target = "topic_name_target"; + + std::cout << "Creating the adapter node..." << std::endl; + + rclcpp::NodeOptions node_options; + node_options.append_parameter_override("msg_type_target", msg_type_target); + node_options.append_parameter_override("topic_name_source", topic_name_source); + node_options.append_parameter_override("topic_name_target", topic_name_target); + + using autoware_auto_msgs_adapter::AutowareAutoMsgsAdapterNode; + AutowareAutoMsgsAdapterNode::SharedPtr node_adapter; + node_adapter = std::make_shared(node_options); + + std::cout << "Creating the subscriber node..." << std::endl; + + auto node_subscriber = std::make_shared("node_subscriber", rclcpp::NodeOptions{}); + + bool test_completed = false; + + const auto msg_map = generate_map_msg(); + auto sub = node_subscriber->create_subscription( + topic_name_target, rclcpp::QoS{1}, + [&msg_map, &test_completed](const autoware_auto_mapping_msgs::msg::HADMapBin::SharedPtr msg) { + EXPECT_EQ(msg->header.stamp, msg_map.header.stamp); + EXPECT_EQ(msg->header.frame_id, msg_map.header.frame_id); + + EXPECT_EQ(msg->map_format, 0); + EXPECT_EQ(msg->format_version, msg_map.version_map_format); + EXPECT_EQ(msg->map_version, msg_map.version_map); + EXPECT_EQ(msg->data, msg_map.data); + + test_completed = true; + }); + + std::cout << "Creating the publisher node..." << std::endl; + + auto node_publisher = std::make_shared("node_publisher", rclcpp::NodeOptions{}); + auto pub = node_publisher->create_publisher( + topic_name_source, rclcpp::QoS{1}); + pub->publish(msg_map); + + auto start_time = std::chrono::system_clock::now(); + auto max_test_dur = std::chrono::seconds(5); + auto timed_out = false; + + while (rclcpp::ok() && !test_completed) { + rclcpp::spin_some(node_subscriber); + rclcpp::spin_some(node_adapter); + rclcpp::spin_some(node_publisher); + rclcpp::sleep_for(std::chrono::milliseconds(50)); + if (std::chrono::system_clock::now() - start_time > max_test_dur) { + timed_out = true; + break; + } + } + + EXPECT_TRUE(test_completed); + EXPECT_FALSE(timed_out); + + // rclcpp::shutdown(); +} diff --git a/system/autoware_auto_msgs_adapter/test/test_msg_predicted_objects.cpp b/system/autoware_auto_msgs_adapter/test/test_msg_predicted_objects.cpp new file mode 100644 index 0000000000000..9f7f0ce8056f5 --- /dev/null +++ b/system/autoware_auto_msgs_adapter/test/test_msg_predicted_objects.cpp @@ -0,0 +1,267 @@ +// Copyright 2023 The 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 + +#include + +#include + +autoware_perception_msgs::msg::PredictedObjects generate_perception_msg() +{ + // generate deterministic random int + std::mt19937 gen(0); + std::uniform_int_distribution<> dis_int(0, 1000000); + auto rand_int = [&dis_int, &gen]() { return dis_int(gen); }; + + autoware_perception_msgs::msg::PredictedObjects msg_perception; + msg_perception.header.stamp = rclcpp::Time(rand_int()); + msg_perception.header.frame_id = "test_frame"; + + autoware_perception_msgs::msg::PredictedObject obj; + // // { + unique_identifier_msgs::msg::UUID uuid_; + std::independent_bits_engine bit_eng(gen); + std::generate(uuid_.uuid.begin(), uuid_.uuid.end(), bit_eng); + obj.object_id = uuid_; + obj.existence_probability = 0.5; + autoware_perception_msgs::msg::ObjectClassification obj_class; + obj_class.label = 0; + obj_class.probability = 0.5; + obj.classification.push_back(obj_class); + + // { + autoware_perception_msgs::msg::PredictedObjectKinematics kin; + kin.initial_pose_with_covariance.pose.position.x = 10; + kin.initial_pose_with_covariance.pose.position.y = 10; + kin.initial_pose_with_covariance.pose.position.z = 0; + kin.initial_pose_with_covariance.pose.orientation.x = 0; + kin.initial_pose_with_covariance.pose.orientation.y = 0; + kin.initial_pose_with_covariance.pose.orientation.z = 0; + kin.initial_pose_with_covariance.pose.orientation.w = 1; + + kin.initial_twist_with_covariance.twist.linear.x = 1; + kin.initial_twist_with_covariance.twist.linear.y = 0; + kin.initial_twist_with_covariance.twist.linear.z = 0; + kin.initial_twist_with_covariance.twist.angular.x = 0; + kin.initial_twist_with_covariance.twist.angular.y = 0; + kin.initial_twist_with_covariance.twist.angular.z = 0; + + kin.initial_acceleration_with_covariance.accel.linear.x = 0; + kin.initial_acceleration_with_covariance.accel.linear.y = 0; + kin.initial_acceleration_with_covariance.accel.linear.z = 0; + kin.initial_acceleration_with_covariance.accel.angular.x = 0; + kin.initial_acceleration_with_covariance.accel.angular.y = 0; + kin.initial_acceleration_with_covariance.accel.angular.z = 0; + + for (size_t i = 0; i < 10; i++) { + kin.predicted_paths[0].path[i].position.x = i; + kin.predicted_paths[0].path[i].position.y = 0; + kin.predicted_paths[0].path[i].position.z = 0; + } + obj.kinematics = kin; + // } + // { + autoware_perception_msgs::msg::Shape s; + s.type = 1; + geometry_msgs::msg::Point32 p; + p.x = 9.0f; + p.y = 11.0f; + p.z = 0.0f; + s.footprint.points.push_back(p); + p.x = 11.0f; + p.y = 11.0f; + p.z = 0.0f; + s.footprint.points.push_back(p); + p.x = 11.0f; + p.y = 9.0f; + p.z = 0.0f; + s.footprint.points.push_back(p); + p.x = 9.0f; + p.y = 9.0f; + p.z = 0.0f; + s.footprint.points.push_back(p); + + s.dimensions.x = 2; + s.dimensions.y = 2; + s.dimensions.z = 2; + + obj.shape = s; + // } + // } + + msg_perception.objects.push_back(obj); + return msg_perception; +} + +TEST(AutowareAutoMsgsAdapter, TestPredictedObjects) // NOLINT for gtest +{ + const std::string msg_type_target = "autoware_auto_perception_msgs/msg/PredictedObjects"; + const std::string topic_name_source = "topic_name_source"; + const std::string topic_name_target = "topic_name_target"; + + std::cout << "Creating the adapter node..." << std::endl; + + rclcpp::NodeOptions node_options; + node_options.append_parameter_override("msg_type_target", msg_type_target); + node_options.append_parameter_override("topic_name_source", topic_name_source); + node_options.append_parameter_override("topic_name_target", topic_name_target); + + using autoware_auto_msgs_adapter::AutowareAutoMsgsAdapterNode; + AutowareAutoMsgsAdapterNode::SharedPtr node_adapter; + node_adapter = std::make_shared(node_options); + + std::cout << "Creating the subscriber node..." << std::endl; + + auto node_subscriber = std::make_shared("node_subscriber", rclcpp::NodeOptions{}); + + bool test_completed = false; + + const auto msg_perception = generate_perception_msg(); + auto sub = node_subscriber->create_subscription< + autoware_auto_perception_msgs::msg::PredictedObjects>( + topic_name_target, rclcpp::QoS{1}, + [&msg_perception, + &test_completed](const autoware_auto_perception_msgs::msg::PredictedObjects::SharedPtr msg) { + EXPECT_EQ(msg->header.stamp, msg_perception.header.stamp); + EXPECT_EQ(msg->header.frame_id, msg_perception.header.frame_id); + EXPECT_EQ(msg->objects[0].object_id.uuid, msg_perception.objects[0].object_id.uuid); + EXPECT_FLOAT_EQ( + msg->objects[0].existence_probability, msg_perception.objects[0].existence_probability); + EXPECT_EQ( + msg->objects[0].classification[0].label, msg_perception.objects[0].classification[0].label); + EXPECT_FLOAT_EQ( + msg->objects[0].classification[0].probability, + msg_perception.objects[0].classification[0].probability); + EXPECT_FLOAT_EQ( + msg->objects[0].kinematics.initial_pose_with_covariance.pose.position.x, + msg_perception.objects[0].kinematics.initial_pose_with_covariance.pose.position.x); + EXPECT_FLOAT_EQ( + msg->objects[0].kinematics.initial_pose_with_covariance.pose.position.y, + msg_perception.objects[0].kinematics.initial_pose_with_covariance.pose.position.y); + EXPECT_FLOAT_EQ( + msg->objects[0].kinematics.initial_pose_with_covariance.pose.position.z, + msg_perception.objects[0].kinematics.initial_pose_with_covariance.pose.position.z); + EXPECT_FLOAT_EQ( + msg->objects[0].kinematics.initial_pose_with_covariance.pose.orientation.x, + msg_perception.objects[0].kinematics.initial_pose_with_covariance.pose.orientation.x); + EXPECT_FLOAT_EQ( + msg->objects[0].kinematics.initial_pose_with_covariance.pose.orientation.y, + msg_perception.objects[0].kinematics.initial_pose_with_covariance.pose.orientation.y); + EXPECT_FLOAT_EQ( + msg->objects[0].kinematics.initial_pose_with_covariance.pose.orientation.z, + msg_perception.objects[0].kinematics.initial_pose_with_covariance.pose.orientation.z); + EXPECT_FLOAT_EQ( + msg->objects[0].kinematics.initial_pose_with_covariance.pose.orientation.w, + msg_perception.objects[0].kinematics.initial_pose_with_covariance.pose.orientation.w); + + EXPECT_FLOAT_EQ( + msg->objects[0].kinematics.initial_twist_with_covariance.twist.linear.x, + msg_perception.objects[0].kinematics.initial_twist_with_covariance.twist.linear.x); + EXPECT_FLOAT_EQ( + msg->objects[0].kinematics.initial_twist_with_covariance.twist.linear.y, + msg_perception.objects[0].kinematics.initial_twist_with_covariance.twist.linear.y); + EXPECT_FLOAT_EQ( + msg->objects[0].kinematics.initial_twist_with_covariance.twist.linear.z, + msg_perception.objects[0].kinematics.initial_twist_with_covariance.twist.linear.z); + EXPECT_FLOAT_EQ( + msg->objects[0].kinematics.initial_twist_with_covariance.twist.angular.x, + msg_perception.objects[0].kinematics.initial_twist_with_covariance.twist.angular.x); + EXPECT_FLOAT_EQ( + msg->objects[0].kinematics.initial_twist_with_covariance.twist.angular.y, + msg_perception.objects[0].kinematics.initial_twist_with_covariance.twist.angular.y); + EXPECT_FLOAT_EQ( + msg->objects[0].kinematics.initial_twist_with_covariance.twist.angular.z, + msg_perception.objects[0].kinematics.initial_twist_with_covariance.twist.angular.z); + + EXPECT_FLOAT_EQ( + msg->objects[0].kinematics.initial_acceleration_with_covariance.accel.linear.x, + msg_perception.objects[0].kinematics.initial_acceleration_with_covariance.accel.linear.x); + EXPECT_FLOAT_EQ( + msg->objects[0].kinematics.initial_acceleration_with_covariance.accel.linear.y, + msg_perception.objects[0].kinematics.initial_acceleration_with_covariance.accel.linear.y); + EXPECT_FLOAT_EQ( + msg->objects[0].kinematics.initial_acceleration_with_covariance.accel.linear.z, + msg_perception.objects[0].kinematics.initial_acceleration_with_covariance.accel.linear.z); + EXPECT_FLOAT_EQ( + msg->objects[0].kinematics.initial_acceleration_with_covariance.accel.angular.x, + msg_perception.objects[0].kinematics.initial_acceleration_with_covariance.accel.angular.x); + EXPECT_FLOAT_EQ( + msg->objects[0].kinematics.initial_acceleration_with_covariance.accel.angular.y, + msg_perception.objects[0].kinematics.initial_acceleration_with_covariance.accel.angular.y); + EXPECT_FLOAT_EQ( + msg->objects[0].kinematics.initial_acceleration_with_covariance.accel.angular.z, + msg_perception.objects[0].kinematics.initial_acceleration_with_covariance.accel.angular.z); + + for (size_t i = 0; i < msg->objects[0].kinematics.predicted_paths[0].path.size(); i++) { + EXPECT_FLOAT_EQ( + msg->objects[0].kinematics.predicted_paths[0].path[i].position.x, + msg_perception.objects[0].kinematics.predicted_paths[0].path[i].position.x); + EXPECT_FLOAT_EQ( + msg->objects[0].kinematics.predicted_paths[0].path[i].position.y, + msg_perception.objects[0].kinematics.predicted_paths[0].path[i].position.y); + EXPECT_FLOAT_EQ( + msg->objects[0].kinematics.predicted_paths[0].path[i].position.z, + msg_perception.objects[0].kinematics.predicted_paths[0].path[i].position.z); + } + + EXPECT_EQ(msg->objects[0].shape.type, msg_perception.objects[0].shape.type); + for (size_t i = 0; i < msg_perception.objects[0].shape.footprint.points.size(); i++) { + EXPECT_FLOAT_EQ( + msg->objects[0].shape.footprint.points[i].x, + msg_perception.objects[0].shape.footprint.points[i].x); + EXPECT_FLOAT_EQ( + msg->objects[0].shape.footprint.points[i].y, + msg_perception.objects[0].shape.footprint.points[i].y); + EXPECT_FLOAT_EQ( + msg->objects[0].shape.footprint.points[i].z, + msg_perception.objects[0].shape.footprint.points[i].z); + } + EXPECT_FLOAT_EQ( + msg->objects[0].shape.dimensions.x, msg_perception.objects[0].shape.dimensions.x); + EXPECT_FLOAT_EQ( + msg->objects[0].shape.dimensions.y, msg_perception.objects[0].shape.dimensions.y); + EXPECT_FLOAT_EQ( + msg->objects[0].shape.dimensions.z, msg_perception.objects[0].shape.dimensions.z); + + test_completed = true; + }); + + std::cout << "Creating the publisher node..." << std::endl; + + auto node_publisher = std::make_shared("node_publisher", rclcpp::NodeOptions{}); + auto pub = node_publisher->create_publisher( + topic_name_source, rclcpp::QoS{1}); + pub->publish(msg_perception); + + auto start_time = std::chrono::system_clock::now(); + auto max_test_dur = std::chrono::seconds(5); + auto timed_out = false; + + while (rclcpp::ok() && !test_completed) { + rclcpp::spin_some(node_subscriber); + rclcpp::spin_some(node_adapter); + rclcpp::spin_some(node_publisher); + rclcpp::sleep_for(std::chrono::milliseconds(50)); + if (std::chrono::system_clock::now() - start_time > max_test_dur) { + timed_out = true; + break; + } + } + + EXPECT_TRUE(test_completed); + EXPECT_FALSE(timed_out); + + // rclcpp::shutdown(); +} diff --git a/system/default_ad_api/package.xml b/system/default_ad_api/package.xml index dfceeabe41122..6a9a22f39cdc4 100644 --- a/system/default_ad_api/package.xml +++ b/system/default_ad_api/package.xml @@ -23,7 +23,8 @@ autoware_planning_msgs component_interface_specs component_interface_utils - lanelet2_extension + geographic_msgs + geography_utils motion_utils nav_msgs rclcpp diff --git a/system/default_ad_api/src/operation_mode.cpp b/system/default_ad_api/src/operation_mode.cpp index a61a1b75697ab..829585ed4b8b4 100644 --- a/system/default_ad_api/src/operation_mode.cpp +++ b/system/default_ad_api/src/operation_mode.cpp @@ -46,12 +46,11 @@ OperationModeNode::OperationModeNode(const rclcpp::NodeOptions & options) for (size_t i = 0; i < module_names.size(); ++i) { const auto name = "/system/component_state_monitor/component/autonomous/" + module_names[i]; const auto qos = rclcpp::QoS(1).transient_local(); - const auto callback = [this, i](const ModeChangeAvailable::ConstSharedPtr msg) { - module_states_[i] = msg->available; + const auto callback = [this, i, module_names](const ModeChangeAvailable::ConstSharedPtr msg) { + module_states_[module_names[i]] = msg->available; }; sub_module_states_.push_back(create_subscription(name, qos, callback)); } - module_states_.resize(module_names.size()); timer_ = rclcpp::create_timer( this, get_clock(), rclcpp::Rate(5.0).period(), std::bind(&OperationModeNode::on_timer, this)); @@ -137,11 +136,22 @@ void OperationModeNode::on_state(const OperationModeState::Message::ConstSharedP void OperationModeNode::on_timer() { bool autonomous_available = true; + std::string unhealthy_components = ""; for (const auto & state : module_states_) { - autonomous_available &= state; + if (!state.second) { + unhealthy_components += unhealthy_components.empty() ? state.first : ", " + state.first; + } + autonomous_available &= state.second; } mode_available_[OperationModeState::Message::AUTONOMOUS] = autonomous_available; + if (!unhealthy_components.empty()) { + RCLCPP_INFO_THROTTLE( + get_logger(), *get_clock(), 3000, + "%s component state is unhealthy. Autonomous is not available.", + unhealthy_components.c_str()); + } + update_state(); } diff --git a/system/default_ad_api/src/operation_mode.hpp b/system/default_ad_api/src/operation_mode.hpp index 5f5699f42cab7..1830b7041b566 100644 --- a/system/default_ad_api/src/operation_mode.hpp +++ b/system/default_ad_api/src/operation_mode.hpp @@ -20,6 +20,7 @@ #include #include +#include #include #include @@ -65,7 +66,7 @@ class OperationModeNode : public rclcpp::Node Cli cli_mode_; Cli cli_control_; - std::vector module_states_; + std::unordered_map module_states_; std::vector::SharedPtr> sub_module_states_; void on_change_to_stop( diff --git a/system/default_ad_api/src/planning.cpp b/system/default_ad_api/src/planning.cpp index 46c8ea4125c9a..5df76e9b5f3cc 100644 --- a/system/default_ad_api/src/planning.cpp +++ b/system/default_ad_api/src/planning.cpp @@ -14,6 +14,8 @@ #include "planning.hpp" +#include + #include #include #include diff --git a/system/default_ad_api/src/planning.hpp b/system/default_ad_api/src/planning.hpp index 03b45fd3b5d41..b533390dcc6fb 100644 --- a/system/default_ad_api/src/planning.hpp +++ b/system/default_ad_api/src/planning.hpp @@ -18,7 +18,7 @@ #include #include #include -#include +#include #include #include diff --git a/system/default_ad_api/src/vehicle.cpp b/system/default_ad_api/src/vehicle.cpp index 61d5b2dde6103..e25933e6ca2ad 100644 --- a/system/default_ad_api/src/vehicle.cpp +++ b/system/default_ad_api/src/vehicle.cpp @@ -14,6 +14,11 @@ #include "vehicle.hpp" +#include +#include + +#include + #include namespace default_ad_api @@ -89,15 +94,6 @@ void VehicleNode::kinematic_state( kinematic_state_msgs_ = msg_ptr; } -Eigen::Vector3d VehicleNode::toBasicPoint3dPt(const geometry_msgs::msg::Point src) -{ - Eigen::Vector3d dst; - dst.x() = src.x; - dst.y() = src.y; - dst.z() = src.z; - return dst; -} - void VehicleNode::acceleration_status( const localization_interface::Acceleration::Message::ConstSharedPtr msg_ptr) { @@ -146,26 +142,16 @@ void VehicleNode::publish_kinematics() vehicle_kinematics.twist.header = kinematic_state_msgs_->header; vehicle_kinematics.twist.header.frame_id = kinematic_state_msgs_->child_frame_id; vehicle_kinematics.twist.twist = kinematic_state_msgs_->twist; - if (map_projector_info_->type == "MGRS") { - lanelet::GPSPoint projected_gps_point = lanelet::projection::MGRSProjector::reverse( - toBasicPoint3dPt(kinematic_state_msgs_->pose.pose.position), map_projector_info_->mgrs_grid); - vehicle_kinematics.geographic_pose.header = kinematic_state_msgs_->header; - vehicle_kinematics.geographic_pose.header.frame_id = "global"; - vehicle_kinematics.geographic_pose.position.latitude = projected_gps_point.lat; - vehicle_kinematics.geographic_pose.position.longitude = projected_gps_point.lon; - vehicle_kinematics.geographic_pose.position.altitude = projected_gps_point.ele; - } else if (map_projector_info_->type == "LocalCartesianUTM") { - lanelet::GPSPoint position{ - map_projector_info_->map_origin.latitude, map_projector_info_->map_origin.longitude}; - lanelet::Origin origin{position}; - lanelet::projection::UtmProjector projector{origin}; - lanelet::GPSPoint projected_gps_point = - projector.reverse(toBasicPoint3dPt(kinematic_state_msgs_->pose.pose.position)); + if (map_projector_info_->projector_type != MapProjectorInfo::LOCAL) { + const geographic_msgs::msg::GeoPoint projected_gps_point = geography_utils::project_reverse( + kinematic_state_msgs_->pose.pose.position, *map_projector_info_); vehicle_kinematics.geographic_pose.header = kinematic_state_msgs_->header; vehicle_kinematics.geographic_pose.header.frame_id = "global"; - vehicle_kinematics.geographic_pose.position.latitude = projected_gps_point.lat; - vehicle_kinematics.geographic_pose.position.longitude = projected_gps_point.lon; - vehicle_kinematics.geographic_pose.position.altitude = projected_gps_point.ele; + vehicle_kinematics.geographic_pose.position.latitude = projected_gps_point.latitude; + vehicle_kinematics.geographic_pose.position.longitude = projected_gps_point.longitude; + vehicle_kinematics.geographic_pose.position.altitude = geography_utils::convert_height( + projected_gps_point.altitude, projected_gps_point.latitude, projected_gps_point.longitude, + map_projector_info_->vertical_datum, MapProjectorInfo::WGS84); } else { vehicle_kinematics.geographic_pose.position.latitude = std::numeric_limits::quiet_NaN(); vehicle_kinematics.geographic_pose.position.longitude = diff --git a/system/default_ad_api/src/vehicle.hpp b/system/default_ad_api/src/vehicle.hpp index 90f8b686206c5..56012152a132b 100644 --- a/system/default_ad_api/src/vehicle.hpp +++ b/system/default_ad_api/src/vehicle.hpp @@ -15,8 +15,6 @@ #ifndef VEHICLE_HPP_ #define VEHICLE_HPP_ -#include "lanelet2_extension/projection/mgrs_projector.hpp" - #include #include #include @@ -27,9 +25,6 @@ #include #include -#include -#include - #include // This file should be included after messages. @@ -83,7 +78,6 @@ class VehicleNode : public rclcpp::Node void publish_kinematics(); void publish_status(); void on_timer(); - Eigen::Vector3d toBasicPoint3dPt(const geometry_msgs::msg::Point src); }; } // namespace default_ad_api diff --git a/system/default_ad_api_helpers/ad_api_adaptors/README.md b/system/default_ad_api_helpers/ad_api_adaptors/README.md index 121befb68c88a..2e299fd7a2e51 100644 --- a/system/default_ad_api_helpers/ad_api_adaptors/README.md +++ b/system/default_ad_api_helpers/ad_api_adaptors/README.md @@ -21,9 +21,11 @@ The clear API is called automatically before setting the route. | Interface | Local Name | Global Name | Description | | ------------ | ------------------ | ------------------------------------- | -------------------------------------------------- | +| Subscription | - | /api/routing/state | The state of the routing API. | | Subscription | ~/input/fixed_goal | /planning/mission_planning/goal | The goal pose of route. Disable goal modification. | | Subscription | ~/input/rough_goal | /rviz/routing/rough_goal | The goal pose of route. Enable goal modification. | | Subscription | ~/input/reroute | /rviz/routing/reroute | The goal pose of reroute. | | Subscription | ~/input/waypoint | /planning/mission_planning/checkpoint | The waypoint pose of route. | | Client | - | /api/routing/clear_route | The route clear API. | | Client | - | /api/routing/set_route_points | The route points set API. | +| Client | - | /api/routing/change_route_points | The route points change API. | diff --git a/system/duplicated_node_checker/CMakeLists.txt b/system/duplicated_node_checker/CMakeLists.txt new file mode 100644 index 0000000000000..6a8089fa85c96 --- /dev/null +++ b/system/duplicated_node_checker/CMakeLists.txt @@ -0,0 +1,20 @@ +cmake_minimum_required(VERSION 3.14) +project(duplicated_node_checker) + +find_package(autoware_cmake REQUIRED) +autoware_package() +find_package(rclcpp REQUIRED) + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/duplicated_node_checker_core.cpp +) + +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "duplicated_node_checker::DuplicatedNodeChecker" + EXECUTABLE duplicated_node_checker_node +) + +ament_auto_package(INSTALL_TO_SHARE + launch + config +) diff --git a/system/duplicated_node_checker/README.md b/system/duplicated_node_checker/README.md new file mode 100644 index 0000000000000..81b53e36c9f6d --- /dev/null +++ b/system/duplicated_node_checker/README.md @@ -0,0 +1,37 @@ +# Duplicated Node Checker + +## Purpose + +This node monitors the ROS 2 environments and detect duplication of node names in the environment. +The result is published as diagnostics. + +### Standalone Startup + +```bash +ros2 launch duplicated_node_checker duplicated_node_checker.launch.xml +``` + +## Inner-workings / Algorithms + +The types of topic status and corresponding diagnostic status are following. + +| Duplication status | Diagnostic status | Description | +| --------------------- | ----------------- | -------------------------- | +| `OK` | OK | No duplication is detected | +| `Duplicated Detected` | ERROR | Duplication is detected | + +## Inputs / Outputs + +### Output + +| Name | Type | Description | +| -------------- | --------------------------------- | ------------------- | +| `/diagnostics` | `diagnostic_msgs/DiagnosticArray` | Diagnostics outputs | + +## Parameters + +{{ json_to_markdown("system/duplicated_node_checker/schema/duplicated_node_checker.schema.json") }} + +## Assumptions / Known limits + +TBD. diff --git a/system/duplicated_node_checker/config/duplicated_node_checker.param.yaml b/system/duplicated_node_checker/config/duplicated_node_checker.param.yaml new file mode 100644 index 0000000000000..54b4f691b6eb1 --- /dev/null +++ b/system/duplicated_node_checker/config/duplicated_node_checker.param.yaml @@ -0,0 +1,4 @@ +/**: + ros__parameters: + update_rate: 10.0 + add_duplicated_node_names_to_msg: false # if true, duplicated node names are added to msg diff --git a/system/duplicated_node_checker/include/duplicated_node_checker/duplicated_node_checker_core.hpp b/system/duplicated_node_checker/include/duplicated_node_checker/duplicated_node_checker_core.hpp new file mode 100644 index 0000000000000..9d806754f3c20 --- /dev/null +++ b/system/duplicated_node_checker/include/duplicated_node_checker/duplicated_node_checker_core.hpp @@ -0,0 +1,55 @@ +// Copyright 2023 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 DUPLICATED_NODE_CHECKER__DUPLICATED_NODE_CHECKER_CORE_HPP_ +#define DUPLICATED_NODE_CHECKER__DUPLICATED_NODE_CHECKER_CORE_HPP_ + +#include +#include + +#include +#include +#include + +namespace duplicated_node_checker +{ +class DuplicatedNodeChecker : public rclcpp::Node +{ +public: + explicit DuplicatedNodeChecker(const rclcpp::NodeOptions & node_options); + std::vector findIdenticalNames(const std::vector input_name_lists) + { + std::unordered_set unique_names; + std::vector identical_names; + for (auto name : input_name_lists) { + if (unique_names.find(name) != unique_names.end()) { + identical_names.push_back(name); + } else { + unique_names.insert(name); + } + } + return identical_names; + } + +private: + void onTimer(); + void produceDiagnostics(diagnostic_updater::DiagnosticStatusWrapper & stat); + + diagnostic_updater::Updater updater_{this}; + rclcpp::TimerBase::SharedPtr timer_; + bool add_duplicated_node_names_to_msg_; +}; +} // namespace duplicated_node_checker + +#endif // DUPLICATED_NODE_CHECKER__DUPLICATED_NODE_CHECKER_CORE_HPP_ diff --git a/system/duplicated_node_checker/launch/duplicated_node_checker.launch.xml b/system/duplicated_node_checker/launch/duplicated_node_checker.launch.xml new file mode 100644 index 0000000000000..87f41f045545d --- /dev/null +++ b/system/duplicated_node_checker/launch/duplicated_node_checker.launch.xml @@ -0,0 +1,7 @@ + + + + + + + diff --git a/system/duplicated_node_checker/package.xml b/system/duplicated_node_checker/package.xml new file mode 100644 index 0000000000000..e2ba225b16330 --- /dev/null +++ b/system/duplicated_node_checker/package.xml @@ -0,0 +1,24 @@ + + + + duplicated_node_checker + 1.0.0 + A package to find out nodes with common names + Shumpei Wakabayashi + yliuhb + Apache 2.0 + + ament_cmake + autoware_cmake + + diagnostic_updater + rclcpp + rclcpp_components + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/system/duplicated_node_checker/schema/duplicated_node_checker.schema.json b/system/duplicated_node_checker/schema/duplicated_node_checker.schema.json new file mode 100644 index 0000000000000..6ee933ecf1a77 --- /dev/null +++ b/system/duplicated_node_checker/schema/duplicated_node_checker.schema.json @@ -0,0 +1,31 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Duplicated Node Checker", + "type": "object", + "definitions": { + "duplicated_node_checker": { + "type": "object", + "properties": { + "update_rate": { + "type": "number", + "default": 10, + "exclusiveMinimum": 2, + "description": "The scanning and update frequency of the checker." + } + }, + "required": ["update_rate"] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/duplicated_node_checker" + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] +} diff --git a/system/duplicated_node_checker/src/duplicated_node_checker_core.cpp b/system/duplicated_node_checker/src/duplicated_node_checker_core.cpp new file mode 100644 index 0000000000000..46c02d9d6e133 --- /dev/null +++ b/system/duplicated_node_checker/src/duplicated_node_checker_core.cpp @@ -0,0 +1,88 @@ +// Copyright 2023 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 "duplicated_node_checker/duplicated_node_checker_core.hpp" + +#include +#include + +#include +#include +#include + +namespace duplicated_node_checker +{ + +DuplicatedNodeChecker::DuplicatedNodeChecker(const rclcpp::NodeOptions & node_options) +: Node("duplicated_node_checker", node_options) +{ + double update_rate = declare_parameter("update_rate"); + add_duplicated_node_names_to_msg_ = declare_parameter("add_duplicated_node_names_to_msg"); + updater_.setHardwareID("duplicated_node_checker"); + updater_.add("duplicated_node_checker", this, &DuplicatedNodeChecker::produceDiagnostics); + + const auto period_ns = rclcpp::Rate(update_rate).period(); + timer_ = rclcpp::create_timer( + this, get_clock(), period_ns, std::bind(&DuplicatedNodeChecker::onTimer, this)); +} + +std::string get_fullname_from_name_ns_pair(std::pair name_and_ns_pair) +{ + std::string full_name; + const std::string & name = name_and_ns_pair.first; + const std::string & ns = name_and_ns_pair.second; + if (ns.back() == '/') { + full_name = ns + name; + } else { + full_name = ns + "/" + name; + } + return full_name; +} + +void DuplicatedNodeChecker::onTimer() +{ + updater_.force_update(); +} + +void DuplicatedNodeChecker::produceDiagnostics(diagnostic_updater::DiagnosticStatusWrapper & stat) +{ + using diagnostic_msgs::msg::DiagnosticStatus; + + std::vector node_names = this->get_node_names(); + std::vector identical_names = findIdenticalNames(node_names); + std::string msg; + int level; + if (identical_names.size() > 0) { + level = DiagnosticStatus::ERROR; + msg = "Error: Duplicated nodes detected"; + if (add_duplicated_node_names_to_msg_) { + std::set unique_identical_names(identical_names.begin(), identical_names.end()); + for (const auto & name : unique_identical_names) { + msg += " " + name; + } + } + for (auto name : identical_names) { + stat.add("Duplicated Node Name", name); + } + } else { + msg = "OK"; + level = DiagnosticStatus::OK; + } + stat.summary(level, msg); +} + +} // namespace duplicated_node_checker + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(duplicated_node_checker::DuplicatedNodeChecker) diff --git a/system/system_diagnostic_graph/CMakeLists.txt b/system/system_diagnostic_graph/CMakeLists.txt new file mode 100644 index 0000000000000..81d8ba39b3d1b --- /dev/null +++ b/system/system_diagnostic_graph/CMakeLists.txt @@ -0,0 +1,26 @@ +cmake_minimum_required(VERSION 3.14) +project(system_diagnostic_graph) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +ament_auto_add_executable(aggregator + src/core/config.cpp + src/core/debug.cpp + src/core/graph.cpp + src/core/nodes.cpp + src/core/exprs.cpp + src/core/modes.cpp + src/main.cpp +) + +ament_auto_add_executable(converter + src/tool.cpp +) + +install( + PROGRAMS util/debug.py + DESTINATION lib/${PROJECT_NAME} +) + +ament_auto_package(INSTALL_TO_SHARE config example launch) diff --git a/system/system_diagnostic_graph/README.md b/system/system_diagnostic_graph/README.md new file mode 100644 index 0000000000000..df22121a778fb --- /dev/null +++ b/system/system_diagnostic_graph/README.md @@ -0,0 +1,59 @@ +# System diagnostic graph + +## Overview + +The system diagnostic graph node subscribes to diagnostic array and publishes aggregated diagnostic graph. +As shown in the diagram below, this node introduces extra diagnostic status for intermediate functional unit. +Diagnostic status dependencies will be directed acyclic graph (DAG). + +![overview](./doc/overview.drawio.svg) + +## Operation mode availability + +For MRM, this node publishes the status of the top-level functional units in the dedicated message. +Therefore, the diagnostic graph must contain functional units with the following names. +This feature breaks the generality of the graph and may be changed to a plugin or another node in the future. + +- /autoware/operation/stop +- /autoware/operation/autonomous +- /autoware/operation/local +- /autoware/operation/remote +- /autoware/operation/emergency-stop +- /autoware/operation/comfortable-stop +- /autoware/operation/pull-over + +## Interface + +| Interface Type | Interface Name | Data Type | Description | +| -------------- | ------------------------------------- | ------------------------------------------------- | ------------------ | +| subscription | `/diagnostics` | `diagnostic_msgs/msg/DiagnosticArray` | Diagnostics input. | +| publisher | `/diagnostics_graph` | `tier4_system_msgs/msg/DiagnosticGraph` | Diagnostics graph. | +| publisher | `/system/operation_mode/availability` | `tier4_system_msgs/msg/OperationModeAvailability` | mode availability. | + +## Parameters + +| Parameter Name | Data Type | Description | +| ----------------- | --------- | ------------------------------------------ | +| `graph_file` | `string` | Path of the config file. | +| `rate` | `double` | Rate of aggregation and topic publication. | +| `input_qos_depth` | `uint` | QoS depth of input array topic. | +| `graph_qos_depth` | `uint` | QoS depth of output graph topic. | + +## Graph file format + +- [GraphFile](./doc/format/graph-file.md) +- [Path](./doc/format/path.md) +- [Node](./doc/format/node.md) + - [Diag](./doc/format/diag.md) + - [Unit](./doc/format/unit.md) + - [And](./doc/format/and.md) + - [Or](./doc/format/or.md) + +## Example + +- [example1.yaml](./example/example1.yaml) +- [example2.yaml](./example/example2.yaml) + +```bash +ros2 launch system_diagnostic_graph example.launch.xml +``` diff --git a/system/system_diagnostic_graph/config/default.param.yaml b/system/system_diagnostic_graph/config/default.param.yaml new file mode 100644 index 0000000000000..9fd572c7926fa --- /dev/null +++ b/system/system_diagnostic_graph/config/default.param.yaml @@ -0,0 +1,7 @@ +/**: + ros__parameters: + mode_availability: true + mode: psim + rate: 1.0 + input_qos_depth: 1000 + graph_qos_depth: 1 diff --git a/system/system_diagnostic_graph/doc/format/and.md b/system/system_diagnostic_graph/doc/format/and.md new file mode 100644 index 0000000000000..a92aa2817832e --- /dev/null +++ b/system/system_diagnostic_graph/doc/format/and.md @@ -0,0 +1,11 @@ +# Unit + +And is a node that is evaluated as the AND of the input nodes. + +## Format + +| Name | Type | Required | Description | +| ---- | ------------------------------------------ | -------- | ------------------------------------- | +| type | string | yes | Specify `and` when using this object. | +| name | string | yes | Name of diagnostic status. | +| list | List<[Diag](./diag.md)\|[Unit](./unit.md)> | yes | List of input node references. | diff --git a/system/system_diagnostic_graph/doc/format/diag.md b/system/system_diagnostic_graph/doc/format/diag.md new file mode 100644 index 0000000000000..9835924f08996 --- /dev/null +++ b/system/system_diagnostic_graph/doc/format/diag.md @@ -0,0 +1,11 @@ +# Diag + +Diag is a node that refers to a source diagnostics. + +## Format + +| Name | Type | Required | Description | +| -------- | ------ | -------- | -------------------------------------- | +| type | string | yes | Specify `diag` when using this object. | +| name | string | yes | Name of diagnostic status. | +| hardware | string | yes | Hardware ID of diagnostic status. | diff --git a/system/system_diagnostic_graph/doc/format/graph-file.md b/system/system_diagnostic_graph/doc/format/graph-file.md new file mode 100644 index 0000000000000..3c4cfec996a4a --- /dev/null +++ b/system/system_diagnostic_graph/doc/format/graph-file.md @@ -0,0 +1,10 @@ +# GraphFile + +GraphFile is the top level object that makes up the configuration file. + +## Format + +| Name | Type | Required | Description | +| ----- | ----------------------- | -------- | ------------------------------ | +| files | List<[Path](./path.md)> | no | Paths of the files to include. | +| nodes | List<[Node](./node.md)> | no | Nodes of the diagnostic graph. | diff --git a/system/system_diagnostic_graph/doc/format/node.md b/system/system_diagnostic_graph/doc/format/node.md new file mode 100644 index 0000000000000..da8e8e57b111f --- /dev/null +++ b/system/system_diagnostic_graph/doc/format/node.md @@ -0,0 +1,9 @@ +# Node + +Node is a base object that makes up the diagnostic graph. + +## Format + +| Name | Type | Required | Description | +| ---- | ------ | -------- | ------------------------------------------- | +| type | string | yes | Node type. See derived objects for details. | diff --git a/system/system_diagnostic_graph/doc/format/or.md b/system/system_diagnostic_graph/doc/format/or.md new file mode 100644 index 0000000000000..3e668b686c9e8 --- /dev/null +++ b/system/system_diagnostic_graph/doc/format/or.md @@ -0,0 +1,11 @@ +# Unit + +Or is a node that is evaluated as the OR of the input nodes. + +## Format + +| Name | Type | Required | Description | +| ---- | ------------------------------------------ | -------- | ------------------------------------ | +| type | string | yes | Specify `or` when using this object. | +| name | string | yes | Name of diagnostic status. | +| list | List<[Diag](./diag.md)\|[Unit](./unit.md)> | yes | List of input node references. | diff --git a/system/system_diagnostic_graph/doc/format/path.md b/system/system_diagnostic_graph/doc/format/path.md new file mode 100644 index 0000000000000..1f27accefa35a --- /dev/null +++ b/system/system_diagnostic_graph/doc/format/path.md @@ -0,0 +1,10 @@ +# Path + +Path is an object that indicates the path of the file to include. + +## Format + +| Name | Type | Required | Description | +| ------- | ------ | -------- | ----------------------------- | +| package | string | yes | Package name. | +| path | string | yes | Relative path in the package. | diff --git a/system/system_diagnostic_graph/doc/format/unit.md b/system/system_diagnostic_graph/doc/format/unit.md new file mode 100644 index 0000000000000..791689aa2d98a --- /dev/null +++ b/system/system_diagnostic_graph/doc/format/unit.md @@ -0,0 +1,10 @@ +# Unit + +Diag is a node that refers to a functional unit. + +## Format + +| Name | Type | Required | Description | +| ---- | ------ | -------- | -------------------------------------- | +| type | string | yes | Specify `unit` when using this object. | +| name | string | yes | Name of diagnostic status. | diff --git a/system/system_diagnostic_graph/doc/overview.drawio.svg b/system/system_diagnostic_graph/doc/overview.drawio.svg new file mode 100644 index 0000000000000..e971c052dedc8 --- /dev/null +++ b/system/system_diagnostic_graph/doc/overview.drawio.svg @@ -0,0 +1,342 @@ + + + + + + + +
+
+
+ /diagnostics_graph +
+
+
+ + /diagnostics_graph + + + + + + +
+
+
+ /system/operation_mode/availability +
+
+
+ + /system/operation_mode/availability + + + + + + +
+
+
+ /diagnostics +
+
+
+ + /diagnostics + + + + + + + + +
+
+
+ Top LiDAR +
+
+
+ + Top LiDAR + + + + + + + + +
+
+
+ Front LiDAR +
+
+
+ + Front LiDAR + + + + + + + + +
+
+
+ obstacle detection +
+
+
+ + obstacle detection + + + + + + + + +
+
+
+ pose estimation +
+
+
+ + pose estimation + + + + + + +
+
+
+ autonomous +
+
+
+ + autonomous + + + + + + +
+
+
+ remote +
+
+
+ + remote + + + + + + + + +
+
+
+ remote command +
+
+
+ + remote command + + + + + + +
+
+
+ local +
+
+
+ + local + + + + + + + + +
+
+
+ joystick command +
+
+
+ + joystick command + + + + + + +
+
+
+ AND +
+
+
+ + AND + + + + + + + + +
+
+
+ stop +
+
+
+ + stop + + + + + + + + +
+
+
+ Front radar +
+
+
+ + Front radar + + + + + + +
+
+
+ OR +
+
+
+ + OR + + + + + + +
+
+
+ MRM +
+
+
+ + MRM + + + + + + + Text is not SVG - cannot display + + + diff --git a/system/system_diagnostic_graph/example/example.launch.xml b/system/system_diagnostic_graph/example/example.launch.xml new file mode 100644 index 0000000000000..1456bfd5c6593 --- /dev/null +++ b/system/system_diagnostic_graph/example/example.launch.xml @@ -0,0 +1,6 @@ + + + + + + diff --git a/system/system_diagnostic_graph/example/example_0.yaml b/system/system_diagnostic_graph/example/example_0.yaml new file mode 100644 index 0000000000000..0ee6e59c8e121 --- /dev/null +++ b/system/system_diagnostic_graph/example/example_0.yaml @@ -0,0 +1,32 @@ +files: + - { package: system_diagnostic_graph, path: example/example_1.yaml } + - { package: system_diagnostic_graph, path: example/example_2.yaml } + +nodes: + - path: /autoware/modes/stop + type: debug-ok + + - path: /autoware/modes/autonomous + type: and + list: + - { type: link, path: /functions/pose_estimation } + - { type: link, path: /functions/obstacle_detection } + + - path: /autoware/modes/local + type: and + list: + - { type: link, path: /external/joystick_command } + + - path: /autoware/modes/remote + type: and + list: + - { type: link, path: /external/remote_command } + + - path: /autoware/modes/emergency-stop + type: debug-ok + + - path: /autoware/modes/comfortable-stop + type: debug-ok + + - path: /autoware/modes/pull-over + type: debug-ok diff --git a/system/system_diagnostic_graph/example/example_1.yaml b/system/system_diagnostic_graph/example/example_1.yaml new file mode 100644 index 0000000000000..5ba93ec3059e4 --- /dev/null +++ b/system/system_diagnostic_graph/example/example_1.yaml @@ -0,0 +1,23 @@ +nodes: + - path: /functions/pose_estimation + type: and + list: + - { type: link, path: /sensing/lidars/top } + + - path: /functions/obstacle_detection + type: or + list: + - { type: link, path: /sensing/lidars/front } + - { type: link, path: /sensing/radars/front } + + - path: /sensing/lidars/top + type: diag + name: "lidar_driver/top: status" + + - path: /sensing/lidars/front + type: diag + name: "lidar_driver/front: status" + + - path: /sensing/radars/front + type: diag + name: "radar_driver/front: status" diff --git a/system/system_diagnostic_graph/example/example_2.yaml b/system/system_diagnostic_graph/example/example_2.yaml new file mode 100644 index 0000000000000..f61f4accbfec8 --- /dev/null +++ b/system/system_diagnostic_graph/example/example_2.yaml @@ -0,0 +1,8 @@ +nodes: + - path: /external/joystick_command + type: diag + name: "external_command_checker: joystick_command" + + - path: /external/remote_command + type: diag + name: "external_command_checker: remote_command" diff --git a/system/system_diagnostic_graph/example/example_diags.py b/system/system_diagnostic_graph/example/example_diags.py new file mode 100755 index 0000000000000..52d6189a63f30 --- /dev/null +++ b/system/system_diagnostic_graph/example/example_diags.py @@ -0,0 +1,64 @@ +#!/usr/bin/env python3 + +# Copyright 2023 The Autoware Contributors +# +# 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. + +from diagnostic_msgs.msg import DiagnosticArray +from diagnostic_msgs.msg import DiagnosticStatus +import rclpy +import rclpy.node +import rclpy.qos + + +class DummyDiagnostics(rclpy.node.Node): + def __init__(self, names): + super().__init__("dummy_diagnostics") + qos = rclpy.qos.qos_profile_system_default + self.diags = self.create_publisher(DiagnosticArray, "/diagnostics", qos) + self.timer = self.create_timer(0.5, self.on_timer) + self.count = 0 + self.array = [self.create_status(name) for name in names] + + def on_timer(self): + error_index = int(self.count / 10) + for index, status in enumerate(self.array, 1): + if index == error_index: + status.level = DiagnosticStatus.ERROR + status.message = "ERROR" + else: + status.level = DiagnosticStatus.OK + status.message = "OK" + + diagnostics = DiagnosticArray() + diagnostics.header.stamp = self.get_clock().now().to_msg() + diagnostics.status = self.array + self.count = (self.count + 1) % 60 + self.diags.publish(diagnostics) + + @staticmethod + def create_status(name: str): + return DiagnosticStatus(name=name, hardware_id="example") + + +if __name__ == "__main__": + diags = [ + "lidar_driver/top: status", + "lidar_driver/front: status", + "radar_driver/front: status", + "external_command_checker: joystick_command", + "external_command_checker: remote_command", + ] + rclpy.init() + rclpy.spin(DummyDiagnostics(diags)) + rclpy.shutdown() diff --git a/system/system_diagnostic_graph/launch/system_diagnostic_graph.launch.xml b/system/system_diagnostic_graph/launch/system_diagnostic_graph.launch.xml new file mode 100644 index 0000000000000..5b9a84947d179 --- /dev/null +++ b/system/system_diagnostic_graph/launch/system_diagnostic_graph.launch.xml @@ -0,0 +1,9 @@ + + + + + + + + + diff --git a/planning/rtc_auto_mode_manager/package.xml b/system/system_diagnostic_graph/package.xml similarity index 58% rename from planning/rtc_auto_mode_manager/package.xml rename to system/system_diagnostic_graph/package.xml index e9c16aac1bb91..ffc0c6c7d5385 100644 --- a/planning/rtc_auto_mode_manager/package.xml +++ b/system/system_diagnostic_graph/package.xml @@ -1,23 +1,19 @@ - rtc_auto_mode_manager + system_diagnostic_graph 0.1.0 - The rtc_auto_mode_manager package - - Fumiya Watanabe - Taiki Tanaka - + The system_diagnostic_graph package + Takagi, Isamu Apache License 2.0 - Fumiya Watanabe - ament_cmake_auto autoware_cmake + diagnostic_msgs rclcpp - rclcpp_components - tier4_rtc_msgs + tier4_system_msgs + yaml_cpp_vendor ament_lint_auto autoware_lint_common diff --git a/system/system_diagnostic_graph/src/core/config.cpp b/system/system_diagnostic_graph/src/core/config.cpp new file mode 100644 index 0000000000000..305860af32840 --- /dev/null +++ b/system/system_diagnostic_graph/src/core/config.cpp @@ -0,0 +1,261 @@ +// Copyright 2023 The Autoware Contributors +// +// 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 "config.hpp" + +#include + +#include +#include +#include +#include + +// DEBUG +#include + +namespace system_diagnostic_graph +{ + +ErrorMarker::ErrorMarker(const std::string & file) +{ + file_ = file; +} + +std::string ErrorMarker::str() const +{ + if (type_.empty()) { + return file_; + } + + std::string result = type_; + for (const auto & index : indices_) { + result += "-" + std::to_string(index); + } + return result + " in " + file_; +} + +ErrorMarker ErrorMarker::type(const std::string & type) const +{ + ErrorMarker mark = *this; + mark.type_ = type; + return mark; +} + +ErrorMarker ErrorMarker::index(size_t index) const +{ + ErrorMarker mark = *this; + mark.indices_.push_back(index); + return mark; +} + +ConfigObject::ConfigObject(const ErrorMarker & mark, YAML::Node yaml, const std::string & type) +{ + if (!yaml.IsMap()) { + throw create_error(mark, type + " is not a dict type"); + } + for (const auto & kv : yaml) { + dict_[kv.first.as()] = kv.second; + } + mark_ = mark; + type_ = type; +} + +ErrorMarker ConfigObject::mark() const +{ + return mark_; +} + +std::optional ConfigObject::take_yaml(const std::string & name) +{ + if (!dict_.count(name)) { + return std::nullopt; + } + const auto yaml = dict_.at(name); + dict_.erase(name); + return yaml; +} + +std::string ConfigObject::take_text(const std::string & name) +{ + if (!dict_.count(name)) { + throw create_error(mark_, "object has no '" + name + "' field"); + } + + const auto yaml = dict_.at(name); + dict_.erase(name); + return yaml.as(); +} + +std::string ConfigObject::take_text(const std::string & name, const std::string & fail) +{ + if (!dict_.count(name)) { + return fail; + } + + const auto yaml = dict_.at(name); + dict_.erase(name); + return yaml.as(); +} + +std::vector ConfigObject::take_list(const std::string & name) +{ + if (!dict_.count(name)) { + return std::vector(); + } + + const auto yaml = dict_.at(name); + dict_.erase(name); + + if (!yaml.IsSequence()) { + throw ConfigError("the '" + name + "' field is not a list type"); + } + return std::vector(yaml.begin(), yaml.end()); +} + +bool ConfigFilter::check(const std::string & mode) const +{ + if (!excludes.empty() && excludes.count(mode) != 0) return false; + if (!includes.empty() && includes.count(mode) == 0) return false; + return true; +} + +ConfigError create_error(const ErrorMarker & mark, const std::string & message) +{ + (void)mark; + return ConfigError(message); +} + +ConfigFilter parse_mode_filter(const ErrorMarker & mark, std::optional yaml) +{ + std::unordered_set excludes; + std::unordered_set includes; + if (yaml) { + ConfigObject dict(mark, yaml.value(), "mode filter"); + + for (const auto & mode : dict.take_list("except")) { + excludes.emplace(mode.as()); + } + for (const auto & mode : dict.take_list("only")) { + includes.emplace(mode.as()); + } + } + return ConfigFilter{excludes, includes}; +} + +FileConfig parse_file_config(const ErrorMarker & mark, YAML::Node yaml) +{ + ConfigObject dict(mark, yaml, "file object"); + const auto relative_path = dict.take_text("path"); + const auto package_name = dict.take_text("package"); + const auto package_path = ament_index_cpp::get_package_share_directory(package_name); + return FileConfig{mark, package_path + "/" + relative_path}; +} + +NodeConfig parse_node_config(const ErrorMarker & mark, YAML::Node yaml) +{ + ConfigObject dict(mark, yaml, "node object"); + const auto path = dict.take_text("path"); + const auto mode = parse_mode_filter(dict.mark(), dict.take_yaml("mode")); + return NodeConfig{path, mode, dict}; +} + +ExprConfig parse_expr_config(const ErrorMarker & mark, YAML::Node yaml) +{ + ConfigObject dict(mark, yaml, "expr object"); + return parse_expr_config(dict); +} + +ExprConfig parse_expr_config(ConfigObject & dict) +{ + const auto type = dict.take_text("type"); + const auto mode = parse_mode_filter(dict.mark(), dict.take_yaml("mode")); + return ExprConfig{type, mode, dict}; +} + +void dump(const ConfigFile & config) +{ + std::cout << "=================================================================" << std::endl; + std::cout << config.mark.str() << std::endl; + for (const auto & file : config.files) { + std::cout << " - f: " << file.path << " (" << file.mark.str() << ")" << std::endl; + } + for (const auto & unit : config.units) { + std::cout << " - u: " << unit.path << " (" << unit.dict.mark().str() << ")" << std::endl; + } + for (const auto & diag : config.diags) { + std::cout << " - d: " << diag.path << " (" << diag.dict.mark().str() << ")" << std::endl; + } +} + +template +auto apply(const ErrorMarker & mark, F & func, const std::vector & list) +{ + std::vector result; + for (size_t i = 0; i < list.size(); ++i) { + result.push_back(func(mark.index(i), list[i])); + } + return result; +} + +ConfigFile load_config_file(const FileConfig & file) +{ + if (!std::filesystem::exists(file.path)) { + throw create_error(file.mark, "config file '" + file.path + "' does not exist"); + } + + const auto yaml = YAML::LoadFile(file.path); + const auto mark = ErrorMarker(file.path); + auto dict = ConfigObject(mark, yaml, "config file"); + + std::vector units; + std::vector diags; + for (const auto & node : dict.take_list("nodes")) { + const auto type = node["type"].as(); + if (type == "diag") { + diags.push_back(node); + } else { + units.push_back(node); + } + } + + ConfigFile config(mark); + config.files = apply(mark.type("file"), parse_file_config, dict.take_list("files")); + config.units = apply(mark.type("unit"), parse_node_config, units); + config.diags = apply(mark.type("diag"), parse_node_config, diags); + return config; +} + +ConfigFile load_config_root(const std::string & path) +{ + const auto mark = ErrorMarker("root file"); + std::vector configs; + configs.push_back(load_config_file(FileConfig{mark, path})); + + // Use an index because updating the vector invalidates the iterator. + for (size_t i = 0; i < configs.size(); ++i) { + for (const auto & file : configs[i].files) { + configs.push_back(load_config_file(file)); + } + } + + ConfigFile result(mark); + for (const auto & config : configs) { + result.files.insert(result.files.end(), config.files.begin(), config.files.end()); + result.units.insert(result.units.end(), config.units.begin(), config.units.end()); + result.diags.insert(result.diags.end(), config.diags.begin(), config.diags.end()); + } + return result; +} + +} // namespace system_diagnostic_graph diff --git a/system/system_diagnostic_graph/src/core/config.hpp b/system/system_diagnostic_graph/src/core/config.hpp new file mode 100644 index 0000000000000..4d679ef575944 --- /dev/null +++ b/system/system_diagnostic_graph/src/core/config.hpp @@ -0,0 +1,112 @@ +// Copyright 2023 The Autoware Contributors +// +// 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 CORE__CONFIG_HPP_ +#define CORE__CONFIG_HPP_ + +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace system_diagnostic_graph +{ + +struct ConfigError : public std::runtime_error +{ + using runtime_error::runtime_error; +}; + +class ErrorMarker +{ +public: + explicit ErrorMarker(const std::string & file = ""); + std::string str() const; + ErrorMarker type(const std::string & type) const; + ErrorMarker index(size_t index) const; + +private: + std::string file_; + std::string type_; + std::vector indices_; +}; + +class ConfigObject +{ +public: + ConfigObject(const ErrorMarker & mark, YAML::Node yaml, const std::string & type); + ErrorMarker mark() const; + std::optional take_yaml(const std::string & name); + std::string take_text(const std::string & name); + std::string take_text(const std::string & name, const std::string & fail); + std::vector take_list(const std::string & name); + +private: + ErrorMarker mark_; + std::string type_; + std::unordered_map dict_; +}; + +struct ConfigFilter +{ + bool check(const std::string & mode) const; + std::unordered_set excludes; + std::unordered_set includes; +}; + +struct ExprConfig +{ + std::string type; + ConfigFilter mode; + ConfigObject dict; +}; + +struct NodeConfig +{ + std::string path; + ConfigFilter mode; + ConfigObject dict; +}; + +struct FileConfig +{ + ErrorMarker mark; + std::string path; +}; + +struct ConfigFile +{ + explicit ConfigFile(const ErrorMarker & mark) : mark(mark) {} + ErrorMarker mark; + std::vector files; + std::vector units; + std::vector diags; +}; + +using ConfigDict = std::unordered_map; + +ConfigError create_error(const ErrorMarker & mark, const std::string & message); +ConfigFile load_config_root(const std::string & path); + +ExprConfig parse_expr_config(const ErrorMarker & mark, YAML::Node yaml); +ExprConfig parse_expr_config(ConfigObject & dict); + +} // namespace system_diagnostic_graph + +#endif // CORE__CONFIG_HPP_ diff --git a/system/system_diagnostic_graph/src/core/debug.cpp b/system/system_diagnostic_graph/src/core/debug.cpp new file mode 100644 index 0000000000000..ed696573521a7 --- /dev/null +++ b/system/system_diagnostic_graph/src/core/debug.cpp @@ -0,0 +1,81 @@ +// Copyright 2023 The Autoware Contributors +// +// 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 "debug.hpp" + +#include "graph.hpp" +#include "nodes.hpp" +#include "types.hpp" + +#include +#include +#include +#include + +namespace system_diagnostic_graph +{ + +const std::unordered_map level_names = { + {DiagnosticStatus::OK, "OK"}, + {DiagnosticStatus::WARN, "WARN"}, + {DiagnosticStatus::ERROR, "ERROR"}, + {DiagnosticStatus::STALE, "STALE"}}; + +void Graph::debug() +{ + std::vector lines; + for (const auto & node : nodes_) { + lines.push_back(node->debug()); + } + + std::array widths = {}; + for (const auto & line : lines) { + for (size_t i = 0; i < diag_debug_size; ++i) { + widths[i] = std::max(widths[i], line[i].length()); + } + } + + const size_t total_width = std::accumulate(widths.begin(), widths.end(), 0); + std::cout << std::string(total_width + 3 * diag_debug_size + 1, '=') << std::endl; + + for (const auto & line : lines) { + for (size_t i = 0; i < diag_debug_size; ++i) { + std::cout << "| " << std::left << std::setw(widths[i]) << line[i] << " "; + } + std::cout << "|" << std::endl; + } +} + +DiagDebugData UnitNode::debug() const +{ + const auto level_name = level_names.at(level()); + const auto index_name = std::to_string(index()); + return {"unit", index_name, level_name, path_, "-----"}; +} + +DiagDebugData DiagNode::debug() const +{ + const auto level_name = level_names.at(level()); + const auto index_name = std::to_string(index()); + return {"diag", index_name, level_name, path_, name_}; +} + +DiagDebugData UnknownNode::debug() const +{ + const auto level_name = level_names.at(level()); + const auto index_name = std::to_string(index()); + return {"test", index_name, level_name, path_, "-----"}; +} + +} // namespace system_diagnostic_graph diff --git a/system/system_diagnostic_graph/src/core/debug.hpp b/system/system_diagnostic_graph/src/core/debug.hpp new file mode 100644 index 0000000000000..bb1bbc6f14230 --- /dev/null +++ b/system/system_diagnostic_graph/src/core/debug.hpp @@ -0,0 +1,29 @@ +// Copyright 2023 The Autoware Contributors +// +// 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 CORE__DEBUG_HPP_ +#define CORE__DEBUG_HPP_ + +#include +#include + +namespace system_diagnostic_graph +{ + +constexpr size_t diag_debug_size = 5; +using DiagDebugData = std::array; + +} // namespace system_diagnostic_graph + +#endif // CORE__DEBUG_HPP_ diff --git a/system/system_diagnostic_graph/src/core/exprs.cpp b/system/system_diagnostic_graph/src/core/exprs.cpp new file mode 100644 index 0000000000000..22281f939cad2 --- /dev/null +++ b/system/system_diagnostic_graph/src/core/exprs.cpp @@ -0,0 +1,216 @@ +// Copyright 2023 The Autoware Contributors +// +// 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 "exprs.hpp" + +#include "nodes.hpp" + +#include +#include +#include +#include +#include + +// DEBUG +#include + +namespace system_diagnostic_graph +{ + +using LinkStatus = std::vector>; + +void extend(LinkStatus & a, const LinkStatus & b) +{ + a.insert(a.end(), b.begin(), b.end()); +} + +void extend_false(LinkStatus & a, const LinkStatus & b) +{ + for (const auto & p : b) { + a.push_back(std::make_pair(p.first, false)); + } +} + +auto create_expr_list(ExprInit & exprs, ConfigObject & config) +{ + std::vector> result; + const auto list = config.take_list("list"); + for (size_t i = 0; i < list.size(); ++i) { + auto dict = parse_expr_config(config.mark().index(i), list[i]); + auto expr = exprs.create(dict); + if (expr) { + result.push_back(std::move(expr)); + } + } + return result; +} + +ConstExpr::ConstExpr(const DiagnosticLevel level) +{ + level_ = level; +} + +ExprStatus ConstExpr::eval() const +{ + ExprStatus status; + status.level = level_; + return status; +} + +std::vector ConstExpr::get_dependency() const +{ + return {}; +} + +LinkExpr::LinkExpr(ExprInit & exprs, ConfigObject & config) +{ + // TODO(Takagi, Isamu): remove + (void)config; + (void)exprs; +} + +void LinkExpr::init(ConfigObject & config, std::unordered_map nodes) +{ + const auto path = config.take_text("path"); + if (!nodes.count(path)) { + throw ConfigError("node path '" + path + "' does not exist"); + } + node_ = nodes.at(path); +} + +ExprStatus LinkExpr::eval() const +{ + ExprStatus status; + status.level = node_->level(); + status.links.push_back(std::make_pair(node_, true)); + return status; +} + +std::vector LinkExpr::get_dependency() const +{ + return {node_}; +} + +AndExpr::AndExpr(ExprInit & exprs, ConfigObject & config, bool short_circuit) +{ + list_ = create_expr_list(exprs, config); + short_circuit_ = short_circuit; +} + +ExprStatus AndExpr::eval() const +{ + if (list_.empty()) { + ExprStatus status; + status.level = DiagnosticStatus::OK; + return status; + } + + ExprStatus status; + status.level = DiagnosticStatus::OK; + for (const auto & expr : list_) { + const auto result = expr->eval(); + status.level = std::max(status.level, result.level); + extend(status.links, result.links); + if (short_circuit_ && status.level != DiagnosticStatus::OK) { + break; + } + } + status.level = std::min(status.level, DiagnosticStatus::ERROR); + return status; +} + +std::vector AndExpr::get_dependency() const +{ + std::vector depends; + for (const auto & expr : list_) { + const auto nodes = expr->get_dependency(); + depends.insert(depends.end(), nodes.begin(), nodes.end()); + } + return depends; +} + +OrExpr::OrExpr(ExprInit & exprs, ConfigObject & config) +{ + list_ = create_expr_list(exprs, config); +} + +ExprStatus OrExpr::eval() const +{ + if (list_.empty()) { + ExprStatus status; + status.level = DiagnosticStatus::OK; + return status; + } + + ExprStatus status; + status.level = DiagnosticStatus::ERROR; + for (const auto & expr : list_) { + const auto result = expr->eval(); + status.level = std::min(status.level, result.level); + extend(status.links, result.links); + } + status.level = std::min(status.level, DiagnosticStatus::ERROR); + return status; +} + +std::vector OrExpr::get_dependency() const +{ + std::vector depends; + for (const auto & expr : list_) { + const auto nodes = expr->get_dependency(); + depends.insert(depends.end(), nodes.begin(), nodes.end()); + } + return depends; +} + +ExprInit::ExprInit(const std::string & mode) +{ + mode_ = mode; +} + +std::unique_ptr ExprInit::create(ExprConfig config) +{ + if (!config.mode.check(mode_)) { + return nullptr; + } + if (config.type == "link") { + auto expr = std::make_unique(*this, config.dict); + links_.push_back(std::make_pair(expr.get(), config.dict)); + return expr; + } + if (config.type == "and") { + return std::make_unique(*this, config.dict, false); + } + if (config.type == "short-circuit-and") { + return std::make_unique(*this, config.dict, true); + } + if (config.type == "or") { + return std::make_unique(*this, config.dict); + } + if (config.type == "debug-ok") { + return std::make_unique(DiagnosticStatus::OK); + } + if (config.type == "debug-warn") { + return std::make_unique(DiagnosticStatus::WARN); + } + if (config.type == "debug-error") { + return std::make_unique(DiagnosticStatus::ERROR); + } + if (config.type == "debug-stale") { + return std::make_unique(DiagnosticStatus::STALE); + } + throw ConfigError("unknown expr type: " + config.type + " " + config.dict.mark().str()); +} + +} // namespace system_diagnostic_graph diff --git a/system/system_diagnostic_graph/src/core/exprs.hpp b/system/system_diagnostic_graph/src/core/exprs.hpp new file mode 100644 index 0000000000000..f582e019d5691 --- /dev/null +++ b/system/system_diagnostic_graph/src/core/exprs.hpp @@ -0,0 +1,104 @@ +// Copyright 2023 The Autoware Contributors +// +// 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 CORE__EXPRS_HPP_ +#define CORE__EXPRS_HPP_ + +#include "config.hpp" +#include "types.hpp" + +#include +#include +#include +#include +#include + +namespace system_diagnostic_graph +{ + +struct ExprStatus +{ + DiagnosticLevel level; + std::vector> links; +}; + +class BaseExpr +{ +public: + virtual ~BaseExpr() = default; + virtual ExprStatus eval() const = 0; + virtual std::vector get_dependency() const = 0; +}; + +class ConstExpr : public BaseExpr +{ +public: + explicit ConstExpr(const DiagnosticLevel level); + ExprStatus eval() const override; + std::vector get_dependency() const override; + +private: + DiagnosticLevel level_; +}; + +class LinkExpr : public BaseExpr +{ +public: + LinkExpr(ExprInit & exprs, ConfigObject & config); + void init(ConfigObject & config, std::unordered_map nodes); + ExprStatus eval() const override; + std::vector get_dependency() const override; + +private: + BaseNode * node_; +}; + +class AndExpr : public BaseExpr +{ +public: + AndExpr(ExprInit & exprs, ConfigObject & config, bool short_circuit); + ExprStatus eval() const override; + std::vector get_dependency() const override; + +private: + bool short_circuit_; + std::vector> list_; +}; + +class OrExpr : public BaseExpr +{ +public: + OrExpr(ExprInit & exprs, ConfigObject & config); + ExprStatus eval() const override; + std::vector get_dependency() const override; + +private: + std::vector> list_; +}; + +class ExprInit +{ +public: + explicit ExprInit(const std::string & mode); + std::unique_ptr create(ExprConfig config); + auto get() const { return links_; } + +private: + std::string mode_; + std::vector> links_; +}; + +} // namespace system_diagnostic_graph + +#endif // CORE__EXPRS_HPP_ diff --git a/system/system_diagnostic_graph/src/core/graph.cpp b/system/system_diagnostic_graph/src/core/graph.cpp new file mode 100644 index 0000000000000..b4fd1d15827f3 --- /dev/null +++ b/system/system_diagnostic_graph/src/core/graph.cpp @@ -0,0 +1,204 @@ +// Copyright 2023 The Autoware Contributors +// +// 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 "graph.hpp" + +#include "config.hpp" +#include "exprs.hpp" +#include "nodes.hpp" + +#include +#include +#include +#include +#include +#include + +// DEBUG +#include + +namespace system_diagnostic_graph +{ + +void topological_sort(std::vector> & input) +{ + std::unordered_map degrees; + std::deque nodes; + std::deque result; + std::deque buffer; + + // Create a list of raw pointer nodes. + for (const auto & node : input) { + nodes.push_back(node.get()); + } + + // Count degrees of each node. + for (const auto & node : nodes) { + for (const auto & link : node->links()) { + ++degrees[link]; + } + } + + // Find initial nodes that are zero degrees. + for (const auto & node : nodes) { + if (degrees[node] == 0) { + buffer.push_back(node); + } + } + + // Sort by topological order. + while (!buffer.empty()) { + const auto node = buffer.front(); + buffer.pop_front(); + for (const auto & link : node->links()) { + if (--degrees[link] == 0) { + buffer.push_back(link); + } + } + result.push_back(node); + } + + // Detect circulation because the result does not include the nodes on the loop. + if (result.size() != nodes.size()) { + throw ConfigError("detect graph circulation"); + } + + // Reverse the result to process from leaf node. + result = std::deque(result.rbegin(), result.rend()); + + // Replace node vector. + std::unordered_map indices; + for (size_t i = 0; i < result.size(); ++i) { + indices[result[i]] = i; + } + std::vector> sorted(input.size()); + for (auto & node : input) { + sorted[indices[node.get()]] = std::move(node); + } + input = std::move(sorted); +} + +Graph::Graph() +{ + // for unique_ptr +} + +Graph::~Graph() +{ + // for unique_ptr +} + +void Graph::init(const std::string & file, const std::string & mode) +{ + ConfigFile root = load_config_root(file); + + std::vector> nodes; + std::unordered_map diags; + std::unordered_map paths; + ExprInit exprs(mode); + + for (auto & config : root.units) { + if (config.mode.check(mode)) { + auto node = std::make_unique(config.path); + nodes.push_back(std::move(node)); + } + } + for (auto & config : root.diags) { + if (config.mode.check(mode)) { + auto node = std::make_unique(config.path, config.dict); + diags[node->name()] = node.get(); + nodes.push_back(std::move(node)); + } + } + + // TODO(Takagi, Isamu): unknown diag names + { + auto node = std::make_unique("/unknown"); + unknown_ = node.get(); + nodes.push_back(std::move(node)); + } + + for (const auto & node : nodes) { + paths[node->path()] = node.get(); + } + + for (auto & config : root.units) { + if (paths.count(config.path)) { + paths.at(config.path)->create(config.dict, exprs); + } + } + for (auto & config : root.diags) { + if (paths.count(config.path)) { + paths.at(config.path)->create(config.dict, exprs); + } + } + + for (auto & [link, config] : exprs.get()) { + link->init(config, paths); + } + + // Sort unit nodes in topological order for update dependencies. + topological_sort(nodes); + + // Set the link index for the ros message. + for (size_t i = 0; i < nodes.size(); ++i) { + nodes[i]->set_index(i); + } + + nodes_ = std::move(nodes); + diags_ = diags; +} + +void Graph::callback(const DiagnosticArray & array, const rclcpp::Time & stamp) +{ + for (const auto & status : array.status) { + const auto iter = diags_.find(status.name); + if (iter != diags_.end()) { + iter->second->callback(status, stamp); + } else { + unknown_->callback(status, stamp); + } + } +} + +void Graph::update(const rclcpp::Time & stamp) +{ + for (const auto & node : nodes_) { + node->update(stamp); + } + stamp_ = stamp; +} + +DiagnosticGraph Graph::message() const +{ + DiagnosticGraph result; + result.stamp = stamp_; + result.nodes.reserve(nodes_.size()); + for (const auto & node : nodes_) { + result.nodes.push_back(node->report()); + } + return result; +} + +std::vector Graph::nodes() const +{ + std::vector result; + result.reserve(nodes_.size()); + for (const auto & node : nodes_) { + result.push_back(node.get()); + } + return result; +} + +} // namespace system_diagnostic_graph diff --git a/system/system_diagnostic_graph/src/core/graph.hpp b/system/system_diagnostic_graph/src/core/graph.hpp new file mode 100644 index 0000000000000..e0060c9111592 --- /dev/null +++ b/system/system_diagnostic_graph/src/core/graph.hpp @@ -0,0 +1,54 @@ +// Copyright 2023 The Autoware Contributors +// +// 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 CORE__GRAPH_HPP_ +#define CORE__GRAPH_HPP_ + +#include "types.hpp" + +#include + +#include +#include +#include +#include +#include + +namespace system_diagnostic_graph +{ + +class Graph final +{ +public: + Graph(); + ~Graph(); + + void init(const std::string & file, const std::string & mode); + void callback(const DiagnosticArray & array, const rclcpp::Time & stamp); + void update(const rclcpp::Time & stamp); + DiagnosticGraph message() const; + std::vector nodes() const; + + void debug(); + +private: + std::vector> nodes_; + std::unordered_map diags_; + UnknownNode * unknown_; + rclcpp::Time stamp_; +}; + +} // namespace system_diagnostic_graph + +#endif // CORE__GRAPH_HPP_ diff --git a/system/system_diagnostic_graph/src/core/modes.cpp b/system/system_diagnostic_graph/src/core/modes.cpp new file mode 100644 index 0000000000000..0ca18f84b0407 --- /dev/null +++ b/system/system_diagnostic_graph/src/core/modes.cpp @@ -0,0 +1,75 @@ +// Copyright 2023 The Autoware Contributors +// +// 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 "modes.hpp" + +#include "config.hpp" +#include "nodes.hpp" + +#include +#include +#include + +namespace system_diagnostic_graph +{ + +OperationModes::OperationModes(rclcpp::Node & node, const std::vector & graph) +{ + pub_ = node.create_publisher("/system/operation_mode/availability", rclcpp::QoS(1)); + + using PathNodes = std::unordered_map; + PathNodes paths; + for (const auto & node : graph) { + paths[node->path()] = node; + } + + const auto find_node = [](const PathNodes & paths, const std::string & name) { + const auto iter = paths.find(name); + if (iter != paths.end()) { + return iter->second; + } + throw ConfigError("summary node '" + name + "' does node exist"); + }; + + // clang-format off + stop_mode_ = find_node(paths, "/autoware/modes/stop"); + autonomous_mode_ = find_node(paths, "/autoware/modes/autonomous"); + local_mode_ = find_node(paths, "/autoware/modes/local"); + remote_mode_ = find_node(paths, "/autoware/modes/remote"); + emergency_stop_mrm_ = find_node(paths, "/autoware/modes/emergency-stop"); + comfortable_stop_mrm_ = find_node(paths, "/autoware/modes/comfortable-stop"); + pull_over_mrm_ = find_node(paths, "/autoware/modes/pull-over"); + // clang-format on +} + +void OperationModes::update(const rclcpp::Time & stamp) const +{ + const auto is_ok = [](const BaseNode * node) { return node->level() == DiagnosticStatus::OK; }; + + // clang-format off + Availability message; + message.stamp = stamp; + message.stop = is_ok(stop_mode_); + message.autonomous = is_ok(autonomous_mode_); + message.local = is_ok(local_mode_); + message.remote = is_ok(remote_mode_); + message.emergency_stop = is_ok(emergency_stop_mrm_); + message.comfortable_stop = is_ok(comfortable_stop_mrm_); + message.pull_over = is_ok(pull_over_mrm_); + // clang-format on + + pub_->publish(message); +} + +} // namespace system_diagnostic_graph diff --git a/system/system_diagnostic_graph/src/core/modes.hpp b/system/system_diagnostic_graph/src/core/modes.hpp new file mode 100644 index 0000000000000..ead1ec10dce93 --- /dev/null +++ b/system/system_diagnostic_graph/src/core/modes.hpp @@ -0,0 +1,51 @@ +// Copyright 2023 The Autoware Contributors +// +// 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 CORE__MODES_HPP_ +#define CORE__MODES_HPP_ + +#include "types.hpp" + +#include + +#include + +#include + +namespace system_diagnostic_graph +{ + +class OperationModes +{ +public: + explicit OperationModes(rclcpp::Node & node, const std::vector & graph); + void update(const rclcpp::Time & stamp) const; + +private: + using Availability = tier4_system_msgs::msg::OperationModeAvailability; + rclcpp::TimerBase::SharedPtr timer_; + rclcpp::Publisher::SharedPtr pub_; + + BaseNode * stop_mode_; + BaseNode * autonomous_mode_; + BaseNode * local_mode_; + BaseNode * remote_mode_; + BaseNode * emergency_stop_mrm_; + BaseNode * comfortable_stop_mrm_; + BaseNode * pull_over_mrm_; +}; + +} // namespace system_diagnostic_graph + +#endif // CORE__MODES_HPP_ diff --git a/system/system_diagnostic_graph/src/core/nodes.cpp b/system/system_diagnostic_graph/src/core/nodes.cpp new file mode 100644 index 0000000000000..bbc4bb4d42561 --- /dev/null +++ b/system/system_diagnostic_graph/src/core/nodes.cpp @@ -0,0 +1,157 @@ +// Copyright 2023 The Autoware Contributors +// +// 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 "nodes.hpp" + +#include "exprs.hpp" + +#include + +#include +#include + +namespace system_diagnostic_graph +{ + +BaseNode::BaseNode(const std::string & path) : path_(path) +{ + index_ = 0; +} + +UnitNode::UnitNode(const std::string & path) : BaseNode(path) +{ + level_ = DiagnosticStatus::STALE; +} + +UnitNode::~UnitNode() +{ + // for unique_ptr +} + +void UnitNode::create(ConfigObject & config, ExprInit & exprs) +{ + expr_ = exprs.create(parse_expr_config(config)); +} + +void UnitNode::update(const rclcpp::Time &) +{ + const auto result = expr_->eval(); + level_ = result.level; + links_.clear(); + for (const auto & [node, used] : result.links) { + DiagnosticLink link; + link.index = node->index(); + link.used = used; + links_.push_back(link); + } +} + +DiagnosticNode UnitNode::report() const +{ + DiagnosticNode message; + message.status.level = level_; + message.status.name = path_; + message.links = links_; + return message; +} + +DiagnosticLevel UnitNode::level() const +{ + return level_; +} + +std::vector UnitNode::links() const +{ + return expr_->get_dependency(); +} + +DiagNode::DiagNode(const std::string & path, ConfigObject & config) : BaseNode(path) +{ + timeout_ = 3.0; // TODO(Takagi, Isamu): parameterize + name_ = config.take_text("name"); + + status_.level = DiagnosticStatus::STALE; +} + +void DiagNode::create(ConfigObject &, ExprInit &) +{ +} + +void DiagNode::update(const rclcpp::Time & stamp) +{ + if (time_) { + const auto elapsed = (stamp - time_.value()).seconds(); + if (timeout_ < elapsed) { + status_ = DiagnosticStatus(); + status_.level = DiagnosticStatus::STALE; + time_ = std::nullopt; + } + } +} + +DiagnosticNode DiagNode::report() const +{ + DiagnosticNode message; + message.status = status_; + message.status.name = path_; + return message; +} + +DiagnosticLevel DiagNode::level() const +{ + return status_.level; +} + +void DiagNode::callback(const DiagnosticStatus & status, const rclcpp::Time & stamp) +{ + status_ = status; + time_ = stamp; +} + +UnknownNode::UnknownNode(const std::string & path) : BaseNode(path) +{ +} + +void UnknownNode::create(ConfigObject &, ExprInit &) +{ +} + +void UnknownNode::update(const rclcpp::Time & stamp) +{ + (void)stamp; +} + +DiagnosticNode UnknownNode::report() const +{ + DiagnosticNode message; + message.status.name = path_; + for (const auto & diag : diagnostics_) { + diagnostic_msgs::msg::KeyValue kv; + kv.key = diag.first; + message.status.values.push_back(kv); + } + return message; +} + +DiagnosticLevel UnknownNode::level() const +{ + return DiagnosticStatus::WARN; +} + +void UnknownNode::callback(const DiagnosticStatus & status, const rclcpp::Time & stamp) +{ + diagnostics_[status.name] = stamp; +} + +} // namespace system_diagnostic_graph diff --git a/system/system_diagnostic_graph/src/core/nodes.hpp b/system/system_diagnostic_graph/src/core/nodes.hpp new file mode 100644 index 0000000000000..1a849cf58b268 --- /dev/null +++ b/system/system_diagnostic_graph/src/core/nodes.hpp @@ -0,0 +1,114 @@ +// Copyright 2023 The Autoware Contributors +// +// 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 CORE__NODES_HPP_ +#define CORE__NODES_HPP_ + +#include "config.hpp" +#include "debug.hpp" +#include "types.hpp" + +#include + +#include +#include +#include +#include +#include +#include + +namespace system_diagnostic_graph +{ + +class BaseNode +{ +public: + explicit BaseNode(const std::string & path); + virtual ~BaseNode() = default; + virtual void create(ConfigObject & config, ExprInit & exprs) = 0; + virtual void update(const rclcpp::Time & stamp) = 0; + virtual DiagnosticNode report() const = 0; + virtual DiagnosticLevel level() const = 0; + virtual DiagDebugData debug() const = 0; + virtual std::vector links() const = 0; + + std::string path() const { return path_; } + + size_t index() const { return index_; } + void set_index(const size_t index) { index_ = index; } + +protected: + const std::string path_; + size_t index_; +}; + +class UnitNode : public BaseNode +{ +public: + explicit UnitNode(const std::string & path); + ~UnitNode() override; + void create(ConfigObject & config, ExprInit & exprs) override; + void update(const rclcpp::Time & stamp) override; + DiagnosticNode report() const override; + DiagnosticLevel level() const override; + DiagDebugData debug() const override; + std::vector links() const override; + +private: + std::unique_ptr expr_; + std::vector links_; + DiagnosticLevel level_; +}; + +class DiagNode : public BaseNode +{ +public: + DiagNode(const std::string & path, ConfigObject & config); + void create(ConfigObject & config, ExprInit & exprs) override; + void update(const rclcpp::Time & stamp) override; + DiagnosticNode report() const override; + DiagnosticLevel level() const override; + DiagDebugData debug() const override; + std::vector links() const override { return {}; } + std::string name() const { return name_; } + + void callback(const DiagnosticStatus & status, const rclcpp::Time & stamp); + +private: + double timeout_; + std::optional time_; + std::string name_; + DiagnosticStatus status_; +}; + +class UnknownNode : public BaseNode +{ +public: + explicit UnknownNode(const std::string & path); + void create(ConfigObject & config, ExprInit & exprs) override; + void update(const rclcpp::Time & stamp) override; + DiagnosticNode report() const override; + DiagnosticLevel level() const override; + DiagDebugData debug() const override; + std::vector links() const override { return {}; } + + void callback(const DiagnosticStatus & status, const rclcpp::Time & stamp); + +private: + std::map diagnostics_; +}; + +} // namespace system_diagnostic_graph + +#endif // CORE__NODES_HPP_ diff --git a/system/system_diagnostic_graph/src/core/types.hpp b/system/system_diagnostic_graph/src/core/types.hpp new file mode 100644 index 0000000000000..2adfbb3f9d4ef --- /dev/null +++ b/system/system_diagnostic_graph/src/core/types.hpp @@ -0,0 +1,44 @@ +// Copyright 2023 The Autoware Contributors +// +// 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 CORE__TYPES_HPP_ +#define CORE__TYPES_HPP_ + +#include +#include +#include +#include +#include + +namespace system_diagnostic_graph +{ + +using diagnostic_msgs::msg::DiagnosticArray; +using diagnostic_msgs::msg::DiagnosticStatus; +using tier4_system_msgs::msg::DiagnosticGraph; +using tier4_system_msgs::msg::DiagnosticLink; +using tier4_system_msgs::msg::DiagnosticNode; +using DiagnosticLevel = DiagnosticStatus::_level_type; + +class BaseNode; +class UnitNode; +class DiagNode; +class UnknownNode; + +class BaseExpr; +class ExprInit; + +} // namespace system_diagnostic_graph + +#endif // CORE__TYPES_HPP_ diff --git a/system/system_diagnostic_graph/src/main.cpp b/system/system_diagnostic_graph/src/main.cpp new file mode 100644 index 0000000000000..722ddcf833577 --- /dev/null +++ b/system/system_diagnostic_graph/src/main.cpp @@ -0,0 +1,85 @@ +// Copyright 2023 The Autoware Contributors +// +// 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 "main.hpp" + +#include +#include + +namespace system_diagnostic_graph +{ + +MainNode::MainNode() : Node("system_diagnostic_graph_aggregator") +{ + // Init diagnostics graph. + { + const auto file = declare_parameter("graph_file"); + const auto mode = declare_parameter("mode"); + graph_.init(file, mode); + graph_.debug(); + } + + // Init plugins + if (declare_parameter("mode_availability")) { + modes_ = std::make_unique(*this, graph_.nodes()); + } + + // Init ros interface. + { + using std::placeholders::_1; + const auto qos_input = rclcpp::QoS(declare_parameter("input_qos_depth")); + const auto qos_graph = rclcpp::QoS(declare_parameter("graph_qos_depth")); + + const auto callback = std::bind(&MainNode::on_diag, this, _1); + sub_input_ = create_subscription("/diagnostics", qos_input, callback); + pub_graph_ = create_publisher("/diagnostics_graph", qos_graph); + + const auto rate = rclcpp::Rate(declare_parameter("rate")); + timer_ = rclcpp::create_timer(this, get_clock(), rate.period(), [this]() { on_timer(); }); + } +} + +MainNode::~MainNode() +{ + // for unique_ptr +} + +void MainNode::on_timer() +{ + const auto stamp = now(); + graph_.update(stamp); + graph_.debug(); + pub_graph_->publish(graph_.message()); + + if (modes_) modes_->update(stamp); +} + +void MainNode::on_diag(const DiagnosticArray::ConstSharedPtr msg) +{ + graph_.callback(*msg, now()); +} + +} // namespace system_diagnostic_graph + +int main(int argc, char ** argv) +{ + using system_diagnostic_graph::MainNode; + rclcpp::init(argc, argv); + rclcpp::executors::SingleThreadedExecutor executor; + auto node = std::make_shared(); + executor.add_node(node); + executor.spin(); + executor.remove_node(node); + rclcpp::shutdown(); +} diff --git a/system/system_diagnostic_graph/src/main.hpp b/system/system_diagnostic_graph/src/main.hpp new file mode 100644 index 0000000000000..6deb0518cd9d0 --- /dev/null +++ b/system/system_diagnostic_graph/src/main.hpp @@ -0,0 +1,47 @@ +// Copyright 2023 The Autoware Contributors +// +// 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 MAIN_HPP_ +#define MAIN_HPP_ + +#include "core/graph.hpp" +#include "core/modes.hpp" +#include "core/types.hpp" + +#include + +#include + +namespace system_diagnostic_graph +{ + +class MainNode : public rclcpp::Node +{ +public: + MainNode(); + ~MainNode(); + +private: + Graph graph_; + std::unique_ptr modes_; + rclcpp::TimerBase::SharedPtr timer_; + rclcpp::Subscription::SharedPtr sub_input_; + rclcpp::Publisher::SharedPtr pub_graph_; + void on_timer(); + void on_diag(const DiagnosticArray::ConstSharedPtr msg); +}; + +} // namespace system_diagnostic_graph + +#endif // MAIN_HPP_ diff --git a/system/system_diagnostic_graph/src/tool.cpp b/system/system_diagnostic_graph/src/tool.cpp new file mode 100644 index 0000000000000..fda2c88a691a2 --- /dev/null +++ b/system/system_diagnostic_graph/src/tool.cpp @@ -0,0 +1,81 @@ +// Copyright 2023 The Autoware Contributors +// +// 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 "tool.hpp" + +#include +#include + +namespace system_diagnostic_graph +{ + +std::string level_to_string(DiagnosticLevel level) +{ + switch (level) { + case DiagnosticStatus::OK: + return "OK"; + case DiagnosticStatus::WARN: + return "WARN"; + case DiagnosticStatus::ERROR: + return "ERROR"; + case DiagnosticStatus::STALE: + return "STALE"; + } + return "UNKNOWN"; +} + +ToolNode::ToolNode() : Node("system_diagnostic_graph_converter") +{ + using std::placeholders::_1; + const auto qos_graph = rclcpp::QoS(1); + const auto qos_array = rclcpp::QoS(1); + + const auto callback = std::bind(&ToolNode::on_graph, this, _1); + sub_graph_ = create_subscription("/diagnostics_graph", qos_graph, callback); + pub_array_ = create_publisher("/diagnostics_array", qos_array); +} + +void ToolNode::on_graph(const DiagnosticGraph::ConstSharedPtr msg) +{ + DiagnosticArray message; + message.header.stamp = msg->stamp; + message.status.reserve(msg->nodes.size()); + for (const auto & node : msg->nodes) { + message.status.push_back(node.status); + for (const auto & link : node.links) { + diagnostic_msgs::msg::KeyValue kv; + const auto & status = msg->nodes[link.index].status; + kv.key = status.name; + kv.value = level_to_string(status.level); + if (link.used) { + message.status.back().values.push_back(kv); + } + } + } + pub_array_->publish(message); +} + +} // namespace system_diagnostic_graph + +int main(int argc, char ** argv) +{ + using system_diagnostic_graph::ToolNode; + rclcpp::init(argc, argv); + rclcpp::executors::SingleThreadedExecutor executor; + auto node = std::make_shared(); + executor.add_node(node); + executor.spin(); + executor.remove_node(node); + rclcpp::shutdown(); +} diff --git a/system/system_diagnostic_graph/src/tool.hpp b/system/system_diagnostic_graph/src/tool.hpp new file mode 100644 index 0000000000000..5ad19b8460c4b --- /dev/null +++ b/system/system_diagnostic_graph/src/tool.hpp @@ -0,0 +1,38 @@ +// Copyright 2023 The Autoware Contributors +// +// 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 TOOL_HPP_ +#define TOOL_HPP_ + +#include "core/types.hpp" + +#include + +namespace system_diagnostic_graph +{ + +class ToolNode : public rclcpp::Node +{ +public: + ToolNode(); + +private: + rclcpp::Subscription::SharedPtr sub_graph_; + rclcpp::Publisher::SharedPtr pub_array_; + void on_graph(const DiagnosticGraph::ConstSharedPtr msg); +}; + +} // namespace system_diagnostic_graph + +#endif // TOOL_HPP_ diff --git a/system/system_diagnostic_graph/util/debug.py b/system/system_diagnostic_graph/util/debug.py new file mode 100755 index 0000000000000..dac917a26a2f6 --- /dev/null +++ b/system/system_diagnostic_graph/util/debug.py @@ -0,0 +1,85 @@ +#!/usr/bin/env python3 + +# Copyright 2023 The Autoware Contributors +# +# 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 rclpy +from rclpy.node import Node +from rclpy.qos import QoSDurabilityPolicy +from rclpy.qos import QoSProfile +from tier4_system_msgs.msg import DiagnosticGraph + + +class StructNode(Node): + def __init__(self): + super().__init__("system_diagnostic_graph_tools_struct") + qos_struct = QoSProfile(depth=1, durability=QoSDurabilityPolicy.TRANSIENT_LOCAL) + self.sub_struct = self.create_subscription( + DiagnosticGraph, "/diagnostics_graph/struct", self.callback, qos_struct + ) + self.message = None + + def callback(self, msg): + self.message = msg + + +class NodeSpinner: + def __init__(self, time): + self.time = time + self.wait = True + + def on_timer(self): + self.wait = False + + def spin(self, node): + timer = node.create_timer(self.time, self.on_timer) + while self.wait: + rclpy.spin_once(node) + node.destroy_timer(timer) + + +class GraphNode: + def __init__(self, msg): + self.name = msg.name + self.links = msg.links + + +class GraphStruct: + def __init__(self, msg): + self.nodes = [GraphNode(node) for node in msg.nodes] + + @staticmethod + def Subscribe(): + spin = NodeSpinner(1.0) + node = StructNode() + spin.spin(node) + return GraphStruct(node.message) + + def visualize(self): + from graphviz import Digraph + + graph = Digraph() + graph.attr("node", shape="box") + for node in self.nodes: + graph.node(node.name) + for link in node.links: + graph.edge(node.name, self.nodes[link].name) + graph.view() + + +if __name__ == "__main__": + rclpy.init() + graph = GraphStruct.Subscribe() + rclpy.shutdown() + graph.visualize() diff --git a/system/system_error_monitor/config/diagnostic_aggregator/localization.param.yaml b/system/system_error_monitor/config/diagnostic_aggregator/localization.param.yaml index 2d0b2b00af343..f82f7d732f5dc 100644 --- a/system/system_error_monitor/config/diagnostic_aggregator/localization.param.yaml +++ b/system/system_error_monitor/config/diagnostic_aggregator/localization.param.yaml @@ -27,5 +27,5 @@ localization_accuracy: type: diagnostic_aggregator/GenericAnalyzer path: localization_accuracy - contains: [": localization_accuracy"] + contains: ["localization: localization_error_monitor"] timeout: 1.0 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 87cf767accc06..f6e13012957aa 100644 --- a/system/system_error_monitor/config/diagnostic_aggregator/system.param.yaml +++ b/system/system_error_monitor/config/diagnostic_aggregator/system.param.yaml @@ -26,6 +26,12 @@ contains: ["service_log_checker"] timeout: 5.0 + duplicated_node_checker: + type: diagnostic_aggregator/GenericAnalyzer + path: duplicated_node_checker + contains: ["duplicated_node_checker"] + timeout: 5.0 + resource_monitoring: type: diagnostic_aggregator/AnalyzerGroup path: resource_monitoring diff --git a/system/system_error_monitor/config/system_error_monitor.param.yaml b/system/system_error_monitor/config/system_error_monitor.param.yaml index 312a2e6186c42..1778f6594f0c3 100644 --- a/system/system_error_monitor/config/system_error_monitor.param.yaml +++ b/system/system_error_monitor/config/system_error_monitor.param.yaml @@ -39,6 +39,7 @@ /autoware/system/emergency_stop_operation: default /autoware/system/service_log_checker: { sf_at: "warn", lf_at: "none", spf_at: "none" } /autoware/system/resource_monitoring: { sf_at: "warn", lf_at: "none", spf_at: "none" } + /autoware/system/duplicated_node_checker: default /autoware/vehicle/node_alive_monitoring: default diff --git a/system/system_error_monitor/config/system_error_monitor.planning_simulation.param.yaml b/system/system_error_monitor/config/system_error_monitor.planning_simulation.param.yaml index 88431dc406fe3..9784259490ec2 100644 --- a/system/system_error_monitor/config/system_error_monitor.planning_simulation.param.yaml +++ b/system/system_error_monitor/config/system_error_monitor.planning_simulation.param.yaml @@ -38,7 +38,9 @@ /autoware/system/node_alive_monitoring: default /autoware/system/emergency_stop_operation: default /autoware/system/service_log_checker: { sf_at: "warn", lf_at: "none", spf_at: "none" } + /autoware/system/duplicated_node_checker: default # /autoware/system/resource_monitoring: { sf_at: "warn", lf_at: "error", spf_at: "none" } + # /autoware/system/duplicated_node_checker: default /autoware/vehicle/node_alive_monitoring: default diff --git a/system/system_error_monitor/include/system_error_monitor/system_error_monitor_core.hpp b/system/system_error_monitor/include/system_error_monitor/system_error_monitor_core.hpp index 75d80b56ea711..0bb95882700a5 100644 --- a/system/system_error_monitor/include/system_error_monitor/system_error_monitor_core.hpp +++ b/system/system_error_monitor/include/system_error_monitor/system_error_monitor_core.hpp @@ -15,6 +15,8 @@ #ifndef SYSTEM_ERROR_MONITOR__SYSTEM_ERROR_MONITOR_CORE_HPP_ #define SYSTEM_ERROR_MONITOR__SYSTEM_ERROR_MONITOR_CORE_HPP_ +#include "tier4_autoware_utils/ros/logger_level_configure.hpp" + #include #include @@ -29,6 +31,7 @@ #include #include +#include #include #include #include @@ -144,6 +147,8 @@ class AutowareErrorMonitor : public rclcpp::Node void updateTimeoutHazardStatus(); bool canAutoRecovery() const; bool isEmergencyHoldingRequired() const; + + std::unique_ptr logger_configure_; }; #endif // SYSTEM_ERROR_MONITOR__SYSTEM_ERROR_MONITOR_CORE_HPP_ diff --git a/system/system_error_monitor/src/system_error_monitor_core.cpp b/system/system_error_monitor/src/system_error_monitor_core.cpp index 1bd7caa19a220..e74ac9c360164 100644 --- a/system/system_error_monitor/src/system_error_monitor_core.cpp +++ b/system/system_error_monitor/src/system_error_monitor_core.cpp @@ -274,6 +274,8 @@ AutowareErrorMonitor::AutowareErrorMonitor() const auto period_ns = rclcpp::Rate(params_.update_rate).period(); timer_ = rclcpp::create_timer( this, get_clock(), period_ns, std::bind(&AutowareErrorMonitor::onTimer, this)); + + logger_configure_ = std::make_unique(this); } void AutowareErrorMonitor::loadRequiredModules(const std::string & key) diff --git a/system/system_monitor/config/ntp_monitor.param.yaml b/system/system_monitor/config/ntp_monitor.param.yaml index db54f70d1ce59..5f46821f98aff 100644 --- a/system/system_monitor/config/ntp_monitor.param.yaml +++ b/system/system_monitor/config/ntp_monitor.param.yaml @@ -3,3 +3,4 @@ server: ntp.nict.jp offset_warn: 0.1 offset_error: 5.0 + timeout: 5 # The chronyc execution timeout will trigger a warning when this timer expires. diff --git a/system/system_monitor/include/system_monitor/ntp_monitor/ntp_monitor.hpp b/system/system_monitor/include/system_monitor/ntp_monitor/ntp_monitor.hpp index f4ee2de666c22..db264d4b7eda2 100644 --- a/system/system_monitor/include/system_monitor/ntp_monitor/ntp_monitor.hpp +++ b/system/system_monitor/include/system_monitor/ntp_monitor/ntp_monitor.hpp @@ -53,6 +53,16 @@ class NTPMonitor : public rclcpp::Node void checkOffset( diagnostic_updater::DiagnosticStatusWrapper & stat); // NOLINT(runtime/references) + /** + * @brief Timer callback to execute chronyc command + */ + void onTimer(); + + /** + * @brief Timeout callback function for executing chronyc + */ + void onTimeout(); + /** * @brief function to execute chronyc * @param [out] outOffset offset value of NTP time @@ -71,6 +81,19 @@ class NTPMonitor : public rclcpp::Node float offset_warn_; //!< @brief NTP offset(sec) to generate warning float offset_error_; //!< @brief NTP offset(sec) to generate error + int timeout_; //!< @brief Timeout duration for executing chronyc + + rclcpp::TimerBase::SharedPtr timer_; //!< @brief Timer to execute chronyc command + rclcpp::CallbackGroup::SharedPtr timer_callback_group_; //!< @brief Callback Group + std::mutex mutex_; //!< @brief Mutex for output from chronyc command + std::string error_str_; //!< @brief Error string + std::string pipe2_err_str_; //!< @brief Error string regarding pipe2 function call + float offset_; //!< @brief Offset value of NTP time + std::map tracking_map_; //!< @brief Output of chronyc tracking + double elapsed_ms_; //!< @brief Execution time of chronyc command + rclcpp::TimerBase::SharedPtr timeout_timer_; //!< @brief Timeout for executing chronyc + std::mutex timeout_mutex_; //!< @brief Mutex regarding timeout for executing chronyc + bool timeout_expired_; //!< @brief Timeout for executing chronyc has expired or not /** * @brief NTP offset status messages diff --git a/system/system_monitor/include/system_monitor/process_monitor/process_monitor.hpp b/system/system_monitor/include/system_monitor/process_monitor/process_monitor.hpp index e114d58770883..42bd2a3ea9236 100644 --- a/system/system_monitor/include/system_monitor/process_monitor/process_monitor.hpp +++ b/system/system_monitor/include/system_monitor/process_monitor/process_monitor.hpp @@ -88,6 +88,14 @@ class ProcessMonitor : public rclcpp::Node */ void getHighMemoryProcesses(const std::string & output); + /** + * @brief get command line from process id + * @param [in] pid process id + * @param [out] command output command line + * @return true if success to get command line name + */ + bool getCommandLineFromPiD(const std::string & pid, std::string * command); + /** * @brief get top-rated processes * @param [in] tasks list of diagnostics tasks for high load procs diff --git a/system/system_monitor/src/ntp_monitor/ntp_monitor.cpp b/system/system_monitor/src/ntp_monitor/ntp_monitor.cpp index 6ccf7a54b940f..0b91b28b2b5f9 100644 --- a/system/system_monitor/src/ntp_monitor/ntp_monitor.cpp +++ b/system/system_monitor/src/ntp_monitor/ntp_monitor.cpp @@ -19,7 +19,7 @@ #include "system_monitor/ntp_monitor/ntp_monitor.hpp" -#include "system_monitor/system_monitor_utility.hpp" +#include #include #include @@ -37,8 +37,12 @@ NTPMonitor::NTPMonitor(const rclcpp::NodeOptions & options) : Node("ntp_monitor", options), updater_(this), offset_warn_(declare_parameter("offset_warn", 0.1)), - offset_error_(declare_parameter("offset_error", 5.0)) + offset_error_(declare_parameter("offset_error", 5.0)), + timeout_(declare_parameter("timeout", 5)), + timeout_expired_(false) { + using namespace std::literals::chrono_literals; + gethostname(hostname_, sizeof(hostname_)); // Check if command exists @@ -47,18 +51,15 @@ NTPMonitor::NTPMonitor(const rclcpp::NodeOptions & options) updater_.setHardwareID(hostname_); updater_.add("NTP Offset", this, &NTPMonitor::checkOffset); -} -void NTPMonitor::update() -{ - updater_.force_update(); + // Start timer to execute top command + timer_callback_group_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + timer_ = rclcpp::create_timer( + this, get_clock(), 1s, std::bind(&NTPMonitor::onTimer, this), timer_callback_group_); } void NTPMonitor::checkOffset(diagnostic_updater::DiagnosticStatusWrapper & stat) { - // Remember start time to measure elapsed time - const auto t_start = SystemMonitorUtility::startMeasurement(); - if (!chronyc_exists_) { stat.summary(DiagStatus::ERROR, "chronyc error"); stat.add( @@ -70,7 +71,18 @@ void NTPMonitor::checkOffset(diagnostic_updater::DiagnosticStatusWrapper & stat) std::string pipe2_err_str; float offset = 0.0f; std::map tracking_map; - error_str = executeChronyc(offset, tracking_map, pipe2_err_str); + double elapsed_ms; + + // thread-safe copy + { + std::lock_guard lock(mutex_); + error_str = error_str_; + pipe2_err_str = pipe2_err_str_; + offset = offset_; + tracking_map = tracking_map_; + elapsed_ms = elapsed_ms_; + } + if (!pipe2_err_str.empty()) { stat.summary(DiagStatus::ERROR, "pipe2 error"); stat.add("pipe2", pipe2_err_str); @@ -94,10 +106,65 @@ void NTPMonitor::checkOffset(diagnostic_updater::DiagnosticStatusWrapper & stat) for (auto itr = tracking_map.begin(); itr != tracking_map.end(); ++itr) { stat.add(itr->first, itr->second); } - stat.summary(level, offset_dict_.at(level)); - // Measure elapsed time since start time and report - SystemMonitorUtility::stopMeasurement(t_start, stat); + // Check timeout has expired regarding executing chronyc + bool timeout_expired = false; + { + std::lock_guard lock(timeout_mutex_); + timeout_expired = timeout_expired_; + } + + if (!timeout_expired) { + stat.summary(level, offset_dict_.at(level)); + } else { + stat.summary(DiagStatus::WARN, "chronyc timeout expired"); + } + + stat.addf("execution time", "%f ms", elapsed_ms); +} + +void NTPMonitor::onTimer() +{ + // Start to measure elapsed time + tier4_autoware_utils::StopWatch stop_watch; + stop_watch.tic("execution_time"); + + std::string error_str; + std::string pipe2_err_str; + float offset = 0.0f; + std::map tracking_map; + + // Start timeout timer for executing chronyc + { + std::lock_guard lock(timeout_mutex_); + timeout_expired_ = false; + } + timeout_timer_ = rclcpp::create_timer( + this, get_clock(), std::chrono::seconds(timeout_), std::bind(&NTPMonitor::onTimeout, this)); + + error_str = executeChronyc(offset, tracking_map, pipe2_err_str); + + // Returning from chronyc, stop timeout timer + timeout_timer_->cancel(); + + const double elapsed_ms = stop_watch.toc("execution_time"); + + // thread-safe copy + { + std::lock_guard lock(mutex_); + error_str_ = error_str; + pipe2_err_str_ = pipe2_err_str; + offset_ = offset; + tracking_map_ = tracking_map; + elapsed_ms_ = elapsed_ms; + } +} + +void NTPMonitor::onTimeout() +{ + RCLCPP_WARN(get_logger(), "Timeout occurred."); + std::lock_guard lock(timeout_mutex_); + timeout_expired_ = true; } std::string NTPMonitor::executeChronyc( diff --git a/system/system_monitor/src/process_monitor/process_monitor.cpp b/system/system_monitor/src/process_monitor/process_monitor.cpp index f8ee314e5a0ef..6f58339f2ff4b 100644 --- a/system/system_monitor/src/process_monitor/process_monitor.cpp +++ b/system/system_monitor/src/process_monitor/process_monitor.cpp @@ -414,6 +414,19 @@ void ProcessMonitor::getHighMemoryProcesses(const std::string & output) getTopratedProcesses(&memory_tasks_, &p2); } +bool ProcessMonitor::getCommandLineFromPiD(const std::string & pid, std::string * command) +{ + std::string commandLineFilePath = "/proc/" + pid + "/cmdline"; + std::ifstream commandFile(commandLineFilePath); + if (commandFile.is_open()) { + std::getline(commandFile, *command); + commandFile.close(); + return true; + } else { + return false; + } +} + void ProcessMonitor::getTopratedProcesses( std::vector> * tasks, bp::pipe * p) { @@ -462,7 +475,14 @@ void ProcessMonitor::getTopratedProcesses( info.virtualImage >> info.residentSize >> info.sharedMemSize >> info.processStatus >> info.cpuUsage >> info.memoryUsage >> info.cpuTime; - std::getline(stream, info.commandName); + std::string program_name; + std::getline(stream, program_name); + + bool flag_find_command_line = getCommandLineFromPiD(info.processId, &info.commandName); + + if (!flag_find_command_line) { + info.commandName = program_name; // if command line is not found, use program name instead + } tasks->at(index)->setDiagnosticsStatus(DiagStatus::OK, "OK"); tasks->at(index)->setProcessInformation(info); @@ -515,7 +535,7 @@ void ProcessMonitor::onTimer() std::ostringstream os; // Get processes - bp::child c("top -bcn1 -o %CPU -w 256", bp::std_out > is_out, bp::std_err > is_err); + bp::child c("top -bn1 -o %CPU -w 128", bp::std_out > is_out, bp::std_err > is_err); c.wait(); if (c.exit_code() != 0) { 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 326193a58dc4e..68fa3764d8ecd 100644 --- a/system/topic_state_monitor/src/topic_state_monitor_core.cpp +++ b/system/topic_state_monitor/src/topic_state_monitor_core.cpp @@ -151,6 +151,13 @@ void TopicStateMonitorNode::checkTopicStatus(diagnostic_updater::DiagnosticStatu stat.addf("topic", "%s", node_param_.topic.c_str()); } + const auto print_warn = [&](const std::string & msg) { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 3000, "%s", msg.c_str()); + }; + const auto print_debug = [&](const std::string & msg) { + RCLCPP_DEBUG_THROTTLE(get_logger(), *get_clock(), 3000, "%s", msg.c_str()); + }; + // Judge level int8_t level = DiagnosticStatus::OK; if (topic_status == TopicStatus::Ok) { @@ -159,15 +166,19 @@ void TopicStateMonitorNode::checkTopicStatus(diagnostic_updater::DiagnosticStatu } else if (topic_status == TopicStatus::NotReceived) { level = DiagnosticStatus::ERROR; stat.add("status", "NotReceived"); + print_debug(node_param_.topic + " has not received."); } else if (topic_status == TopicStatus::WarnRate) { level = DiagnosticStatus::WARN; stat.add("status", "WarnRate"); + print_warn(node_param_.topic + " topic rate has dropped to the warning level."); } else if (topic_status == TopicStatus::ErrorRate) { level = DiagnosticStatus::ERROR; stat.add("status", "ErrorRate"); + print_warn(node_param_.topic + " topic rate has dropped to the error level."); } else if (topic_status == TopicStatus::Timeout) { level = DiagnosticStatus::ERROR; stat.add("status", "Timeout"); + print_warn(node_param_.topic + " topic is timeout."); } // Add key-value 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 e2fc1b3a37362..3580f0546bbfb 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 @@ -22,10 +22,10 @@ #include "raw_vehicle_cmd_converter/brake_map.hpp" #include "rclcpp/rclcpp.hpp" #include "tf2/utils.h" +#include "tier4_autoware_utils/ros/logger_level_configure.hpp" #include "tier4_autoware_utils/ros/transform_listener.hpp" #include -#include #include "autoware_auto_vehicle_msgs/msg/steering_report.hpp" #include "autoware_auto_vehicle_msgs/msg/velocity_report.hpp" @@ -44,6 +44,7 @@ #include "tier4_vehicle_msgs/msg/actuation_command_stamped.hpp" #include "tier4_vehicle_msgs/msg/actuation_status_stamped.hpp" #include "tier4_vehicle_msgs/srv/update_accel_brake_map.hpp" +#include "visualization_msgs/msg/marker_array.hpp" #include #include @@ -369,6 +370,8 @@ class AccelBrakeMapCalibrator : public rclcpp::Node COMMAND = 1, }; + std::unique_ptr logger_configure_; + public: explicit AccelBrakeMapCalibrator(const rclcpp::NodeOptions & node_options); }; diff --git a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/rviz/occupancy.rviz b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/rviz/occupancy.rviz index c430c21562f32..ffcb63f428f1b 100644 --- a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/rviz/occupancy.rviz +++ b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/rviz/occupancy.rviz @@ -17,10 +17,6 @@ Panels: - /Current View1 Name: Views Splitter Ratio: 0.5 - - Class: tier4_localization_rviz_plugin/InitialPoseButtonPanel - Name: InitialPoseButtonPanel - - Class: tier4_localization_rviz_plugin/InitialPoseButtonPanel - Name: InitialPoseButtonPanel Preferences: PromptSaveOnExit: true Toolbars: @@ -133,8 +129,6 @@ Window Geometry: Height: 1862 Hide Left Dock: true Hide Right Dock: true - InitialPoseButtonPanel: - collapsed: false QMainWindow State: 000000ff00000000fd0000000400000000000005070000068cfc020000000efb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003d0000068c000000c900fffffffb0000000a00560069006500770073000000030c000003bd000000a400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007300000005470000012b0000000000000000fb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d0065007200610100000682000000eb0000000000000000fb0000000a0049006d00610067006501000005d6000000f30000000000000000fb0000002c0049006e0069007400690061006c0050006f007300650042007500740074006f006e00500061006e0065006c000000068f000000de0000007700fffffffb0000002c0049006e0069007400690061006c0050006f007300650042007500740074006f006e00500061006e0065006c000000068f000000de0000007700fffffffb0000000a0049006d00610067006500000002ef000003da0000000000000000000000010000015f000006fffc0200000002fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000006fb0000005afc0100000002fb0000000800540069006d00650100000000000006fb000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000006fb0000068c00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false 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 1910631b11e53..ecfc312d2bdfd 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 @@ -232,6 +232,8 @@ AccelBrakeMapCalibrator::AccelBrakeMapCalibrator(const rclcpp::NodeOptions & nod // timer initTimer(1.0 / update_hz_); initOutputCSVTimer(30.0); + + logger_configure_ = std::make_unique(this); } void AccelBrakeMapCalibrator::initOutputCSVTimer(double period_s) diff --git a/vehicle/raw_vehicle_cmd_converter/include/raw_vehicle_cmd_converter/node.hpp b/vehicle/raw_vehicle_cmd_converter/include/raw_vehicle_cmd_converter/node.hpp index 27346e99b60fb..98693337b5c28 100644 --- a/vehicle/raw_vehicle_cmd_converter/include/raw_vehicle_cmd_converter/node.hpp +++ b/vehicle/raw_vehicle_cmd_converter/include/raw_vehicle_cmd_converter/node.hpp @@ -19,6 +19,7 @@ #include "raw_vehicle_cmd_converter/brake_map.hpp" #include "raw_vehicle_cmd_converter/pid.hpp" #include "raw_vehicle_cmd_converter/steer_map.hpp" +#include "tier4_autoware_utils/ros/logger_level_configure.hpp" #include @@ -115,6 +116,8 @@ class RawVehicleCommandConverterNode : public rclcpp::Node // for debugging rclcpp::Publisher::SharedPtr debug_pub_steer_pid_; DebugValues debug_steer_; + + std::unique_ptr logger_configure_; }; } // namespace raw_vehicle_cmd_converter diff --git a/vehicle/raw_vehicle_cmd_converter/src/node.cpp b/vehicle/raw_vehicle_cmd_converter/src/node.cpp index e45439ecb4d41..0b0e14a845200 100644 --- a/vehicle/raw_vehicle_cmd_converter/src/node.cpp +++ b/vehicle/raw_vehicle_cmd_converter/src/node.cpp @@ -84,6 +84,8 @@ RawVehicleCommandConverterNode::RawVehicleCommandConverterNode( "~/input/steering", 1, std::bind(&RawVehicleCommandConverterNode::onSteering, this, _1)); debug_pub_steer_pid_ = create_publisher( "/vehicle/raw_vehicle_cmd_converter/debug/steer_pid", 1); + + logger_configure_ = std::make_unique(this); } void RawVehicleCommandConverterNode::publishActuationCmd() diff --git a/vehicle/vehicle_info_util/CMakeLists.txt b/vehicle/vehicle_info_util/CMakeLists.txt index f490b0680a19a..ca72b0ed2db65 100644 --- a/vehicle/vehicle_info_util/CMakeLists.txt +++ b/vehicle/vehicle_info_util/CMakeLists.txt @@ -5,6 +5,7 @@ find_package(autoware_cmake REQUIRED) autoware_package() ament_auto_add_library(vehicle_info_util SHARED + src/vehicle_info.cpp src/vehicle_info_util.cpp ) diff --git a/vehicle/vehicle_info_util/include/vehicle_info_util/vehicle_info.hpp b/vehicle/vehicle_info_util/include/vehicle_info_util/vehicle_info.hpp index 967e71933a209..cdbbe0427b145 100644 --- a/vehicle/vehicle_info_util/include/vehicle_info_util/vehicle_info.hpp +++ b/vehicle/vehicle_info_util/include/vehicle_info_util/vehicle_info.hpp @@ -15,7 +15,7 @@ #ifndef VEHICLE_INFO_UTIL__VEHICLE_INFO_HPP_ #define VEHICLE_INFO_UTIL__VEHICLE_INFO_HPP_ -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" +#include "tier4_autoware_utils/geometry/boost_geometry.hpp" namespace vehicle_info_util { @@ -47,76 +47,18 @@ struct VehicleInfo double min_height_offset_m; double max_height_offset_m; - tier4_autoware_utils::LinearRing2d createFootprint(const double margin = 0.0) const - { - return createFootprint(margin, margin); - } - + tier4_autoware_utils::LinearRing2d createFootprint(const double margin = 0.0) const; tier4_autoware_utils::LinearRing2d createFootprint( - const double lat_margin, const double lon_margin) const - { - using tier4_autoware_utils::LinearRing2d; - using tier4_autoware_utils::Point2d; - - const double x_front = front_overhang_m + wheel_base_m + lon_margin; - const double x_center = wheel_base_m / 2.0; - const double x_rear = -(rear_overhang_m + lon_margin); - const double y_left = wheel_tread_m / 2.0 + left_overhang_m + lat_margin; - const double y_right = -(wheel_tread_m / 2.0 + right_overhang_m + lat_margin); - - LinearRing2d footprint; - footprint.push_back(Point2d{x_front, y_left}); - footprint.push_back(Point2d{x_front, y_right}); - footprint.push_back(Point2d{x_center, y_right}); - footprint.push_back(Point2d{x_rear, y_right}); - footprint.push_back(Point2d{x_rear, y_left}); - footprint.push_back(Point2d{x_center, y_left}); - footprint.push_back(Point2d{x_front, y_left}); - - return footprint; - } + const double lat_margin, const double lon_margin) const; }; /// Create vehicle info from base parameters -inline VehicleInfo createVehicleInfo( +VehicleInfo createVehicleInfo( const double wheel_radius_m, const double wheel_width_m, const double wheel_base_m, const double wheel_tread_m, const double front_overhang_m, const double rear_overhang_m, const double left_overhang_m, const double right_overhang_m, const double vehicle_height_m, - const double max_steer_angle_rad) -{ - // Calculate derived parameters - const double vehicle_length_m_ = front_overhang_m + wheel_base_m + rear_overhang_m; - const double vehicle_width_m_ = wheel_tread_m + left_overhang_m + right_overhang_m; - const double min_longitudinal_offset_m_ = -rear_overhang_m; - const double max_longitudinal_offset_m_ = front_overhang_m + wheel_base_m; - const double min_lateral_offset_m_ = -(wheel_tread_m / 2.0 + right_overhang_m); - const double max_lateral_offset_m_ = wheel_tread_m / 2.0 + left_overhang_m; - const double min_height_offset_m_ = 0.0; - const double max_height_offset_m_ = vehicle_height_m; + const double max_steer_angle_rad); - return VehicleInfo{ - // Base parameters - wheel_radius_m, - wheel_width_m, - wheel_base_m, - wheel_tread_m, - front_overhang_m, - rear_overhang_m, - left_overhang_m, - right_overhang_m, - vehicle_height_m, - max_steer_angle_rad, - // Derived parameters - vehicle_length_m_, - vehicle_width_m_, - min_longitudinal_offset_m_, - max_longitudinal_offset_m_, - min_lateral_offset_m_, - max_lateral_offset_m_, - min_height_offset_m_, - max_height_offset_m_, - }; -} } // namespace vehicle_info_util #endif // VEHICLE_INFO_UTIL__VEHICLE_INFO_HPP_ diff --git a/vehicle/vehicle_info_util/package.xml b/vehicle/vehicle_info_util/package.xml index 41cb10003a1de..139f8439d60c9 100644 --- a/vehicle/vehicle_info_util/package.xml +++ b/vehicle/vehicle_info_util/package.xml @@ -4,7 +4,6 @@ vehicle_info_util 0.1.0 The vehicle_info_util package - Yamato ANDO Taiki Tanaka @@ -12,6 +11,7 @@ Shumpei Wakabayashi Apache License 2.0 + Yamato ANDO ament_cmake_auto autoware_cmake diff --git a/vehicle/vehicle_info_util/src/vehicle_info.cpp b/vehicle/vehicle_info_util/src/vehicle_info.cpp new file mode 100644 index 0000000000000..1a47252f82d04 --- /dev/null +++ b/vehicle/vehicle_info_util/src/vehicle_info.cpp @@ -0,0 +1,88 @@ +// Copyright 2015-2021 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 "vehicle_info_util/vehicle_info.hpp" + +namespace vehicle_info_util +{ +tier4_autoware_utils::LinearRing2d VehicleInfo::createFootprint(const double margin) const +{ + return createFootprint(margin, margin); +} + +tier4_autoware_utils::LinearRing2d VehicleInfo::createFootprint( + const double lat_margin, const double lon_margin) const +{ + using tier4_autoware_utils::LinearRing2d; + using tier4_autoware_utils::Point2d; + + const double x_front = front_overhang_m + wheel_base_m + lon_margin; + const double x_center = wheel_base_m / 2.0; + const double x_rear = -(rear_overhang_m + lon_margin); + const double y_left = wheel_tread_m / 2.0 + left_overhang_m + lat_margin; + const double y_right = -(wheel_tread_m / 2.0 + right_overhang_m + lat_margin); + + LinearRing2d footprint; + footprint.push_back(Point2d{x_front, y_left}); + footprint.push_back(Point2d{x_front, y_right}); + footprint.push_back(Point2d{x_center, y_right}); + footprint.push_back(Point2d{x_rear, y_right}); + footprint.push_back(Point2d{x_rear, y_left}); + footprint.push_back(Point2d{x_center, y_left}); + footprint.push_back(Point2d{x_front, y_left}); + + return footprint; +} + +VehicleInfo createVehicleInfo( + const double wheel_radius_m, const double wheel_width_m, const double wheel_base_m, + const double wheel_tread_m, const double front_overhang_m, const double rear_overhang_m, + const double left_overhang_m, const double right_overhang_m, const double vehicle_height_m, + const double max_steer_angle_rad) +{ + // Calculate derived parameters + const double vehicle_length_m_ = front_overhang_m + wheel_base_m + rear_overhang_m; + const double vehicle_width_m_ = wheel_tread_m + left_overhang_m + right_overhang_m; + const double min_longitudinal_offset_m_ = -rear_overhang_m; + const double max_longitudinal_offset_m_ = front_overhang_m + wheel_base_m; + const double min_lateral_offset_m_ = -(wheel_tread_m / 2.0 + right_overhang_m); + const double max_lateral_offset_m_ = wheel_tread_m / 2.0 + left_overhang_m; + const double min_height_offset_m_ = 0.0; + const double max_height_offset_m_ = vehicle_height_m; + + return VehicleInfo{ + // Base parameters + wheel_radius_m, + wheel_width_m, + wheel_base_m, + wheel_tread_m, + front_overhang_m, + rear_overhang_m, + left_overhang_m, + right_overhang_m, + vehicle_height_m, + max_steer_angle_rad, + // Derived parameters + vehicle_length_m_, + vehicle_width_m_, + min_longitudinal_offset_m_, + max_longitudinal_offset_m_, + min_lateral_offset_m_, + max_lateral_offset_m_, + min_height_offset_m_, + max_height_offset_m_, + }; +} + +} // namespace vehicle_info_util